MoveIt Servo 如何通过 FollowJointTrajectoryControllerHandle Action Server 通信

一、通信架构图

text

MoveIt Servo (moveit_servo node) ↓ MoveItSimpleControllerManager (插件管理器) ↓ FollowJointTrajectoryControllerHandle (您看到的这个类) ↓ [内部创建 Action Client] ↓ ROS 2 Action Client (自动生成) ↓ [通过 ROS 2 Action 协议] ↓ 您的 Action Server (myrobot_driver) ↓ 真实硬件

二、关键通信机制

1.Action Client 的创建

FollowJointTrajectoryControllerHandle在初始化时会创建 Action Client：

cpp

// 构造函数中（未显示但存在） 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.发送轨迹的核心代码

cpp

bool 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

步骤2：Servo 运行时

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_ptr<GoalHandle> 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_shared<FollowJointTrajectory::Result>(); result->error_code = FollowJointTrajectory::Result::SUCCESSFUL; goal_handle->succeed(result); } };

四、数据流转详解

MoveIt Servo → Action Client

cpp

// 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 → 您的 Server

cpp

// 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 需要满足的要求

必须实现的接口

cpp

class YourActionServer { // 1. 必须提供正确的 Action 名称 action_server_ = create_server<FollowJointTrajectory>( 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); } };

七、通信时序图

text

MoveIt Servo FollowJointTrajectoryControllerHandle Your Action Server | | | |--sendTrajectory()------>| | | |--async_send_goal()---------------->| | | | | |<--goal_response_callback-----------| |<--return true-----------| | | | | | | | (执行轨迹) | | | | |<--result (succeed/abort)-----------| | | | | |--controllerDoneCallback()--------->| | | |

八、关键点总结

九、完整的工作示例

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 名称匹配，通信就会自动建立。