机器人 / 系统课时

通过 Action 发送导航目标

通过 Action 发送导航目标

本课把 RViz 中手动下发目标的操作改成可重复调用的 ROS 2 Action 请求。你将构造 map 坐标系下的目标位姿,向 Nav2 发送任务,持续读取距离反馈,并根据最终状态判断机器人是否真正到达。

学习目标

  • 确认 /navigate_to_pose 服务端已就绪,并正确填写地图坐标和方向。
  • 使用 Python Action 客户端发送目标、订阅反馈及等待结果。
  • 区分目标被接受、任务正在执行与最终成功或失败。

确认服务端和导航前提

适用于 ROS 2 Jazzy + Nav2 的单机器人仿真。先启动 Gazebo、地图和定位、Nav2;在 RViz 中确认 Fixed Frame 为 map,定位已收敛,目标点落在地图的可通行区域。bt_navigator 必须激活,才能提供默认的 /navigate_to_pose Action Server。

在已加载 Jazzy 环境的终端检查服务端类型:

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

预期输出(未使用机器人命名空间时):

text
/navigate_to_pose [nav2_msgs/action/NavigateToPose]

没有这一行时,先核对 Nav2 是否启动、bt_navigator 生命周期是否为 active,以及两个终端的 ROS_DOMAIN_ID 是否一致。如果项目使用命名空间,例如 /robot1/navigate_to_pose,后续代码中的 Action 名称也应改成实际名称。

目标位姿与 Action 交互

nav2_msgs/action/NavigateToPose 的目标包含 pose 和可选的 behavior_tree。其中 pose 是 geometry_msgs/PoseStamped:header.frame_id='map' 指明全局地图坐标系,header.stamp 应使用节点时钟,position.x/y 以米计。示例设为 (1.0, 0.5) 米、航向角 0°,对应四元数 x=0, y=0, z=0, w=1;实际运行前须按自己的地图替换坐标。

Action 有三种不同的信息,不应混为一次函数返回:服务器首先答复目标是否接受;执行期间反复发送 distance_remaining 和 number_of_recoveries 等反馈;导航结束后才返回带有终态 status 的结果。behavior_tree 保持默认空字符串时,Nav2 使用其配置的默认导航行为树。

导航 Action 请求、反馈与最终结果的时序关系

编写可运行的 Python 客户端

创建 nav_goal_client.py。下面的完整脚本使用 ActionClient.send_goal_async() 获得目标句柄,再用 get_result_async() 等待终态;等待 Future 时仍会处理反馈回调。这里的 1.0, 0.5 只是示范坐标,必须保证该点可达。

python
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 on_feedback(message):
    fb = message.feedback
    print(f'反馈: 剩余距离约 {fb.distance_remaining:.2f} m, 恢复次数={fb.number_of_recoveries}')

def main():
    rclpy.init()
    node = Node('nav_goal_client')
    client = ActionClient(node, NavigateToPose, '/navigate_to_pose')
    try:
        if not client.wait_for_server(timeout_sec=10.0):
            print('未发现 /navigate_to_pose Action Server')
            return

        # 目标点属于 map 坐标系;单位为米
        goal = NavigateToPose.Goal()
        goal.pose.header.frame_id = 'map'
        goal.pose.header.stamp = node.get_clock().now().to_msg()
        goal.pose.pose.position.x = 1.0
        goal.pose.pose.position.y = 0.5
        goal.pose.pose.orientation.w = 1.0  # 航向角为 0°

        # 接收目标不代表已经到达
        sent = client.send_goal_async(goal, feedback_callback=on_feedback)
        rclpy.spin_until_future_complete(node, sent)
        handle = sent.result()
        if not handle.accepted:
            print('目标被拒绝')
            return
        print('目标已接受')

        pending = handle.get_result_async()
        rclpy.spin_until_future_complete(node, pending)
        reply = pending.result()
        if reply.status == GoalStatus.STATUS_SUCCEEDED:
            print('任务成功: SUCCEEDED')
        elif reply.status == GoalStatus.STATUS_ABORTED:
            print(f'任务失败: ABORTED, error_code={reply.result.error_code}, error_msg={reply.result.error_msg}')
        elif reply.status == GoalStatus.STATUS_CANCELED:
            print('任务取消: CANCELED')
        else:
            print(f'其他状态码: {reply.status}')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

预期结果:服务器接受后打印“目标已接受”,导航过程中打印距离反馈,结束时打印 SUCCEEDED、ABORTED 或 CANCELED。真正的最终状态来自 get_result_async() 响应中的 status;失败时还应读取 result.error_code 和 result.error_msg,不能只凭距离推断成功。

运行并检查导航结果

先确保 Gazebo 的 /clock 正在更新,并保持地图、定位和 Nav2 正常运行。对于使用仿真时间的系统,从保存脚本的目录执行:

bash
python3 nav_goal_client.py --ros-args -p use_sim_time:=true

预期输出示例(目标可达时,反馈条数与数值随环境变化,可能穿插在接受提示前后):

text
目标已接受
反馈: 剩余距离约 <DISTANCE> m, 恢复次数=<COUNT>
任务成功: SUCCEEDED

其中 <DISTANCE> 和 <COUNT> 代表实际运行中变化的数值,不是程序逐字打印的内容;日志中的反馈可能出现多次。验收时同时观察 RViz 中的机器人位置和朝向,确认它进入 Goal Checker 允许的位置与角度容差。终态 SUCCEEDED 才说明 Action 任务成功,单独出现“目标已接受”不能作为验收依据。

识别失败和取消状态

现象 检查与处理
10 秒内未发现 Action Server 检查 /navigate_to_pose 名称、bt_navigator 的 active 状态及 ROS_DOMAIN_ID。
目标被接受后得到 ABORTED 读取 error_code/error_msg,核对 map → odom → base_link 的 TF、定位结果和目标栅格是否可通行。
持续有反馈但距离不下降 观察 /odom 是否更新、实际速度命令话题是否接到底盘,并排查局部代价地图中的障碍物。
返回 CANCELED 表示任务已取消;检查是否有其他客户端发出取消请求或导航系统任务被替换。

若要在执行中查询服务器的原始 Goal 状态,可在另一终端运行 ros2 topic echo /navigate_to_pose/_action/status,通过 Goal UUID 区分不同目标:STATUS_ACCEPTED=1、STATUS_EXECUTING=2、STATUS_SUCCEEDED=4、STATUS_CANCELED=5、STATUS_ABORTED=6。状态话题主要用于诊断,自己的客户端仍以目标句柄及最终 Result 为准。

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

目录与阅读设置

阅读设置

17

记录一个想法

通过 Action 发送导航目标

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

登录哪都通