机器人 / 系统课时

处理导航反馈、取消与结果

处理导航反馈、取消与结果

导航目标被接收,不等于机器人已经到达。这里以 ROS 2 Jazzy 的 NavigateToPose 为例,读取路径剩余距离和预计剩余时间,在运行中发起取消,并根据真正的终态区分成功、取消与失败,避免把取消请求的确认误当作导航结束。

学习目标

  • 读取并正确解释 NavigateToPose 的实时反馈。
  • 对已接受的目标发送取消请求,并继续等待最终结果。
  • 区分 SUCCEEDED、CANCELED、ABORTED 和目标拒绝。

读取反馈而不误判进度

bt_navigator 提供 /navigate_to_pose Action。其反馈不是一次性响应,而是在任务执行期间持续更新;先使用 ros2 interface show nav2_msgs/action/NavigateToPose 核对本机接口。

反馈字段 含义与用法
distance_remaining 当前规划路径的近似剩余长度,单位 m,不是到目标的直线距离
estimated_time_remaining 估计剩余时长,类型为 Duration,用 sec + nanosec / 1e9 换算为秒
navigation_time 当前任务已运行的时长,同样是 Duration
number_of_recoveries 已触发的恢复行为次数;非零不等于失败
current_pose 当前机器人姿态,可用于检查实际位置和目标是否对应

预计时间依赖当前路径和运动速度:低速、原地转向、重新规划时可能暂时为零或大幅变化。不要看到剩余距离一度增加就断言导航失败,也不要因为 distance_remaining 接近零就提前宣布成功。

分清请求、确认与终态

Action 客户端先发送目标并检查 goal_handle.accepted;只有接受后,才有该目标的反馈和结果可等待。取消需要通过对应目标句柄调用 cancel_goal_async()。响应中的 goals_canceling 非空,仅表示服务端接受了取消请求;机器人仍可能正在执行停止流程。

NavigateToPose 目标、反馈、取消确认与终态的时序关系

get_result_async() 才会提供最终的 status 和 result。可能发生目标在取消请求到达前已经成功的竞态,因此无论是否发起取消,都要以最后收到的 GoalStatus 为准。

编写并运行客户端

前提是 ROS 2 Jazzy、Nav2 和仿真已运行,机器人定位有效,/navigate_to_pose 有 Action Server。示例目标 (2.0, 0.0) 是 map 坐标,必须按 RViz 中的可通行区域修改;把 CANCEL_AFTER_S 改为 None 可测试自然到达。

将下面的完整程序保存为 nav_watch.py:

python
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

CANCEL_AFTER_S = 5.0

def seconds(duration):
    return duration.sec + duration.nanosec / 1e9

class NavWatch(Node):
    def __init__(self):
        super().__init__("nav_watch")
        self.client = ActionClient(self, NavigateToPose, "/navigate_to_pose")
        self.goal_handle = None
        self.result_future = None
        self.cancel_timer = None
        self.last_log = 0.0

    def on_feedback(self, message):
        now = time.monotonic()
        if now - self.last_log < 1.0:
            return
        self.last_log = now
        fb = message.feedback
        print(
            f"反馈: 距离={fb.distance_remaining:.2f} m, "
            f"预计={seconds(fb.estimated_time_remaining):.1f} s, "
            f"已用={seconds(fb.navigation_time):.1f} s, "
            f"恢复={fb.number_of_recoveries}"
        )

    def request_cancel(self):
        self.cancel_timer.cancel()  # 只发送一次取消请求
        if self.result_future.done():
            return
        print("发送取消请求")
        future = self.goal_handle.cancel_goal_async()
        future.add_done_callback(self.on_cancel_response)

    def on_cancel_response(self, future):
        if future.result().goals_canceling:
            print("取消请求已受理,等待终态")
        else:
            print("取消未受理,继续等待终态")

    def run(self):
        if not self.client.wait_for_server(timeout_sec=10.0):
            print("动作服务器不可用")
            return
        goal = NavigateToPose.Goal()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x = 2.0
        goal.pose.pose.position.y = 0.0
        goal.pose.pose.orientation.w = 1.0  # 偏航角为 0

        send_future = self.client.send_goal_async(
            goal, feedback_callback=self.on_feedback
        )
        rclpy.spin_until_future_complete(self, send_future)
        self.goal_handle = send_future.result()
        if not self.goal_handle.accepted:
            print("目标被拒绝")
            return

        print("目标已接受")
        self.result_future = self.goal_handle.get_result_async()
        if CANCEL_AFTER_S is not None:
            self.cancel_timer = self.create_timer(
                CANCEL_AFTER_S, self.request_cancel
            )
        rclpy.spin_until_future_complete(self, self.result_future)
        wrapped = self.result_future.result()
        if wrapped.status == GoalStatus.STATUS_SUCCEEDED:
            print("最终状态: SUCCEEDED")
        elif wrapped.status == GoalStatus.STATUS_CANCELED:
            print("最终状态: CANCELED")
        elif wrapped.status == GoalStatus.STATUS_ABORTED:
            print(
                f"最终状态: ABORTED, 错误码={wrapped.result.error_code}, "
                f"原因={wrapped.result.error_msg}"
            )
        else:
            print(f"最终状态: {wrapped.status}")

def main():
    rclpy.init()
    node = NavWatch()
    try:
        node.run()
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()

预期结果:程序等待服务器、发送目标并打印反馈;在仿真时间运行约 5 秒后请求取消,最终仍等待真实终态。执行命令(仿真发布 /clock,并保证仿真未暂停):

bash
source /opt/ros/jazzy/setup.bash
python3 nav_watch.py --ros-args -p use_sim_time:=true

预期输出(目标足够远且取消被接受时的一种情况;<DISTANCE>、<TIME>、<N> 是随环境变化的值):

text
目标已接受
反馈: 距离=<DISTANCE> m, 预计=<TIME> s, 已用=<TIME> s, 恢复=<N>
发送取消请求
取消请求已受理,等待终态
最终状态: CANCELED

根据终态处理业务结果

结果或事件 正确处理
目标未被接受 拒绝已发生,不能当作 ABORTED,也无须请求取消
STATUS_SUCCEEDED(4) 到达目标;可开始下一项任务
STATUS_CANCELED(5) 此目标取消完成;清理该任务的等待状态
STATUS_ABORTED(6) 导航异常终止;记录 result.error_code 与 result.error_msg 并检查日志

只检查 result.error_code == 0 不能替代终态判断。对于 Jazzy,失败时的 error_msg 不一定提供完整根因,仍应结合 bt_navigator、规划器、控制器和 TF 日志定位。

验证取消与常见异常

先选距离机器人数米且可到达的目标,保留 CANCEL_AFTER_S = 5.0,确认看到 CANCELED;再改为 None,让机器人自然到达并看到 SUCCEEDED。若取消前已抵达,出现 SUCCEEDED 是正常竞态,应改选更远的可达点,而不是强行把它判成取消成功。

如果提示“动作服务器不可用”,执行 ros2 action info /navigate_to_pose,检查是否存在 Action Server,并确认命名空间与 Nav2 生命周期状态。若一直没有反馈,优先检查 map → odom → base_link 的 TF、定位和行为树导航日志。出现 CANCELED 后还要观察底盘是否停止运动;Action 终态并不能代替对实际速度反馈的安全检查。

接口字段和类型可对照 ROS 2 Jazzy 的 NavigateToPose.action,不要把 Rolling 分支新增的反馈字段直接写进 Jazzy 客户端。

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

目录与阅读设置

阅读设置

17

记录一个想法

处理导航反馈、取消与结果

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

登录哪都通