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！

---