MoveGroup 规划轨迹 → 直接交给 MoveIt2 Servo 执行

我给你最简单、最稳定、工业常用 的方法，不用改复杂代码，直接打通！

---

✅ 核心原理（一句话）

1. MoveGroup 做运动规划 → 输出一条完整轨迹
2. 把轨迹解析成一串点
3. 逐点发给 MoveIt Servo
4. Servo 平滑实时执行

---

🚀 【最简单方案】C++ 直接转发轨迹给 Servo

我直接给你能复制即用的代码！

作用：

订阅 /plan 规划结果 → 转成 Servo 可执行的轨迹流 → 发给 /arm_controller/joint_trajectory

---

✅ 完整代码：plan_to_servo.cpp

cpp

运行

#include <rclcpp/rclcpp.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>

using namespace std::placeholders;
using namespace std::chrono_literals;

class PlanToServo : public rclcpp::Node
{
public:
    PlanToServo() : Node("plan_to_servo")
    {
        // 订阅 MoveGroup 输出的规划轨迹
        plan_sub_ = this->create_subscription<moveit_msgs::msg::RobotTrajectory>(
            "/plan", 10, std::bind(&PlanToServo::callback, this, _1));

        // 发布给 MoveIt Servo 执行
        servo_pub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>(
            "/arm_controller/joint_trajectory", 10);

        RCLCPP_INFO(get_logger(), "✅ 规划轨迹 → MoveIt Servo 执行节点已启动");
    }

private:
    rclcpp::Subscription<moveit_msgs::msg::RobotTrajectory>::SharedPtr plan_sub_;
    rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr servo_pub_;

    void callback(const moveit_msgs::msg::RobotTrajectory::SharedPtr msg)
    {
        RCLCPP_INFO(get_logger(), "📥 收到 MoveGroup 规划轨迹，开始转发给 Servo 执行");

        auto traj = msg->joint_trajectory;

        // 逐点发布，让 Servo 平滑执行
        for (size_t i = 0; i < traj.points.size(); i += 2)  // 每2个点发一次，更平滑
        {
            trajectory_msgs::msg::JointTrajectory out;
            out.header.stamp = this->now();
            out.joint_names = traj.joint_names;
            out.points.push_back(traj.points[i]);

            servo_pub_->publish(out);
            rclcpp::sleep_for(30ms);  // 控制速度
        }

        RCLCPP_INFO(get_logger(), "✅ 轨迹执行完成");
    }
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PlanToServo>());
    rclcpp::shutdown();
    return 0;
}

---

✅ CMakeLists.txt 添加

cmake

add_executable(plan_to_servo src/plan_to_servo.cpp)
ament_target_dependencies(plan_to_servo
  rclcpp
  moveit_msgs
  trajectory_msgs
)

install(TARGETS
  plan_to_servo
  DESTINATION lib/${PROJECT_NAME}
)

---

🎯 【完整通路】

plaintext

MoveGroup 规划
  → /plan（规划轨迹）
  → plan_to_servo 节点
  → /arm_controller/joint_trajectory（发给 Servo）
  → 你的驱动
  → 电机运动

---

🚀 【启动方法】

bash

运行

# 1. 启动 MoveGroup + Servo + 你的驱动
ros2 launch moveit2_tutorials servo_group_robot.launch.py

# 2. 启动轨迹转发节点
ros2 run moveit2_tutorials plan_to_servo

# 3. 用 MoveGroup 规划（RVIZ 里拖动）
# 4. 规划完成 → 自动被 Servo 执行！

---

🎉 【效果】

你在 RVIZ 里：

1. 拖动机械臂
2. 点 Plan
3. 自动交给 Servo 执行
4. 真实机械臂平滑运动

---

✅ 【最重要】你现有的驱动 完全不用改！

你之前写的：

cpp

运行

subscribe("/arm_controller/joint_trajectory", ...)