moveit servo 发指令给real arm

一、先明确：这个文件不直接接收 Servo！

它的角色是：MoveIt 内部的 “控制器客户端”，只接收 MoveIt 核心（MoveGroup）的轨迹，再发给你的 Action Server。

但Servo 也是 MoveIt 的一部分，Servo 计算出的轨迹，最终也是交给 MoveGroup → 再交给这个文件 → 再发给你。

所以你要的是：

Servo 生成轨迹 → MoveIt 核心 → 这个文件（Action Client） → 你的 Action Server

二、MoveIt Servo 如何工作（你必须懂）

Servo 是实时控制：

• 接收Twist（摇杆 / 键盘 / 手柄）
• 实时计算一小段关节轨迹（通常只有 1 个点）
• 按publish_period（你设的 0.03s）不断发布

Servo 本身不发 Action！它只发两种消息：

1. trajectory_msgs/msg/JointTrajectory（话题）
2. std_msgs/msg/Float64MultiArray（话题）

它默认不直接调用 Action！这就是你之前收不到的原因。

三、完整链路：Servo → 你的驱动（必须走这 3 层）

plaintext

1. MoveIt Servo 每30ms计算一小段轨迹 → 发布话题：/arm_controller/joint_trajectory 2. MoveIt MoveGroup / ControllerManager 监听这个话题 → 把轨迹交给 **FollowJointTrajectoryControllerHandle**（你贴的这个文件） 3. FollowJointTrajectoryControllerHandle（Action Client） → 封装成 FollowJointTrajectory::Goal → 发送 Action 到：/arm_controller/follow_joint_trajectory 4. 你的驱动（Action Server） → 收到 Goal → 执行

四、这个文件（FollowJointTrajectoryControllerHandle）的作用

它是 MoveIt 与你驱动之间的 “Action 转发器”

它内部结构（关键成员）

cpp

运行

// 这是 Action 客户端！连接你的驱动 rclcpp_action::Client<FollowJointTrajectory>::SharedPtr controller_action_client_; // MoveIt 调用它发送轨迹 bool sendTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) { // 1. 检查是否连接你的驱动 if (!isConnected()) return false; // 2. 封装成 Action Goal FollowJointTrajectory::Goal goal; goal.trajectory = trajectory.joint_trajectory; // 3. 发送给你的驱动！ controller_action_client_->async_send_goal(goal, ...); }

它怎么 “收到” Servo 的轨迹？

不是直接收！

• Servo 发JointTrajectory话题
• MoveIt 内部（ControllerManager）接收
• MoveIt 调用这个文件的 sendTrajectory ()
• 这个文件再发给你

五、你必须做的 4 个配置（才能通）

1. servo_parameters.yaml（关键！）

让 Servo发给 MoveIt 控制器话题，而不是直接发别处：

yaml

# 输出类型必须是 JointTrajectory（话题） command_out_type: "trajectory_msgs/msg/JointTrajectory" # 必须和控制器名字一致！ command_out_topic: /arm_controller/joint_trajectory publish_joint_positions: true publish_joint_velocities: true publish_joint_accelerations: false

2. moveit_controller_manager.yaml（必须！）

告诉 MoveIt：用 SimpleControllerManager + 加载这个 FollowJointTrajectoryControllerHandle

yaml

controller_names: - arm_controller arm_controller: # 必须是 FollowJointTrajectory type: FollowJointTrajectory # Action 命名空间（和你驱动一致） action_ns: follow_joint_trajectory default: true # 必须和你驱动、URDF 完全一样！ joints: - JM0 - JM1-2 - JM4-3 - JM4 - JM5 - YB

3. 你的驱动 Action 名（必须完全匹配）

cpp

运行

action_server_ = rclcpp_action::create_server<FollowJointTrajectory>( this, # 必须是：/控制器名/action_ns "/arm_controller/follow_joint_trajectory", ... );

4. launch 文件中启用 ControllerManager

python

运行

# 在 MoveIt launch 中 move_group_params = { "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", "moveit_controller_manager_config": "$(find your_config)/config/moveit_controller_manager.yaml" }

六、最关键：Servo 如何触发这个文件发送？

内部流程（逐秒）

1. 你按键盘→ 发Twist给 Servo
2. Servo 计算→ 生成 1 个点的JointTrajectory
3. Servo 发布→/arm_controller/joint_trajectory
4. MoveIt ControllerManager 接收
5. MoveIt 调用→FollowJointTrajectoryControllerHandle::sendTrajectory()
6. 这个文件封装→FollowJointTrajectory::Goal
7. 这个文件发送→ 你的驱动 Action Server
8. 你的驱动执行→handle_goal()→execute()

七、为什么你之前收不到？（99% 概率）

你之前在servo_parameters.yaml写了：

yaml

command_out_type: "follow_joint_trajectory" # ❌ 错误！ command_out_topic: "/arm_controller/follow_joint_trajectory" # ❌ 错误！

这是完全错误的！

• command_out_type不支持 Action
• Servo永远不会直接发 Action
• 必须发trajectory_msgs/msg/JointTrajectory话题

八、你贴的这个文件：发送代码逐行解释

cpp

运行

bool FollowJointTrajectoryControllerHandle::sendTrajectory( const moveit_msgs::msg::RobotTrajectory& trajectory) { // 检查 Action 客户端是否存在 if (!controller_action_client_) return false; // 检查是否连上你的驱动 if (!isConnected()) { RCLCPP_ERROR("Action client not connected"); return false; } // 封装 Goal（和你驱动接收的完全一样） control_msgs::action::FollowJointTrajectory::Goal goal; goal.trajectory = trajectory.joint_trajectory; // 发送给你的驱动！ auto future = controller_action_client_->async_send_goal(goal, options); current_goal_ = future.get(); return true; }

这一行就是发给你驱动的：

cpp

运行

controller_action_client_->async_send_goal(goal, options);

九、你的驱动收到的是什么？（完全对应）

这个文件发送：

cpp

运行

goal.trajectory = trajectory.joint_trajectory;

你的驱动接收：

cpp

运行

void execute(const std::shared_ptr<GoalHandle> goal_handle) { auto goal = goal_handle->get_goal(); auto& points = goal->trajectory.points; // ← 就是 Servo 生成的点！ ... }

完全一致！

十、一句话总结（你一定要记住）

MoveIt Servo 不直接发 Action。它发话题 → MoveIt 核心 → 这个文件（Action Client） → 你的 Action Server。

十一、现在立刻能跑的正确配置（复制即用）

1. servo_parameters.yaml

yaml

command_out_type: "trajectory_msgs/msg/JointTrajectory" command_out_topic: /arm_controller/joint_trajectory publish_period: 0.03 publish_joint_positions: true publish_joint_velocities: true publish_joint_accelerations: false

2. moveit_controller_manager.yaml

yaml

controller_names: - arm_controller arm_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true joints: - JM0 - JM1-2 - JM4-3 - JM4 - JM5 - YB

3. 你的驱动（不变，正确）

cpp

运行

action_server_ = rclcpp_action::create_server<FollowJointTrajectory>( this, "/arm_controller/follow_joint_trajectory", ... );

十二、测试命令（按顺序跑）

bash

运行

# 1. 启动你的驱动 ros2 run your_package myrobot_driver # 2. 启动 MoveIt（含 ControllerManager） ros2 launch your_moveit_config moveit.launch.py # 3. 启动 Servo ros2 launch your_moveit_config servo.launch.py # 4. 启动键盘控制 ros2 run moveit_servo servo_keyboard_input

现在按键盘 → 你的驱动 100% 收到 Goal！