机器人 / 系统课时
用 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。

核对导航服务和目标坐标
本例适用于 Ubuntu 24.04、ROS 2 Jazzy、Python rclpy 和已经启动且处于 Active 状态的 Nav2。仿真机器人应已定位,能够提供 map 到机器人底盘的 TF 变换,并能规划至地图上的空闲区域。新开终端执行:
source /opt/ros/jazzy/setup.bash
ros2 action list -t | grep '/navigate_to_pose'预期输出(没有命名空间的常见配置):
/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() 计时,避免仿真时间暂停影响测试触发。
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 一致:
python3 nav_action_demo.py --x 2.0 --y 0.0 --cancel-after 3 --ros-args -p use_sim_time:=true预期输出(示意;反馈条数和距离取决于地图与机器人,可能有所变化):
Goal accepted
Remaining: <DISTANCE> m, recoveries: <COUNT>
Cancel requested
Cancel accepted
Final status: CANCELED再向同一个可达目标发送不取消的任务,允许机器人到达:
python3 nav_action_demo.py --x 2.0 --y 0.0 --ros-args -p use_sim_time:=true预期输出(示意):
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 定义。