📚 课程目录第4章编写C++节点实现笛卡尔空间规划

4. 编写C++节点实现笛卡尔空间规划

任务目标

编写一个ROS2 C++节点，通过MoveIt2的C++ API为机械臂末端执行器规划一条笛卡尔空间路径。节点将接收一系列目标位姿（位置+姿态），调用computeCartesianPath接口生成路径，并验证路径规划成功率。

环境依赖

ROS2 Humble或更高版本
MoveIt2（ros-humble-moveit）
已配置的机械臂URDF/SRDF（本案例以panda机械臂为例）
MoveIt2自带的move_group启动环境

核心实现

以下节点实现笛卡尔空间规划：定义一系列末端位姿（含位置与姿态），调用computeCartesianPath计算路径，并输出规划结果与路径点数量。

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/msg/pose.hpp>
#include <vector>

class CartesianPlanner : public rclcpp::Node
{
public:
    CartesianPlanner() : Node("cartesian_planner_node")
    {
        // 使用async方式初始化MoveGroupInterface
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
            shared_from_this(), "panda_arm");
        plan_cartesian_path();
    }

private:
    void plan_cartesian_path()
    {
        // 获取当前末端执行器位姿作为起点
        geometry_msgs::msg::Pose start_pose = move_group_->getCurrentPose().pose;

        // 定义笛卡尔路径的多个路点（waypoints）
        std::vector<geometry_msgs::msg::Pose> waypoints;

        // 路点1：沿X轴平移0.1米
        geometry_msgs::msg::Pose wp1 = start_pose;
        wp1.position.x += 0.1;
        waypoints.push_back(wp1);

        // 路点2：沿Y轴平移0.1米
        geometry_msgs::msg::Pose wp2 = wp1;
        wp2.position.y += 0.1;
        waypoints.push_back(wp2);

        // 路点3：沿Z轴下降0.05米
        geometry_msgs::msg::Pose wp3 = wp2;
        wp3.position.z -= 0.05;
        waypoints.push_back(wp3);

        // 设置规划参数
        double eef_step = 0.01;    // 末端步长（米）
        double jump_threshold = 0.0; // 禁用跳跃阈值检查

        // 执行笛卡尔路径规划
        moveit_msgs::msg::RobotTrajectory trajectory;
        double fraction = move_group_->computeCartesianPath(
            waypoints, eef_step, jump_threshold, trajectory);

        // 输出规划结果
        RCLCPP_INFO(this->get_logger(), "笛卡尔路径规划完成率: %.2f%%", fraction * 100.0);

        if (fraction >= 0.9)
        {
            RCLCPP_INFO(this->get_logger(), "规划成功，路径点数量: %zu", 
                        trajectory.joint_trajectory.points.size());
            
            // 可选：执行规划（需在真实环境或仿真中）
            // move_group_->execute(trajectory);
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "规划失败，无法完成完整路径");
        }
    }

    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
};

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

编译与运行

将上述代码保存为cartesian_planner.cpp，放入你的ROS2包（如src/cartesian_demo/src/）
在CMakeLists.txt中添加依赖：

find_package(moveit_ros_planning_interface REQUIRED)
find_package(geometry_msgs REQUIRED)

add_executable(cartesian_planner src/cartesian_planner.cpp)
target_link_libraries(cartesian_planner ${moveit_ros_planning_interface_LIBRARIES})
ament_target_dependencies(cartesian_planner rclcpp geometry_msgs)

编译并source环境：

colcon build --packages-select cartesian_demo
source install/setup.bash

启动MoveIt2环境（以panda为例）：

ros2 launch moveit_resources_panda_moveit_config demo.launch.py

另开终端运行节点：

ros2 run cartesian_demo cartesian_planner

验证方法

预期输出：终端显示“笛卡尔路径规划完成率: 100.00%”和“规划成功，路径点数量: N”（N为实际路径点数量，通常大于3）
可视化验证：在RViz中观察机械臂末端是否沿直线路径依次经过三个路点位置
通过条件：规划完成率≥90%且无错误日志；若完成率低于90%，检查机械臂是否处于奇异位形或路点是否超出工作空间

关键说明

eef_step控制路径插值密度，值越小路径越平滑但计算量增大
jump_threshold设为0表示不检查关节跳跃，适用于连续路径
若需实际执行轨迹，取消注释move_group_->execute(trajectory)，但需确保机械臂处于安全环境
本节点使用shared_from_this()要求节点以std::make_shared方式创建，已满足