机器人 / 系统课时
处理导航反馈、取消与结果
处理导航反馈、取消与结果
导航目标被接收,不等于机器人已经到达。这里以 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 非空,仅表示服务端接受了取消请求;机器人仍可能正在执行停止流程。

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:
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,并保证仿真未暂停):
source /opt/ros/jazzy/setup.bash
python3 nav_watch.py --ros-args -p use_sim_time:=true预期输出(目标足够远且取消被接受时的一种情况;<DISTANCE>、<TIME>、<N> 是随环境变化的值):
目标已接受
反馈: 距离=<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 客户端。