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

my_real_arm_driver.cppcpp运行#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::ServerGoalHandleFollowJointTrajectory; class MyRealArmDriver : public rclcpp::Node { public: MyRealArmDriver() : Node(my_real_arm_drivers) { // 1. 创建 Action Server action_server_ rclcpp_action::create_serverFollowJointTrajectory( 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_publishersensor_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_ptrconst FollowJointTrajectory::Goal) { RCLCPP_INFO(this-get_logger(), 收到新轨迹请求); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse handle_cancel(const std::shared_ptrGoalHandle) { RCLCPP_INFO(this-get_logger(), ❌ 请求取消轨迹); return rclcpp_action::CancelResponse::ACCEPT; } void handle_accepted(const std::shared_ptrGoalHandle goal_handle) { // 线程执行轨迹不阻塞 Action 服务 std::thread{std::bind(MyRealArmDriver::execute_trajectory, this, std::placeholders::_1), goal_handle}.detach(); } // 执行轨迹核心在这里驱动真实机械臂 void execute_trajectory(const std::shared_ptrGoalHandle 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_sharedFollowJointTrajectory::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::ServerFollowJointTrajectory::SharedPtr action_server_; rclcpp::Publishersensor_msgs::msg::JointState::SharedPtr joint_pub_; std::vectorstd::string joint_names_; std::vectordouble current_positions_; std::thread publish_thread_; }; int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node std::make_sharedMyRealArmDriver(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }CMakeLists.txt 必须配置cmakecmake_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 依赖xmlbuildtool_dependament_cmake/buildtool_depend dependrclcpp/depend dependrclcpp_action/depend dependcontrol_msgs/depend dependsensor_msgs/depend这个 C 节点实现了什么完全和你 Python 版本功能一模一样启动 Action Server/arm_controller/follow_joint_trajectory接收 MoveIt / 终端发送的轨迹在 execute_trajectory 里驱动你的真实机械臂50Hz 发布 /joint_statesRViz2 自动同步真实机械臂姿态最重要你的 launch 文件必须这样写不能启动 ros2_control_nodepython运行# 完全删除以下内容 # ros2_control_node # spawner arm_controller # spawner joint_state_broadcaster # 只启动你的驱动节点 def generate_launch_description(): your_driver Node( packagemy_arm_driver, executablemy_real_arm_driver, outputscreen ) 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_ 给电机 //