自定义action server 接收arm_controller 指令

功能：接收来自 MoveIt / MoveIt Servo 的机械臂轨迹指令，你只需要在里面写控制真实机械臂的代码即可。

一、先给你最重要的对应关系（必须看）

你现在的配置：

yaml

arm_controller:
  action_ns: follow_joint_trajectory

→ 你的 Action Server 必须监听：

/arm_controller/follow_joint_trajectory

只要监听这个话题，所有 RViz / MoveIt / Servo 指令都会流到你这里！

---

二、C++ 自定义 Action Server（完整可运行）

文件名：robot_arm_action_server.cpp

cpp

运行

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <control_msgs/action/follow_joint_trajectory.hpp>

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

class RobotArmActionServer : public rclcpp::Node
{
public:
  RobotArmActionServer() : Node("robot_arm_action_server")
  {
    // 【关键】创建 Action Server，匹配你的话题名
    action_server_ = rclcpp_action::create_server<FollowJointTrajectory>(
      this,
      "/arm_controller/follow_joint_trajectory",  // 必须和你配置一致
      std::bind(&RobotArmActionServer::handle_goal, this, _1, _2),
      std::bind(&RobotArmActionServer::handle_cancel, this, _1),
      std::bind(&RobotArmActionServer::handle_accepted, this, _1)
    );

    RCLCPP_INFO(this->get_logger(), "✅ 自定义机械臂 Action Server 已启动");
    RCLCPP_INFO(this->get_logger(), "✅ 监听：/arm_controller/follow_joint_trajectory");
  }

private:
  rclcpp_action::Server<FollowJointTrajectory>::SharedPtr action_server_;

  // 1. 收到目标时调用
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID &,
    std::shared_ptr<const FollowJointTrajectory::Goal> goal)
  {
    RCLCPP_INFO(this->get_logger(), "📥 收到轨迹目标，关节数：%d",
                (int)goal->trajectory.joint_names.size());
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  // 2. 收到取消指令时调用
  rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandle>)
  {
    RCLCPP_WARN(this->get_logger(), "❌ 任务被取消");
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  // 3. 开始执行轨迹
  void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle)
  {
    std::thread{std::bind(&RobotArmActionServer::execute, this, _1), goal_handle}.detach();
  }

  // 【核心】执行轨迹 → 你在这里控制真实机械臂
  void execute(const std::shared_ptr<GoalHandle> goal_handle)
  {
    auto goal = goal_handle->get_goal();
    auto result = std::make_shared<FollowJointTrajectory::Result>();

    RCLCPP_INFO(this->get_logger(), "---------------------------------------");
    RCLCPP_INFO(this->get_logger(), "🚀 开始执行轨迹，点数：%d",
                (int)goal->trajectory.points.size());

    // 遍历所有轨迹点
    for (auto &point : goal->trajectory.points)
    {
      // ==============================================
      // 👇👇👇 【你写真实机械臂控制代码】
      // ==============================================
      std::vector<double> positions = point.positions;
      std::vector<double> velocities = point.velocities;

      RCLCPP_INFO(this->get_logger(), "📌 执行关节位置：");
      for (double p : positions) {
        RCLCPP_INFO(this->get_logger(), "  %.3f", p);
      }

      // 这里加：串口/CAN/以太网发给真机
      // 例如：send_trajectory_to_robot(positions, velocities);

      // 模拟延时（真机删除）
      rclcpp::sleep_for(std::chrono::milliseconds(100));
    }

    // 执行完成
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "✅ 轨迹执行完成！");
  }
};

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

---

三、CMakeLists.txt 必须加的依赖

cmake

find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(control_msgs REQUIRED)

add_executable(robot_arm_action_server src/robot_arm_action_server.cpp)
ament_target_dependencies(robot_arm_action_server
  rclcpp
  rclcpp_action
  control_msgs
)

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

---

四、package.xml 必须加

xml

<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>control_msgs</depend>

---

五、这个程序的作用（超级重要）

它就是你真实机械臂的 "指令入口"！

指令流现在变成：

plaintext

MoveIt / MoveIt Servo
    ↓
/arm_controller/follow_joint_trajectory
    ↓
【你的 C++ Action Server】
    ↓
真实机械臂硬件

你只要在 execute () 函数里写硬件控制代码，整个系统就通了！

---

六、如何测试？

1. 启动你原来的 launch
2. 启动这个 action server
3. 在 RViz 里拖动机械臂 → 发送指令
4. 你会看到 C++ 程序收到所有关节位置！

---

七、我再给你一句最关键的话

你现在的整个系统，指令最终就是发到这个 Action Server

你只需要在这里写真机驱动，就能控制真实机械臂