动作通信开发中,我使用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 handles
if self._goal_handle is None:
return
# Update state
self._goal_handle.update_goal_state(event)
# Publish state change
self._action_server._handle.publish_status()
# If it's a terminal state, then also notify the action server
if 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