MoveIt Servo 如何通过 FollowJointTrajectoryControllerHandle Action Server 通信

一、通信架构图textMoveIt Servo (moveit_servo node) ↓ MoveItSimpleControllerManager (插件管理器) ↓ FollowJointTrajectoryControllerHandle (您看到的这个类) ↓ [内部创建 Action Client] ↓ ROS 2 Action Client (自动生成) ↓ [通过 ROS 2 Action 协议] ↓ 您的 Action Server (myrobot_driver) ↓ 真实硬件二、关键通信机制1.Action Client 的创建FollowJointTrajectoryControllerHandle在初始化时会创建 Action Clientcpp// 构造函数中未显示但存在 FollowJointTrajectoryControllerHandle::FollowJointTrajectoryControllerHandle( rclcpp::Node::SharedPtr node, const std::string name, const std::string action_namespace) { // 创建 Action Client连接到您的 Server controller_action_client_ rclcpp_action::create_client control_msgs::action::FollowJointTrajectory( node, action_namespace // 例如: arm_controller/follow_joint_trajectory ); }2.发送轨迹的核心代码cppbool FollowJointTrajectoryControllerHandle::sendTrajectory( const moveit_msgs::msg::RobotTrajectory trajectory) { // 1. 检查连接状态 if (!isConnected()) { RCLCPP_ERROR(Action client not connected to action server); return false; } // 2. 构建 Goal control_msgs::action::FollowJointTrajectory::Goal goal goal_template_; goal.trajectory trajectory.joint_trajectory; // 填充轨迹数据 goal.multi_dof_trajectory trajectory.multi_dof_joint_trajectory; // 3. 设置回调函数接收 Server 的响应 send_goal_options.goal_response_callback [this](goal_handle) { if (!goal_handle) RCLCPP_WARN(Goal request rejected); // Server 拒绝 else RCLCPP_INFO(Goal request accepted!); // Server 接受 }; // 4. 异步发送 Goal 到您的 Action Server auto current_goal_future controller_action_client_-async_send_goal( goal, send_goal_options); // 5. 等待 Server 响应 current_goal_ current_goal_future.get(); return true; }三、完整的通信流程步骤1初始化阶段cpp// MoveIt 启动时加载配置 // controllers.yaml 内容 // arm_controller: // action_ns: arm_controller/follow_joint_trajectory ← 您的 Server 名称 // type: FollowJointTrajectory // FollowJointTrajectoryControllerHandle 创建 Action Client // 连接到您的 Server: /arm_controller/follow_joint_trajectory步骤2Servo 运行时cpp// MoveIt Servo 持续运行当需要发送轨迹时 // 1. Servo 调用 getControllerHandle(arm_controller) // 2. 获得 FollowJointTrajectoryControllerHandle 实例 // 3. 调用 sendTrajectory() 方法 // 4. 该方法通过 Action Client 发送到您的 Server步骤3您的 Server 接收并处理cpp// 您的 Action Server 代码 class FollowJointTrajectoryAction : public rclcpp::Node { // 这个回调会被触发 void execute(const std::shared_ptrGoalHandle goal_handle) { auto goal goal_handle-get_goal(); auto points goal-trajectory.points; // 接收轨迹点 // 执行轨迹... for (const auto point : points) { send_to_motors(point.positions); // 发送给电机 } // 返回成功结果 auto result std::make_sharedFollowJointTrajectory::Result(); result-error_code FollowJointTrajectory::Result::SUCCESSFUL; goal_handle-succeed(result); } };四、数据流转详解MoveIt Servo → Action Clientcpp// MoveIt Servo 生成的轨迹 moveit_msgs::msg::RobotTrajectory trajectory; trajectory.joint_trajectory.joint_names {joint1, joint2, ...}; trajectory.joint_trajectory.points[0].positions {0.1, 0.2, 0.3, ...}; // ↓ sendTrajectory() 转换 control_msgs::action::FollowJointTrajectory::Goal goal; goal.trajectory trajectory.joint_trajectory; // 直接赋值 // ↓ Action Client 序列化并发送 controller_action_client_-async_send_goal(goal, ...);Action Client → 您的 Servercpp// ROS 2 中间件传输 // Topic: /arm_controller/follow_joint_trajectory/_action/send_goal // 您的 Server 接收 void handle_goal(const Goal::SharedPtr goal) { // goal-trajectory.points 包含所有轨迹点 }五、连接验证方法1.检查 Action Client 连接状态cpp// 在您的代码中添加调试信息 bool isConnected() { if (!controller_action_client_-wait_for_action_server(std::chrono::seconds(1))) { RCLCPP_ERROR(LOGGER, Cannot connect to your Action Server!); return false; } return true; }2.监控通信bash# 查看所有 Action ros2 action list # 应该看到 # /arm_controller/follow_joint_trajectory ← 您的 Server # 监控 Action 消息 ros2 topic echo /arm_controller/follow_joint_trajectory/_action/status六、您的 Server 需要满足的要求必须实现的接口cppclass YourActionServer { // 1. 必须提供正确的 Action 名称 action_server_ create_serverFollowJointTrajectory( this, arm_controller/follow_joint_trajectory, // ← 必须匹配配置 ... ); // 2. 必须正确处理关节名称 bool handle_goal(goal) { // 检查关节名称是否匹配 if (goal-trajectory.joint_names ! expected_joints) { return REJECT; // 返回 INVALID_JOINTS 错误码 } return ACCEPT; } // 3. 必须正确执行轨迹所有点不只是最后一个 void execute(goal_handle) { for (const auto point : goal-trajectory.points) { // 执行每个点 move_to_position(point.positions); // 等待到达 sleep(point.time_from_start); } result-error_code SUCCESSFUL; goal_handle-succeed(result); } };七、通信时序图textMoveIt Servo FollowJointTrajectoryControllerHandle Your Action Server | | | |--sendTrajectory()------| | | |--async_send_goal()----------------| | | | | |--goal_response_callback-----------| |--return true-----------| | | | | | | | (执行轨迹) | | | | |--result (succeed/abort)-----------| | | | | |--controllerDoneCallback()---------| | | |八、关键点总结组件作用关键代码FollowJointTrajectoryControllerHandleMoveIt 侧的客户端async_send_goal(goal)controller_action_client_ROS 2 Action Client连接到您的 ServersendTrajectory()发送轨迹的接口被 MoveIt Servo 调用controllerDoneCallback()接收执行结果处理 Server 返回的结果九、完整的工作示例cpp// 1. 您的 Server 启动 ros2 run myrobot_driver myrobot_driver_node // 2. MoveIt Servo 启动自动创建 Action Client ros2 launch moveit_servo servo.launch.py // 3. 当用户在 RViz 中拖动末端执行器时 // - MoveIt Servo 计算轨迹 // - 调用 sendTrajectory() // - Action Client 发送到您的 Server // - 您的 Server 执行并返回结果 // - controllerDoneCallback() 记录结果结论FollowJointTrajectoryControllerHandle是 MoveIt 与您的 Action Server 之间的直接通信桥梁。它通过 ROS 2 Action 协议自动将 MoveIt 规划的轨迹发送到您的 Server无需您修改任何 MoveIt 代码。只要您的 Server 实现了标准的FollowJointTrajectoryAction 接口并且 Action 名称匹配通信就会自动建立。