机器人 / 系统课时

用 Action 管理可取消的导航任务

用 Action 管理可取消的导航任务

机器人接到导航目标后,需要持续报告剩余距离,支持中途取消,并最终确认成功、失败或取消。本篇基于 ROS 2 Jazzy 和 Nav2 的 NavigateToPose Action,编写可运行的 Python 客户端完成这三项操作。

学习目标

  • 区分 Action 的目标、反馈、结果和取消机制,以及它与 Service 的适用范围。
  • 使用 rclpy 向 /navigate_to_pose 发送目标并读取剩余距离。
  • 实测取消与正常完成,区分取消受理和最终取消状态。

为什么导航使用 Action

导航通常持续数秒甚至数分钟,客户端既要知道任务是否被接受,又要持续获得进度,还可能临时撤销。NavigateToPose 的目标是 PoseStamped,反馈包含 distance_remaining(剩余路径距离,单位米)和 number_of_recoveries(恢复行为次数)。

通信方式 适合的任务 交互特征
Service 查询参数、切换设备开关(如 SetBool) 一次请求和一次响应;没有内建的任务反馈与取消协议
Action 导航到目标点、机械臂执行耗时动作 发送目标、持续反馈、获取最终结果,还能请求取消

Action 的目标接受响应只代表服务端接单,不表示机器人已经到达。取消请求的响应如果包含正在取消的目标,也不代表运动已经停止;需要继续等待最终结果中的 GoalStatus.STATUS_CANCELED。

导航 Action 的目标、反馈、取消与最终结果时序

核对导航服务和目标坐标

本例适用于 Ubuntu 24.04、ROS 2 Jazzy、Python rclpy 和已经启动且处于 Active 状态的 Nav2。仿真机器人应已定位,能够提供 map 到机器人底盘的 TF 变换,并能规划至地图上的空闲区域。新开终端执行:

bash
source /opt/ros/jazzy/setup.bash
ros2 action list -t | grep '/navigate_to_pose'

预期输出(没有命名空间的常见配置):

text
/navigate_to_pose [nav2_msgs/action/NavigateToPose]

若没有输出,先确认 Nav2 启动与生命周期状态;使用命名空间时,实际 Action 名称可能不同,代码也要同步修改。可通过 ros2 interface show nav2_msgs/action/NavigateToPose 核对接口:目标字段为 pose 和 behavior_tree,后者留空表示使用默认行为树。

示例坐标取 frame_id=map、x=2.0、y=0.0,朝向四元数设置为 w=1.0(偏航角 0)。这是地图坐标而非机器人前方 2 米;运行前须确认该点在本机地图的可通行范围内。

编写反馈与取消客户端

将下面的完整代码保存为 nav_action_demo.py。--cancel-after 是接单后计时的取消触发秒数,设为 0 表示不主动取消。使用 time.monotonic() 计时,避免仿真时间暂停影响测试触发。

python
import argparse
import time

import rclpy
from action_msgs.msg import GoalStatus
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from rclpy.node import Node

def main():
    parser = argparse.ArgumentParser()
    parser.add_argument('--x', type=float, default=2.0)
    parser.add_argument('--y', type=float, default=0.0)
    parser.add_argument('--cancel-after', type=float, default=0.0)
    args, ros_args = parser.parse_known_args()

    rclpy.init(args=ros_args)
    node = Node('nav_action_demo')
    client = ActionClient(node, NavigateToPose, '/navigate_to_pose')

    def on_feedback(message):
        f = message.feedback
        print(f'Remaining: {f.distance_remaining:.2f} m, '
              f'recoveries: {f.number_of_recoveries}')

    try:
        if not client.wait_for_server(timeout_sec=5.0):
            print('Action server unavailable')
            return

        goal = NavigateToPose.Goal()
        goal.pose.header.frame_id = 'map'
        goal.pose.header.stamp = node.get_clock().now().to_msg()
        goal.pose.pose.position.x = args.x
        goal.pose.pose.position.y = args.y
        goal.pose.pose.orientation.w = 1.0  # 0 rad 朝向

        # 接收目标是否被接受,同时注册导航过程反馈
        send_future = client.send_goal_async(goal, feedback_callback=on_feedback)
        rclpy.spin_until_future_complete(node, send_future)
        handle = send_future.result()
        if not handle.accepted:
            print('Goal rejected')
            return
        print('Goal accepted')

        # 目标接受不等于到达;结果需要单独等待
        result_future = handle.get_result_async()
        start = time.monotonic()
        cancel_sent = False
        while rclpy.ok() and not result_future.done():
            rclpy.spin_once(node, timeout_sec=0.1)
            if (args.cancel_after > 0 and not cancel_sent
                    and not result_future.done()
                    and time.monotonic() - start >= args.cancel_after):
                cancel_sent = True
                print('Cancel requested')
                cancel_future = handle.cancel_goal_async()
                rclpy.spin_until_future_complete(node, cancel_future, timeout_sec=2.0)
                if cancel_future.done():
                    reply = cancel_future.result()
                    print('Cancel accepted' if reply.goals_canceling
                          else 'Cancel not accepted or already finished')
                else:
                    print('Cancel reply timeout')

        if result_future.done():
            wrapped = result_future.result()
            names = {
                GoalStatus.STATUS_SUCCEEDED: 'SUCCEEDED',
                GoalStatus.STATUS_CANCELED: 'CANCELED',
                GoalStatus.STATUS_ABORTED: 'ABORTED',
            }
            print('Final status:', names.get(wrapped.status, wrapped.status))
            if wrapped.status == GoalStatus.STATUS_ABORTED:
                print('Nav2 error_code:', wrapped.result.error_code)
    except KeyboardInterrupt:
        print('Interrupted; goal cancellation not confirmed')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

预期结果:Nav2 接受目标后,终端输出 Goal accepted 与若干条 Remaining 反馈;到达目标时输出 Final status: SUCCEEDED。启用延时取消后,程序会发出 cancel_goal_async() 并仍等待 get_result_async() 的最终状态。Cancel accepted 只说明取消请求被受理,不能代替最终结果。

验证取消和正常完成

先选择距离起点足够远、确实可达的目标,在机器人尚未抵达前触发取消。在已运行 Gazebo 的终端执行下列命令;use_sim_time:=true 让客户端时间戳与仿真 /clock 一致:

bash
python3 nav_action_demo.py --x 2.0 --y 0.0 --cancel-after 3 --ros-args -p use_sim_time:=true

预期输出(示意;反馈条数和距离取决于地图与机器人,可能有所变化):

text
Goal accepted
Remaining: <DISTANCE> m, recoveries: <COUNT>
Cancel requested
Cancel accepted
Final status: CANCELED

再向同一个可达目标发送不取消的任务,允许机器人到达:

bash
python3 nav_action_demo.py --x 2.0 --y 0.0 --ros-args -p use_sim_time:=true

预期输出(示意):

text
Goal accepted
Remaining: <DISTANCE> m, recoveries: <COUNT>
Final status: SUCCEEDED

<DISTANCE>、<COUNT> 为运行时变化的距离和次数。如果机器人在 3 秒前就已到达,首次测试可能得到 SUCCEEDED 而非 CANCELED;应重置起点或选择更远的可达目标后重试。真机不使用仿真时,移除 --ros-args -p use_sim_time:=true。

从状态判断问题出在哪里

  • Action server unavailable:运行 ros2 action list -t,检查 Nav2 是否已激活、Action 是否有命名空间,以及各终端的 ROS_DOMAIN_ID 是否一致。
  • Goal rejected:目标未被服务端接受;检查接口类型、目标字段、Nav2 行为树与服务端日志。目标不可达也可能在接受后才以 ABORTED 结束。
  • Final status: ABORTED:导航已结束但失败;结合打印的 Nav2 error_code、TF、定位状态和代价地图排查。不要把 Goal accepted 当成成功。
  • Cancel not accepted or already finished:可能任务先一步完成或取消未被接受;以最终 SUCCEEDED、CANCELED 或 ABORTED 为准。

当业务需要长时间执行、周期进度和中途撤销时使用 Action;单次快速开关或查询使用 Service。接口细节可对照 Nav2 Jazzy 的 NavigateToPose 定义。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用 Action 管理可取消的导航任务

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通