自定义机械臂驱动（Action Server + /joint_states 发布）

my_real_arm_driver.cpp

cpp

运行

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include <thread>
#include <chrono>

using namespace std::chrono_literals;
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using GoalHandle = rclcpp_action::ServerGoalHandle<FollowJointTrajectory>;

class MyRealArmDriver : public rclcpp::Node
{
public:
    MyRealArmDriver() : Node("my_real_arm_drivers")
    {
        // 1. 创建 Action Server
        action_server_ = rclcpp_action::create_server<FollowJointTrajectory>(
            this,
            "/arm_controller/follow_joint_trajectory",
            std::bind(&MyRealArmDriver::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&MyRealArmDriver::handle_cancel, this, std::placeholders::_1),
            std::bind(&MyRealArmDriver::handle_accepted, this, std::placeholders::_1)
        );

        RCLCPP_INFO(this->get_logger(), "✅ Action Server 已启动: /arm_controller/follow_joint_trajectory");

        // 2. 创建 /joint_states 发布器
        joint_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);

        // 关节名称（必须和 URDF / MoveIt 一致）
        joint_names_ = {"JM0", "JM1-2", "JM4-3", "JM4", "JM5", "YB"};

        // 当前关节角度
        current_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

        // 启动线程发布 /joint_states
        publish_thread_ = std::thread(&MyRealArmDriver::publish_joint_states, this);
    }

    ~MyRealArmDriver()
    {
        if (publish_thread_.joinable())
            publish_thread_.join();
    }

private:
    // ====================== Action Server 相关 ======================
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID &,
        std::shared_ptr<const FollowJointTrajectory::Goal>)
    {
        RCLCPP_INFO(this->get_logger(), "📥 收到新轨迹请求");
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandle>)
    {
        RCLCPP_INFO(this->get_logger(), "❌ 请求取消轨迹");
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle)
    {
        // 线程执行轨迹，不阻塞 Action 服务
        std::thread{std::bind(&MyRealArmDriver::execute_trajectory, this, std::placeholders::_1), goal_handle}.detach();
    }

    // 执行轨迹（核心：在这里驱动真实机械臂）
    void execute_trajectory(const std::shared_ptr<GoalHandle> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "🚀 开始执行轨迹");

        const auto goal = goal_handle->get_goal();
        const auto &points = goal->trajectory.points;

        // 遍历所有轨迹点
        for (const auto &pt : points)
        {
            // 获取目标位置
            current_positions_ = pt.positions;

            RCLCPP_INFO(this->get_logger(), "目标位置: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f]",
                current_positions_[0], current_positions_[1], current_positions_[2],
                current_positions_[3], current_positions_[4], current_positions_[5]);

            // =========================
            // 在这里写你的真实机械臂驱动代码
            // 发送 current_positions_ 给电机
            // =========================

            std::this_thread::sleep_for(50ms);
        }

        // 返回成功
        auto result = std::make_shared<FollowJointTrajectory::Result>();
        result->error_code = 0;
        goal_handle->succeed(result);

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

    // ====================== 发布 /joint_states ======================
    void publish_joint_states()
    {
        rclcpp::Rate rate(50); // 50Hz 发布

        while (rclcpp::ok())
        {
            sensor_msgs::msg::JointState msg;
            msg.header.stamp = this->now();
            msg.name = joint_names_;
            msg.position = current_positions_;

            joint_pub_->publish(msg);
            rate.sleep();
        }
    }

    // 变量
    rclcpp_action::Server<FollowJointTrajectory>::SharedPtr action_server_;
    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
    std::vector<std::string> joint_names_;
    std::vector<double> current_positions_;
    std::thread publish_thread_;
};

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

---

CMakeLists.txt 必须配置

cmake

cmake_minimum_required(VERSION 3.8)
project(my_arm_driver)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(control_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)

add_executable(my_real_arm_driver src/my_real_arm_driver.cpp)

ament_target_dependencies(my_real_arm_driver
  rclcpp
  rclcpp_action
  control_msgs
  sensor_msgs
)

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

ament_package()

---

package.xml 依赖

xml

<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>control_msgs</depend>
<depend>sensor_msgs</depend>

---

这个 C++ 节点实现了什么？

完全和你 Python 版本功能一模一样：

1. 启动 Action Server /arm_controller/follow_joint_trajectory
2. 接收 MoveIt / 终端发送的轨迹
3. 在 execute_trajectory 里驱动你的真实机械臂
4. 50Hz 发布 /joint_states
5. RViz2 自动同步真实机械臂姿态

---

最重要：你的 launch 文件必须这样写（不能启动 ros2_control_node）

python

运行

# 完全删除以下内容！
# ros2_control_node
# spawner arm_controller
# spawner joint_state_broadcaster

# 只启动你的驱动节点
def generate_launch_description():
    your_driver = Node(
        package="my_arm_driver",
        executable="my_real_arm_driver",
        output="screen"
    )

    return LaunchDescription([your_driver])

---

测试命令（你发这个，C++ 节点立刻收到）

bash

运行

ros2 action send_goal /arm_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{
  trajectory: {
    joint_names: [JM0, JM1-2, JM4-3, JM4, JM5, YB],
    points: [
      {
        positions: [0.5, 0.0, 0.5, 0.0, 0.0, 0.0],
        time_from_start: {sec: 2, nanosec: 0}
      }
    ]
  }
}"

---

你只需要改一个地方就能驱动真实机械臂

在 execute_trajectory() 函数里：

cpp

运行

// =========================
// 在这里写你的真实机械臂驱动
// 串口 / CAN / 网口 发送 current_positions_ 给电机
// =========================