动作通信开发中,我使用rclpy编写代码,进行feedback等操作,前期实现的较为顺利,但所有逻辑均编写完后,却无法将goal_handle提交为succeed状态,以下是我的部分代码:
self.server = ActionServer(self,Nav,"nav",execute_callback=self.execute,goal_callback=self.handle_goal,cancel_callback=self.handle_cancel,handle_accepted_callback=self.handle_accepted,)self.turtle1_pose = None
出现了[exer04_action_server-2] rclpy._rclpy_pybind11.RCLError: Failed to update goal state: goal_handle attempted invalid transition from state ACCEPTED with event SUCCEED, at ./src/rcl_action/goal_handle.c:95的问题,也就是不能将ACCEPTED状态转变为SUCCEED,但是在源码中查询发现Python实现的rclpy、goal_handle仅有ACCEPT和REJECT两个可选状态
那为何之前不编写goal_callback、cancel_callback、及handle_accepted_callback这几个函数的时候不会出现这种问题?我又去查看了源码发现了问题所在,即若不自己实现handle_accepted_callback()函数,这个函数默认有一个default_handle_accepted_callback()的实现,且这个实现中包含更新状态的部分代码,但是这些代码我们自己不能实现,也没给具体的转换状态的方法,是一个死循环。
def default_handle_accepted_callback(goal_handle):"""Execute the goal."""goal_handle.execute()def _update_state(self, event):with self._lock:# Ignore updates for already destructed goal handlesif self._goal_handle is None:return# Update stateself._goal_handle.update_goal_state(event)# Publish state changeself._action_server._handle.publish_status()# If it's a terminal state, then also notify the action serverif not self._goal_handle.is_active():self._action_server.notify_goal_done()def execute(self, execute_callback=None):# It's possible that there has been a request to cancel the goal prior to executing.# In this case we want to avoid the illegal state transition to EXECUTING# but still call the users execute callback to let them handle canceling the goal.if not self.is_cancel_requested:self._update_state(_rclpy.GoalEvent.EXECUTE)self._action_server.notify_execute(self, execute_callback)
由于我的execute_callback回调函数名为execute(),与源码实现的默认回调函数重名,所以覆盖了他的回调函数,而他的回调函数除了变更状态以外没有其他作用,于是其实可以将自己的回调函数改名,并在succeed()函数前使用他的默认execute()方法,这样就解决了问题。
self.server = ActionServer(self,Nav,"nav",execute_callback=self.my_execute,goal_callback=self.handle_goal,cancel_callback=self.handle_cancel,handle_accepted_callback=self.handle_accepted,)......def my_execute(self, goal_handle):pass......if rclpy.ok():goal_handle.execute()goal_handle.succeed()self.get_logger().info('任务结束!')return result