Ros2中goal_handle状态SUCCEED及ACCEPTED在rclpy中的死循环(未彻底解决,可参考我的另一篇彻底解决篇)

动作通信开发中,我使用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
  • 6
    点赞
  • 10
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值