Moveit2 C++ API

一、介绍

MoveIt2 是 ROS2 下广泛使用的机械臂运动规划框架，提供了路径规划、碰撞检测、逆运动学求解、轨迹执行、运动仿真等功能。虽然 MoveIt2 提供了 RViz2 GUI + MoveGroup 接口 来完成大部分规划任务，但在一些复杂应用中：

需要在代码中动态生成目标姿态和路径
需要对运动规划进行二次封装或集成到自定义机器人控制系统
需要将规划结果与外部传感器、视觉识别、AI算法结合

此时就需要直接使用 MoveIt2 C++ API。也可以使用Python版本，但是据说没有C++版本好用。下面我们尝试自己编写代码来控制机械臂。

二、案例

整体的结构是这样的：

本部分我们聚焦于左侧，使用API来控制Moveit2。

2.1 快速上手：姿态切换

创建一个新的包作为控制包：

bash

cd ros2_ws/src
ros2 pkg create my_robot_commander_cpp --dependencies rclcpp

在src下新建文件test_moveit.cpp:

cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char **argv)
{ 
    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("test_moveit");
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    auto spinner = std::thread([&executor]() { executor.spin(); });

    auto arm = moveit::planning_interface::MoveGroupInterface(node, "arm");
    arm.setMaxVelocityScalingFactor(1.0);
    arm.setMaxAccelerationScalingFactor(1.0);

    // 设置初始位置和目标位置
    arm.setStartStateToCurrentState();
    arm.setNamedTarget("pose_1");

    // 规划运动路径
    moveit::planning_interface::MoveGroupInterface::Plan Plan1;
    bool success1 = (arm.plan(Plan1) == moveit::core::MoveItErrorCode::SUCCESS);

    if (success1)
    {
        // 运行运动
        arm.execute(Plan1);
    }
    else
    {
        RCLCPP_ERROR(node->get_logger(), "Planning failed");
    }

    rclcpp::shutdown();
    spinner.join();
    return 0;
}

这段代码展示了如何使用 MoveIt2 C++ API 控制机械臂：

初始化 ROS2 节点

cpp

rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("test_moveit");

创建 ROS2 节点，为 MoveIt2 提供运行环境。

启动回调处理线程

cpp

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
auto spinner = std::thread([&executor]() { executor.spin(); });

持续处理 ROS2 消息和 MoveIt2 动作反馈。

创建 MoveGroupInterface 对象
cpp
```
auto arm = moveit::planning_interface::MoveGroupInterface(node, "arm");
```
1
"arm" 是机械臂规划组，通过它可以规划和执行运动。

设置速度、起点和目标

cpp

arm.setMaxVelocityScalingFactor(1.0);
arm.setMaxAccelerationScalingFactor(1.0);
arm.setStartStateToCurrentState();
arm.setNamedTarget("pose_1");

控制机械臂运动速度，设置规划起点和目标姿态。

规划轨迹并执行

cpp

moveit::planning_interface::MoveGroupInterface::Plan Plan1;
if (arm.plan(Plan1) == moveit::core::MoveItErrorCode::SUCCESS)
    arm.execute(Plan1);
else
    RCLCPP_ERROR(node->get_logger(), "Planning failed");

根据起点和目标生成轨迹，规划成功后执行运动，否则报错。

关闭 ROS2
cpp
```
rclcpp::shutdown();
spinner.join();
```
1
2
释放资源，程序结束。

最后修改项目的CMakelist:

cmake_minimum_required(VERSION 3.8)
project(my_robot_commander_cpp)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)

add_executable(test_moveit src/test_moveit.cpp)
ament_target_dependencies(test_moveit rclcpp moveit_ros_planning_interface)

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


ament_package()

并在package.xml里添加依赖：

xml

  <depend>rclcpp</depend>
  <depend>moveit_ros_planning_interface</depend>

编译：

cd ros2_ws
colcon build
source install/setup.bash

启动launch文件

ros2 launch my_robot_bringup robot.launch.py

然后执行我们编写的控制器测试包：

ros2 run my_robot_commander_cpp test_moveit

运行成功，机器人的目标位置发生了改变：

2.2 细节控制：指定关节角度

代码如下：

cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char **argv)
{ 
    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("test_moveit");
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    auto spinner = std::thread([&executor]() { executor.spin(); });

    auto arm = moveit::planning_interface::MoveGroupInterface(node, "arm");
    arm.setMaxVelocityScalingFactor(1.0);
    arm.setMaxAccelerationScalingFactor(1.0);

    // 指定关节角度
    std::vector<double> joints = {1.5,0.5,0.0,1.5,0.0,-0.7};
    arm.setStartStateToCurrentState();
    arm.setJointValueTarget(joints);

    moveit::planning_interface::MoveGroupInterface::Plan Plan1;
    bool success1 = (arm.plan(Plan1) == moveit::core::MoveItErrorCode::SUCCESS);
    if (success1)
    {
        arm.execute(Plan1);
    }
    else
    {
        RCLCPP_ERROR(node->get_logger(), "Planning failed");
    }



    rclcpp::shutdown();
    spinner.join();
    return 0;
}

执行效果如下：

2.3 目标姿态控制（重要）

代码如下：

cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char **argv)
{ 
    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("test_moveit");
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    auto spinner = std::thread([&executor]() { executor.spin(); });

    auto arm = moveit::planning_interface::MoveGroupInterface(node, "arm");
    arm.setMaxVelocityScalingFactor(1.0);
    arm.setMaxAccelerationScalingFactor(1.0);

    // 指定目标姿态
    tf2::Quaternion q;
    q.setRPY(3.14, 0.0, 0.0);
    q = q.normalize();

    geometry_msgs::msg::PoseStamped target_pose;
    target_pose.header.frame_id = "base_link";
    target_pose.pose.position.x = 0.0;
    target_pose.pose.position.y = -0.7;
    target_pose.pose.position.z = 0.4;
    target_pose.pose.orientation.x = q.getX();
    target_pose.pose.orientation.y = q.getY();
    target_pose.pose.orientation.z = q.getZ();
    target_pose.pose.orientation.w = q.getW();

    arm.setStartStateToCurrentState();
    arm.setPoseTarget(target_pose);

    moveit::planning_interface::MoveGroupInterface::Plan Plan1;
    bool success1 = (arm.plan(Plan1) == moveit::core::MoveItErrorCode::SUCCESS);
    if (success1)
    {
        arm.execute(Plan1);
    }
    else
    {
        RCLCPP_ERROR(node->get_logger(), "Planning failed");
    }


    rclcpp::shutdown();
    spinner.join();
    return 0;
}

在 MoveIt2 中，通过 Pose（位置 + 姿态）来描述机械臂末端执行器的目标点。本段代码主要分为两部分：

① 设置目标姿态（orientation）

cpp

tf2::Quaternion q;
q.setRPY(3.14, 0.0, 0.0);  // 通过 Roll、Pitch、Yaw 设置目标旋转
q = q.normalize();

这里使用 欧拉角 RPY（Roll、Pitch、Yaw） 来定义末端执行器的方向：

Roll = 3.14 → 绕 X 轴旋转 180°
Pitch = 0
Yaw = 0

然后将其转换成四元数（MoveIt 使用四元数表示姿态）。最后：

cpp

target_pose.pose.orientation.x = q.getX();
...

将四元数填入目标姿态。

这一步决定机械臂末端执行器面向哪个方向，例如爪子是向下、向上还是侧向。

② 设置目标位置（position）

cpp

target_pose.header.frame_id = "base_link";  // 目标点坐标参考 base_link

target_pose.pose.position.x = 0.0;
target_pose.pose.position.y = -0.7;
target_pose.pose.position.z = 0.4;

目标位置是一个 3D 坐标，含义如下：

x = 0.0 → 前后方向（前为正）
y = -0.7 → 左右方向（右为负）
z = 0.4 → 垂直高度（上为正）

这些坐标全部基于 base_link 坐标系，即机械臂底座作为参考点。

这一步决定机械臂末端要到达空间中的具体点。

PoseTarget = 位置 + 姿态

MoveIt 中：

位置（Position） = x, y, z姿态（Orientation） = 四元数 q

两者共同决定机械臂末端到达的完整位姿（Pose）：

cpp

arm.setPoseTarget(target_pose);

之后就由 MoveIt 来规划路径。

运行后呈现这个状态：

2.4 笛卡尔路径规划

在机械臂控制中，“笛卡尔路径（Cartesian Path）”指的是：让机械臂末端执行器沿着空间中的一条直线或曲线运动，而不是关节角度随便变化的路径。

也就是说：

机器人末端（tool0 / ee_link）在 3D 空间中的轨迹是连续且可控的轨迹，而不是 MoveIt 自动规划出的任意关节路径。

MoveIt 默认规划是在关节空间里规划：

机械臂可能会走奇怪的弯路，末端轨迹不是直线，无法精确控制末端在空间的路径

对以下任务特别不适用：

抓取、插入、贴装（必须直线移动）
点胶、焊接（需要固定轨迹）
在狭窄空间运动（不能乱走弯路）

所以需要使用 “笛卡尔路径”。

在2.3 的代码基础上加上一段笛卡尔路径规划，让机械臂运动完末端抬升20cm:

cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char **argv)
{ 
    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("test_moveit");
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    auto spinner = std::thread([&executor]() { executor.spin(); });

    auto arm = moveit::planning_interface::MoveGroupInterface(node, "arm");
    arm.setMaxVelocityScalingFactor(1.0);
    arm.setMaxAccelerationScalingFactor(1.0);

 
    // 指定目标姿态
    tf2::Quaternion q;
    q.setRPY(3.14, 0.0, 0.0);
    q = q.normalize();

    geometry_msgs::msg::PoseStamped target_pose;
    target_pose.header.frame_id = "base_link";
    target_pose.pose.position.x = 0.0;
    target_pose.pose.position.y = -0.7;
    target_pose.pose.position.z = 0.4;
    target_pose.pose.orientation.x = q.getX();
    target_pose.pose.orientation.y = q.getY();
    target_pose.pose.orientation.z = q.getZ();
    target_pose.pose.orientation.w = q.getW();

    arm.setStartStateToCurrentState();
    arm.setPoseTarget(target_pose);

    moveit::planning_interface::MoveGroupInterface::Plan Plan1;
    bool success1 = (arm.plan(Plan1) == moveit::core::MoveItErrorCode::SUCCESS);
    if (success1)
    {
        arm.execute(Plan1);
    }
    else
    {
        RCLCPP_ERROR(node->get_logger(), "Planning failed");
    }

    // 笛卡尔空间路径规划
    std::vector<geometry_msgs::msg::Pose> waypoints;
    geometry_msgs::msg::Pose pose1 = arm.getCurrentPose().pose;
    pose1.position.z += 0.2;
    waypoints.push_back(pose1);

    moveit_msgs::msg::RobotTrajectory trajectory;
    double fraction = arm.computeCartesianPath(waypoints, 0.01,  0.0,  trajectory);

    if (fraction >= 0.9)
    {
        arm.execute(trajectory);
    }
    else
    {
        RCLCPP_ERROR(node->get_logger(), "Cartesian path planning failed");
    }

    rclcpp::shutdown();
    spinner.join();
    return 0;
}

执行后效果如下图，可以看到最后这段直线就是我们想要的结果：

代码： https://github.com/DuRuofu/moveit2_learn/commit/d4f15d76241b578cc3732bb6a418ed352cd40721

三、编写实际的控制节点(应用)

在 my_robot_commander_cpp包新建文件 commander_template.cpp:

cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <example_interfaces/msg/bool.hpp>
#include <tf2/LinearMath/Quaternion.h>

using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface;

class Commander
{
private:
    std::shared_ptr<rclcpp::Node> node_;
    std::shared_ptr<MoveGroupInterface> arm_;
    std::shared_ptr<MoveGroupInterface> gripper_;
    rclcpp::Subscription<example_interfaces::msg::Bool>::SharedPtr open_gripper_sub_;

    /**
     * @brief 通用规划与执行函数
     * @param interface MoveGroupInterface 指针（arm 或 gripper）
     */
    void planAndExecute(const std::shared_ptr<MoveGroupInterface> &interface)
    {
        MoveGroupInterface::Plan plan;
        bool success = (interface->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);

        if (success)
        {
            interface->execute(plan);
        }
    }

    /**
     * @brief 接收 /open_gripper 话题回调
     */
    void OpenGripperCallback(const example_interfaces::msg::Bool::SharedPtr msg)
    {
        if (msg->data)
            openGripper();
        else
            closeGripper();
    }

public:
    /**
     * @brief 构造函数，初始化两个 MoveGroup（arm + gripper）
     */
    Commander(std::shared_ptr<rclcpp::Node> node)
    {
        node_ = node;

        arm_ = std::make_shared<MoveGroupInterface>(node_, "arm");
        arm_->setMaxVelocityScalingFactor(1.0);
        arm_->setMaxAccelerationScalingFactor(1.0);

        gripper_ = std::make_shared<MoveGroupInterface>(node_, "gripper");

        // 创建订阅者（打开/关闭手爪）
        open_gripper_sub_ = node_->create_subscription<example_interfaces::msg::Bool>(
            "open_gripper",
            10,
            std::bind(&Commander::OpenGripperCallback, this, std::placeholders::_1)
        );
    }

    /**
     * @brief 通过 MoveIt NamedTarget 运动
     */
    void goToNamedTarget(const std::string &name)
    {
        arm_->setStartStateToCurrentState();
        arm_->setNamedTarget(name);
        planAndExecute(arm_);
    }

    /**
     * @brief 通过关节角度运动
     */
    void goToJointTarget(const std::vector<double> &joint_values)
    {
        arm_->setStartStateToCurrentState();
        arm_->setJointValueTarget(joint_values);
        planAndExecute(arm_);
    }

    /**
     * @brief 通过末端位姿运动，可切换关节空间规划或笛卡尔路径
     */
    void goToPoseTarget(double x, double y, double z, double roll, double pitch, double yaw, bool cartesian_path=false)
    {
        // 转姿态为四元数
        tf2::Quaternion q;
        q.setRPY(roll, pitch, yaw);
        q.normalize();

        geometry_msgs::msg::PoseStamped target_pose;
        target_pose.header.frame_id = "base_link";
        target_pose.pose.position.x = x;
        target_pose.pose.position.y = y;
        target_pose.pose.position.z = z;
        target_pose.pose.orientation.x = q.getX();
        target_pose.pose.orientation.y = q.getY();
        target_pose.pose.orientation.z = q.getZ();
        target_pose.pose.orientation.w = q.getW();

        arm_->setStartStateToCurrentState();

        if (!cartesian_path)
        {
            // 普通关节空间规划
            arm_->setPoseTarget(target_pose);
            planAndExecute(arm_);
        }
        else
        {
            // 笛卡尔路径规划
            std::vector<geometry_msgs::msg::Pose> waypoints;
            waypoints.push_back(target_pose.pose);

            moveit_msgs::msg::RobotTrajectory trajectory;

            double fraction = arm_->computeCartesianPath(
                waypoints,   // 路点列表
                0.01,        // eef_step（越小越平滑）
                0.0,         // jump_threshold
                trajectory,  // 输出轨迹
                false        // 不避障，可保证路径成功
            );

            if (fraction >= 0.9)
            {
                arm_->execute(trajectory);
            }
            else
            {
                RCLCPP_ERROR(node_->get_logger(), "Cartesian path planning failed (%.2f)", fraction);
            }
        }
    }

    /**
     * @brief 打开手爪
     */
    void openGripper()
    {
        gripper_->setStartStateToCurrentState();
        gripper_->setNamedTarget("gripper_open");
        planAndExecute(gripper_);
    }

    /**
     * @brief 关闭手爪
     */
    void closeGripper()
    {
        gripper_->setStartStateToCurrentState();
        gripper_->setNamedTarget("gripper_closed");
        planAndExecute(gripper_);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("commander");

    Commander commander(node);

    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172

在package.xml里添加依赖：

xml

  <depend>example_interfaces</depend>

在CMakeLists.txt里添加编译选项：

cmake_minimum_required(VERSION 3.8)
project(my_robot_commander_cpp)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(example_interfaces REQUIRED)


add_executable(test_moveit src/test_moveit.cpp)
ament_target_dependencies(test_moveit rclcpp moveit_ros_planning_interface)


add_executable(commander src/commander_template.cpp)
ament_target_dependencies(commander rclcpp example_interfaces moveit_ros_planning_interface)

install(TARGETS
  test_moveit
  commander
  DESTINATION lib/${PROJECT_NAME}/
)


ament_package()

这里的代码我就不在这里写解释了，大可复制给AI解读。在代码里我们封装了第二部分的几个方法，并且针对夹爪创建了一个布尔类型的消息订阅。可以通过open_gripper来控制夹爪状态。

编译：

cd ros2_ws
colcon build
source install/setup.bash

启动launch文件

ros2 launch my_robot_bringup robot.launch.py

然后执行我们编写的控制器封装：

ros2 run my_robot_commander_cpp commander

正常启动：

在另一个终端发送夹爪控制指令：

ros2 topic pub -1 /open_gripper example_interfaces/msg/Bool "{data: false}"

就可以看到rviz里的夹爪闭合了，这里就不放图了。

按照这个思路，我们还可以进一步，增加其他消息接口，来控制机械臂移动，代码如下：

cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <example_interfaces/msg/bool.hpp>
#include <example_interfaces/msg/float64_multi_array.hpp>
#include <tf2/LinearMath/Quaternion.h>

using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface;

class Commander
{
private:
    std::shared_ptr<rclcpp::Node> node_;
    std::shared_ptr<MoveGroupInterface> arm_;
    std::shared_ptr<MoveGroupInterface> gripper_;
    rclcpp::Subscription<example_interfaces::msg::Bool>::SharedPtr open_gripper_sub_;
    rclcpp::Subscription<example_interfaces::msg::Float64MultiArray>::SharedPtr joint_cmd_sub_;

    /**
     * @brief 通用规划与执行函数
     * @param interface MoveGroupInterface 指针（arm 或 gripper）
     */
    void planAndExecute(const std::shared_ptr<MoveGroupInterface> &interface)
    {
        MoveGroupInterface::Plan plan;
        bool success = (interface->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);

        if (success)
        {
            interface->execute(plan);
        }
    }

    /**
     * @brief 接收 /open_gripper 话题回调
     */
    void openGripperCallback(const example_interfaces::msg::Bool::SharedPtr msg)
    {
        if (msg->data)
            openGripper();
        else
            closeGripper();
    }
    /**
     * @brief 接收 /joint_cmd 话题回调
     */
    void jointCmdCallback(const example_interfaces::msg::Float64MultiArray::SharedPtr msg)
    {
        std::vector<double> joint_values;

        if (msg->data.size()== 6)
        {
            goToJointTarget(msg->data);
        }
    }

public:
    /**
     * @brief 构造函数，初始化两个 MoveGroup（arm + gripper）
     */
    Commander(std::shared_ptr<rclcpp::Node> node)
    {
        node_ = node;

        arm_ = std::make_shared<MoveGroupInterface>(node_, "arm");
        arm_->setMaxVelocityScalingFactor(1.0);
        arm_->setMaxAccelerationScalingFactor(1.0);

        gripper_ = std::make_shared<MoveGroupInterface>(node_, "gripper");

        // 创建订阅者（打开/关闭手爪）
        open_gripper_sub_ = node_->create_subscription<example_interfaces::msg::Bool>(
            "open_gripper",
            10,
            std::bind(&Commander::openGripperCallback, this, std::placeholders::_1)
        );

        // 创建订阅者（关节角度控制）
        joint_cmd_sub_ = node_->create_subscription<example_interfaces::msg::Float64MultiArray>(
            "joint_command",
            10,
            std::bind(&Commander::jointCmdCallback, this, std::placeholders::_1)
        );
    }

    /**
     * @brief 通过 MoveIt NamedTarget 运动
     */
    void goToNamedTarget(const std::string &name)
    {
        arm_->setStartStateToCurrentState();
        arm_->setNamedTarget(name);
        planAndExecute(arm_);
    }

    /**
     * @brief 通过关节角度运动
     */
    void goToJointTarget(const std::vector<double> &joint_values)
    {
        arm_->setStartStateToCurrentState();
        arm_->setJointValueTarget(joint_values);
        planAndExecute(arm_);
    }

    /**
     * @brief 通过末端位姿运动，可切换关节空间规划或笛卡尔路径
     */
    void goToPoseTarget(double x, double y, double z, double roll, double pitch, double yaw, bool cartesian_path=false)
    {
        // 转姿态为四元数
        tf2::Quaternion q;
        q.setRPY(roll, pitch, yaw);
        q.normalize();

        geometry_msgs::msg::PoseStamped target_pose;
        target_pose.header.frame_id = "base_link";
        target_pose.pose.position.x = x;
        target_pose.pose.position.y = y;
        target_pose.pose.position.z = z;
        target_pose.pose.orientation.x = q.getX();
        target_pose.pose.orientation.y = q.getY();
        target_pose.pose.orientation.z = q.getZ();
        target_pose.pose.orientation.w = q.getW();

        arm_->setStartStateToCurrentState();

        if (!cartesian_path)
        {
            // 普通关节空间规划
            arm_->setPoseTarget(target_pose);
            planAndExecute(arm_);
        }
        else
        {
            // 笛卡尔路径规划
            std::vector<geometry_msgs::msg::Pose> waypoints;
            waypoints.push_back(target_pose.pose);

            moveit_msgs::msg::RobotTrajectory trajectory;

            double fraction = arm_->computeCartesianPath(
                waypoints,   // 路点列表
                0.01,        // eef_step（越小越平滑）
                0.0,         // jump_threshold
                trajectory,  // 输出轨迹
                false        // 不避障，可保证路径成功
            );

            if (fraction >= 0.9)
            {
                arm_->execute(trajectory);
            }
            else
            {
                RCLCPP_ERROR(node_->get_logger(), "Cartesian path planning failed (%.2f)", fraction);
            }
        }
    }

    /**
     * @brief 打开手爪
     */
    void openGripper()
    {
        gripper_->setStartStateToCurrentState();
        gripper_->setNamedTarget("gripper_open");
        planAndExecute(gripper_);
    }

    /**
     * @brief 关闭手爪
     */
    void closeGripper()
    {
        gripper_->setStartStateToCurrentState();
        gripper_->setNamedTarget("gripper_closed");
        planAndExecute(gripper_);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("commander");

    Commander commander(node);

    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}

这里添加了一个 joint_command 主题订阅，用于控制机器人的每个关节角度：

可以使用下面的命令测试：

bash

ros2 topic pub -1 /joint_command example_interfaces/msg/Float64MultiArray "{data: [0.5,0.5,0.5,0.5,0.5,0.5]}"

我们还可以自定义接口实现更精细的控制：

这里的细节就不描述了，都是Ros2基本功

创建接口包：

cd ros2_ws/src/
ros2 pkg create my_robot_interfaces
cd my_robot_interfaces
mkdir msg

新建接口文件：

float64 x
float64 y
float64 z
float64 roll
float64 pitch
float64 yaw
bool cartesian_path

编译配置：

cmake_minimum_required(VERSION 3.8)
project(my_robot_interfaces)

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

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/PoseCommand.msg"
 )

ament_export_dependencies(rosidl_default_runtime)

ament_package()

最终代码如下：

https://github.com/DuRuofu/moveit2_learn/commit/8e0c21744dd3bddda38ef1f9c448c1e34c80bcf8

增加了自定义消息类型的 pose_command 接口，可以使用下面的命令测试：

ros2 topic pub -1 /pose_command my_robot_interfaces/msg/PoseCommand "{x: 0.7,y: 0.0,z: 0.4, roll: 3.14, pitch: 0.0,yaw: 0.0, cartesian_path: false}"

Moveit2 C++ API ​

一、介绍 ​

二、案例 ​

2.1 快速上手：姿态切换 ​

2.2 细节控制：指定关节角度 ​

2.3 目标姿态控制（重要） ​

2.4 笛卡尔路径规划 ​

三、编写实际的控制节点(应用) ​