ROS2 Humble下用Python写Action服务端与客户端：一个模拟机器人移动的完整示例

ROS2 Humble下Python Action开发实战从机器人状态机到多线程优化在机器人开发中异步任务处理是个永恒的话题。想象一下当你需要让机器人移动2米的同时还要实时监测环境变化或者在进行机械臂轨迹规划时允许用户随时取消当前操作——这正是ROS2 Action设计的初衷。与简单的服务调用不同Action提供了三大核心优势长时间任务执行、实时进度反馈和任务取消机制。本文将带你用Python从零构建一个完整的机器人移动模拟系统深入剖析rclpy.action的设计哲学并解决实际开发中那些官方文档没告诉你的坑。1. 环境准备与工程架构1.1 创建Python功能包在ROS2 Humble环境下我们首先需要建立一个标准的Python功能包。与C不同Python包的创建有一些特殊配置需要注意ros2 pkg create robot_action_py \ --build-type ament_python \ --dependencies rclpy robot_control_interfaces \ --node-name action_server action_client \ --maintainer-name your_name \ --maintainer-email your_emaildomain.com关键参数解析--build-type ament_python指定Python包类型--dependencies声明依赖的ROS2接口--node-name自动生成节点入口文件提示ROS2 Python包的目录结构遵循Python模块规范所有源代码应放在package_name/package_name目录下1.2 工程文件结构规划一个规范的Action项目通常包含以下文件robot_action_py/ ├── robot_action_py/ │ ├── __init__.py │ ├── robot.py # 机器人模拟类 │ ├── action_server.py # Action服务端 │ └── action_client.py # Action客户端 ├── setup.py └── package.xml在setup.py中需要确保正确配置入口点entry_points{ console_scripts: [ action_server robot_action_py.action_server:main, action_client robot_action_py.action_client:main, ], },2. 机器人状态机实现2.1 设计移动控制状态机在robot.py中我们实现一个具有完整状态管理的机器人类from robot_control_interfaces.action import MoveRobot import math class Robot: 模拟二维平面移动机器人 def __init__(self): self._current_pose 0.0 # 当前位置 self._target_pose 0.0 # 目标位置 self._status MoveRobot.Feedback.STATUS_READY self._move_step 0.1 # 单步移动距离系数 def set_goal(self, distance): 设置移动目标 if self._status ! MoveRobot.Feedback.STATUS_READY: return False self._target_pose self._current_pose distance self._status MoveRobot.Feedback.STATUS_MOVING return True def move_step(self): 执行单步移动 if self._status ! MoveRobot.Feedback.STATUS_MOVING: return False direction math.copysign(1, self._target_pose - self._current_pose) step_size direction * min( abs(self._target_pose - self._current_pose), self._move_step ) self._current_pose step_size if self._check_goal_reached(): self._status MoveRobot.Feedback.STATUS_COMPLETED return True def _check_goal_reached(self): 检查是否到达目标 return math.isclose( self._current_pose, self._target_pose, abs_tol0.01 ) def get_feedback(self): 生成反馈消息 feedback MoveRobot.Feedback() feedback.pose self._current_pose feedback.status self._status return feedback状态转换图示意[READY] - [MOVING] - [COMPLETED] ↑ | └----------┘ (通过set_goal重置)2.2 异常处理机制完善的机器人控制需要考虑各种异常情况class Robot: # ...原有代码... def emergency_stop(self): 紧急停止 self._status MoveRobot.Feedback.STATUS_EMERGENCY_STOP def handle_cancel(self): 处理取消请求 if self._status MoveRobot.Feedback.STATUS_MOVING: self._status MoveRobot.Feedback.STATUS_CANCELLED return True return False3. Action服务端深度解析3.1 服务端核心架构在action_server.py中构建完整的Action服务import rclpy from rclpy.action import ActionServer from rclpy.node import Node from robot_control_interfaces.action import MoveRobot from .robot import Robot class RobotActionServer(Node): def __init__(self): super().__init__(robot_action_server) self.robot Robot() self._action_server ActionServer( self, MoveRobot, move_robot, self._execute_callback, goal_callbackself._goal_callback, cancel_callbackself._cancel_callback ) self.get_logger().info(Action服务器已启动...) def _goal_callback(self, goal_request): 处理新目标请求 self.get_logger().info(f收到新目标: 移动 {goal_request.distance}米) if not self.robot.set_goal(goal_request.distance): return rclpy.action.server.GoalResponse.REJECT return rclpy.action.server.GoalResponse.ACCEPT def _cancel_callback(self, cancel_request): 处理取消请求 self.get_logger().info(收到取消请求) if self.robot.handle_cancel(): return rclpy.action.server.CancelResponse.ACCEPT return rclpy.action.server.CancelResponse.REJECT async def _execute_callback(self, goal_handle): 执行移动任务 self.get_logger().info(开始执行移动任务...) while rclpy.ok() and not goal_handle.is_cancel_requested: if not self.robot.move_step(): break feedback self.robot.get_feedback() goal_handle.publish_feedback(feedback) await asyncio.sleep(0.1) # 非阻塞式等待 result MoveRobot.Result() result.pose self.robot.get_feedback().pose if goal_handle.is_cancel_requested: goal_handle.canceled() self.get_logger().warn(任务被取消) elif self.robot.get_feedback().status MoveRobot.Feedback.STATUS_COMPLETED: goal_handle.succeed() self.get_logger().info(任务完成!) return result关键设计要点使用异步execute_callback避免阻塞主线程明确的状态反馈机制完善的取消处理流程3.2 多线程执行器优化默认的单线程执行器会导致rate.sleep()死锁问题解决方案from rclpy.executors import MultiThreadedExecutor def main(argsNone): rclpy.init(argsargs) server RobotActionServer() # 使用多线程执行器 executor MultiThreadedExecutor(num_threads4) executor.add_node(server) try: executor.spin() except KeyboardInterrupt: server.get_logger().info(服务器关闭中...) finally: server.destroy_node() rclpy.shutdown()线程分配策略1个线程处理Action通信1个线程执行任务处理2个备用线程处理其他回调4. 智能Action客户端实现4.1 客户端状态管理在action_client.py中实现带超时管理的客户端import rclpy from rclpy.action import ActionClient from rclpy.node import Node from robot_control_interfaces.action import MoveRobot class RobotActionClient(Node): def __init__(self): super().__init__(robot_action_client) self._client ActionClient(self, MoveRobot, move_robot) self._goal_handle None self._send_goal_timer self.create_timer(1.0, self.send_goal) def send_goal(self): 发送移动目标 self._send_goal_timer.cancel() goal_msg MoveRobot.Goal() goal_msg.distance 2.5 # 默认移动2.5米 self._client.wait_for_server(timeout_sec5.0) self._send_goal_future self._client.send_goal_async( goal_msg, feedback_callbackself._feedback_callback ) self._send_goal_future.add_done_callback(self._goal_response_callback) def _goal_response_callback(self, future): 处理目标响应 self._goal_handle future.result() if not self._goal_handle.accepted: self.get_logger().error(目标被拒绝) return self.get_logger().info(目标已接受执行中...) self._get_result_future self._goal_handle.get_result_async() self._get_result_future.add_done_callback(self._result_callback) def _feedback_callback(self, feedback_msg): 处理进度反馈 feedback feedback_msg.feedback self.get_logger().info( f当前进度: {feedback.pose:.2f}m | 状态: {feedback.status} ) def _result_callback(self, future): 处理最终结果 result future.result().result self.get_logger().info(f最终位置: {result.pose:.2f}m) rclpy.shutdown()4.2 高级功能扩展为客户端添加更多实用功能class RobotActionClient(Node): # ...原有代码... def cancel_goal(self): 取消当前目标 if self._goal_handle is not None: future self._goal_handle.cancel_goal_async() future.add_done_callback(self._cancel_done) def _cancel_done(self, future): 取消完成回调 cancel_response future.result() if len(cancel_response.goals_canceling) 0: self.get_logger().info(取消请求已接受) else: self.get_logger().warning(取消请求被拒绝)5. 调试技巧与性能优化5.1 常见问题排查指南问题现象可能原因解决方案Action调用无响应服务端未启动检查ros2 action list输出反馈信息延迟单线程阻塞使用MultiThreadedExecutor取消请求无效未正确处理回调实现cancel_callback目标频繁被拒状态机未重置检查机器人ready状态5.2 性能优化策略回调分组优化from rclpy.callback_groups import ReentrantCallbackGroup self._action_server ActionServer( self, MoveRobot, move_robot, execute_callbackself._execute_callback, callback_groupReentrantCallbackGroup() )QoS配置调整from rclpy.qos import QoSProfile action_qos QoSProfile( depth10, reliabilityrclpy.qos.ReliabilityPolicy.RELIABLE ) self._action_server ActionServer( # ...其他参数... feedback_pub_qos_profileaction_qos )执行器线程数建议简单场景2-4个线程复杂系统CPU核心数×1.5在实际机器人项目中Action通信的稳定性往往决定了整个系统的可靠性。记得在正式部署前进行压力测试模拟网络延迟和节点重启等情况。我曾在一个仓储机器人项目中发现当Action消息频率超过50Hz时需要特别优化反馈消息的数据量否则会导致整个系统延迟增加。