1. ROS2 Action Client实现多目标点顺序巡航
在机器人导航开发中，让机器人按预定路线巡航多个目标点是常见需求。传统方法通常需要手动编写状态机管理导航过程，而ROS2的Action机制为这类任务提供了更优雅的解决方案。本文将详细介绍如何使用ROS2 Action Client实现多目标点的顺序巡航，包含完整的代码实现、原理分析和实战技巧。
2. 核心概念与准备工作
2.1 ROS2 Action机制解析
Action是ROS2中用于长时间运行任务的通信机制，相比Service更适合导航这类需要持续反馈的操作。一个完整的Action包含三个部分：
- Goal：客户端发送给服务器的任务请求（如目标点坐标）
- Feedback：服务器执行过程中持续发送的进度反馈
- Result：任务完成后返回的最终结果
在导航场景中，Action机制允许我们：
- 实时获取机器人当前位置和剩余距离
- 在任务执行过程中取消操作
- 处理任务失败和重试逻辑
2.2 环境配置与依赖安装
实现多目标点巡航需要以下环境准备：
bash复制# 安装ROS2 Humble版本（推荐）
sudo apt install ros-humble-desktop
# 安装Nav2相关功能包
sudo apt install ros-humble-nav2-*
# 安装Python依赖
pip install pynput
提示：如果使用其他ROS2版本（如Foxy或Galactic），只需将上述命令中的"humble"替换为对应版本名称即可。
3. 代码实现详解
3.1 Action Client基础结构
我们首先创建一个继承自rclpy.node.Node的Action Client类：
python复制#!/usr/bin/env python3
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from nav2_msgs.action import NavigateToPose
from geometry_msgs.msg import PoseStamped
class CruiseActionClient(Node):
def __init__(self):
super().__init__('cruise_action_client')
# 创建Action Client
self._action_client = ActionClient(
self,
NavigateToPose,
'navigate_to_pose'
)
# 初始化巡航参数
self.target_poses = self._generate_target_poses()
self.retry_max = 3
self.current_target_idx = 0
3.2 目标点生成方法
目标点列表可以通过以下方法生成，每个目标点包含位置和朝向信息：
python复制def _generate_target_poses(self):
"""生成巡航目标点列表"""
target_list = []
# 目标点1
pose1 = PoseStamped()
pose1.header.frame_id = 'map'
pose1.pose.position.x = 1.0
pose1.pose.position.y = 0.0
pose1.pose.orientation.w = 1.0 # 无旋转
target_list.append(pose1)
# 目标点2
pose2 = PoseStamped()
pose2.header.frame_id = 'map'
pose2.pose.position.x = 1.0
pose2.pose.position.y = 1.0
pose2.pose.orientation.w = 1.0
target_list.append(pose2)
return target_list
注意：实际应用中，建议从配置文件或参数服务器加载目标点坐标，而不是硬编码在代码中。
3.3 目标点发送与重试逻辑
发送单个目标点并处理重试的核心代码如下：
python复制def _send_goal(self, target_pose, retry_count=0):
"""发送目标点请求并处理重试逻辑"""
# 等待Action Server上线
if not self._action_client.wait_for_server(timeout_sec=5.0):
self.get_logger().error('Action Server未就绪')
return False
# 构造Goal消息
goal_msg = NavigateToPose.Goal()
goal_msg.pose = target_pose
# 发送目标并等待结果
future = self._action_client.send_goal_async(
goal_msg,
feedback_callback=self._feedback_callback
)
rclpy.spin_until_future_complete(self, future)
# 处理结果
if not future.result().accepted:
if retry_count < self.retry_max:
self.get_logger().info(f'第{retry_count+1}次重试...')
time.sleep(1)
return self._send_goal(target_pose, retry_count+1)
return False
return True
4. 高级功能实现
4.1 实时反馈处理
通过反馈回调函数，我们可以实时获取导航状态：
python复制def _feedback_callback(self, feedback_msg):
"""处理导航反馈信息"""
feedback = feedback_msg.feedback
pose = feedback.current_pose.pose
distance = feedback.distance_remaining
self.get_logger().info(
f'当前位置: x={pose.position.x:.2f}, y={pose.position.y:.2f} | '
f'剩余距离: {distance:.2f}米'
)
4.2 取消机制实现
使用独立线程监听ESC键实现取消功能：
python复制def _start_key_listener(self):
"""启动按键监听线程"""
def on_press(key):
try:
if key == keyboard.Key.esc:
self.cancel_current_goal()
return False
except:
pass
threading.Thread(
target=lambda: keyboard.Listener(on_press=on_press).start(),
daemon=True
).start()
5. 完整巡航流程控制
主巡航逻辑控制目标点顺序执行：
python复制def run_cruise(self):
"""执行顺序巡航主逻辑"""
while self.current_target_idx < len(self.target_poses):
success = self._send_goal(
self.target_poses[self.current_target_idx]
)
if success:
self.current_target_idx += 1
else:
break
if self.current_target_idx == len(self.target_poses):
self.get_logger().info('巡航任务完成!')
6. 实战技巧与问题排查
6.1 性能优化建议
-
多线程执行器：使用
MultiThreadedExecutor避免回调阻塞python复制
executor = MultiThreadedExecutor() executor.add_node(node) executor.spin() -
目标点预处理：提前验证目标点是否可达，减少运行时失败
-
合理的重试间隔：重试之间添加适当延迟（如1秒）
6.2 常见问题解决方案
问题1：Action Server无响应
- 检查Server是否正常运行：
ros2 topic list | grep navigate_to_pose - 确认Action名称匹配
问题2：反馈信息不更新
- 检查机器人定位是否正常
- 确认Feedback话题有数据：
ros2 topic echo /navigate_to_pose/_action/feedback
问题3：取消请求无效
- 确保在Goal被接受后再发送取消
- 检查Server是否实现了取消处理逻辑
7. 扩展应用场景
本文介绍的基础框架可以扩展应用到以下场景：
- 巡逻机器人：定时巡航预设检查点
- 物流AGV：在不同工作站间自动运输物料
- 清洁机器人：按规划路径完成区域清扫
通过修改目标点生成逻辑和反馈处理，可以轻松适配各种业务需求。例如，添加动态目标点支持：
python复制def add_dynamic_target(self, x, y):
"""动态添加目标点"""
new_pose = PoseStamped()
new_pose.header.frame_id = 'map'
new_pose.pose.position.x = x
new_pose.pose.position.y = y
self.target_poses.append(new_pose)
在实际项目中，建议将巡航逻辑封装为独立ROS2节点，通过Service或Topic接收外部控制命令，实现更灵活的集成。