四、使用MoveGroup C++接口——案例代码分解（八）

目录

0.前言

1. move_group.launch.py启动文件介绍

2. move_group_interface_tutorial启动文件介绍

3.move_group_interface主程序

4.任务分解并执行

4.1 规划到姿态目标

4.2 规划到关节空间目标

4.3 带路径约束的规划

4.4 笛卡尔路径规划

4.5 环境中的障碍物

4.6 将物体附加到机器人

4.7 从机器人上分离和移除物体

5.总结

 参考资料

---

0.前言

        在前述章节的基础下，解释本章move_group案例代码的各部分功能，并执行程序，显示机械臂规划运动结果。

1. move_group.launch.py启动文件介绍

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    # 创建MoveIt配置对象，指定了机器人描述文件和控制器配置文件
    moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")
        .robot_description(file_path="config/panda.urdf.xacro")  # 机器人模型描述文件
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")  # 运动控制器配置
        .to_moveit_configs()  # 生成MoveIt配置对象
    )
    
    # 定义启动move_group节点，负责处理MoveIt的动作服务
    run_move_group_node = Node(
        package="moveit_ros_move_group",  # 包名
        executable="move_group",  # 节点名
        output="screen",  # 输出到屏幕
        parameters=[moveit_config.to_dict()],  # 参数传递MoveIt配置
    )

    # 定义RViz节点，负责可视化
    rviz_config_file = (
        get_package_share_directory("moveit2_tutorials") + "/launch/move_group.rviz"  # RViz配置文件路径
    )
    rviz_node = Node(
        package="rviz2",  # 包名
        executable="rviz2",  # 节点名
        name="rviz2",  # 节点名称
        output="log",  # 输出日志
        arguments=["-d", rviz_config_file],  # 传递RViz配置文件
        parameters=[
            moveit_config.robot_description,  # 机器人描述
            moveit_config.robot_description_semantic,  # 语义化机器人描述
            moveit_config.robot_description_kinematics,  # 运动学参数
            moveit_config.planning_pipelines,  # 规划管道
            moveit_config.joint_limits,  # 关节限制
        ],
    )

    # 定义静态TF节点，用于发布静态坐标变换
    static_tf = Node(
        package="tf2_ros",  # 包名
        executable="static_transform_publisher",  # 节点名
        name="static_transform_publisher",  # 节点名称
        output="log",  # 输出日志
        arguments=["--frame-id", "world", "--child-frame-id", "panda_link0"],  # 参数设置坐标变换
    )

    # 定义机器人状态发布节点
    robot_state_publisher = Node(
        package="robot_state_publisher",  # 包名
        executable="robot_state_publisher",  # 节点名
        name="robot_state_publisher",  # 节点名称
        output="both",  # 同时输出到屏幕和日志
        parameters=[moveit_config.robot_description],  # 传递机器人描述
    )

    # 定义ros2_control节点，使用FakeSystem作为硬件接口
    ros2_controllers_path = os.path.join(
        get_package_share_directory("moveit_resources_panda_moveit_config"),
        "config",
        "ros2_controllers.yaml",  # 控制器配置文件路径
    )
    ros2_control_node = Node(
        package="controller_manager",  # 包名
        executable="ros2_control_node",  # 节点名
        parameters=[ros2_controllers_path],  # 传递控制器配置文件
        remappings=[
            ("/controller_manager/robot_description", "/robot_description"),  # 话题重映射
        ],
        output="both",  # 同时输出到屏幕和日志
    )

    # 启动控制器
    load_controllers = []
    for controller in [
        "panda_arm_controller",  # 控制器名称
        "panda_hand_controller",  # 控制器名称
        "joint_state_broadcaster",  # 控制器名称
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {}".format(controller)],  # 执行控制器启动命令
                shell=True,  # 使用shell运行命令
                output="screen",  # 输出到屏幕
            )
        ]

    # 返回LaunchDescription对象，包含所有定义的节点和控制器
    return LaunchDescription(
        [
            rviz_node,  # RViz可视化节点
            static_tf,  # 静态TF发布节点
            robot_state_publisher,  # 机器人状态发布节点
            run_move_group_node,  # move_group节点
            ros2_control_node,  # ros2_control节点
        ]
        + load_controllers  # 加载控制器
    )

        这段代码定义了一个 ROS 2 启动文件，包含了机器人系统的所有必要组件和配置，以支持机器人运动规划和控制。它设置了MoveIt2、可视化工具RViz、机器人状态发布器、静态坐标变换以及控制器管理器。通过这个启动文件，用户可以一次性启动所有相关节点，使机器人系统能够进行运动规划、控制和可视化。

2. move_group_interface_tutorial启动文件介绍

from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    # 创建MoveIt配置对象
    moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()
    # MoveItConfigsBuilder用于构建MoveIt的配置，这里使用了一个名为"moveit_resources_panda"的资源
    # 调用to_moveit_configs()方法来生成所需的MoveIt配置对象

    # 定义MoveGroupInterface示例节点
    move_group_demo = Node(
        name="move_group_interface_tutorial",  # 节点的名称
        package="moveit2_tutorials",  # 包名：包含MoveGroupInterface教程的ROS 2包
        executable="move_group_interface_tutorial",  # 可执行文件名：要运行的节点的名称
        output="screen",  # 输出类型：将日志输出到屏幕
        parameters=[
            moveit_config.robot_description,  # 机器人描述参数：提供机器人的URDF模型信息
            moveit_config.robot_description_semantic,  # 语义机器人描述参数：提供机器人的语义信息
            moveit_config.robot_description_kinematics,  # 运动学描述参数：提供机器人的运动学信息
        ],
    )

    # 返回启动描述，包含所有定义的节点
    return LaunchDescription([move_group_demo])
    # LaunchDescription对象用于描述要启动的所有节点
    # 在这个例子中，只包含了一个节点，即MoveGroupInterface节点

        用于执行move_group_interface程序。

3.move_group_interface主程序

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>

#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit_msgs/msg/collision_object.hpp>

#include <moveit_visual_tools/moveit_visual_tools.h>

// 所有使用ROS日志记录的源文件都应在文件顶部定义一个文件特定的
// 静态常量rclcpp::Logger,位于最小范围的命名空间中(如果有的话)
static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);

  // 我们为当前状态监视器启动一个单线程执行器,以获取有关机器人状态的信息
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(move_group_node);
  std::thread([&executor]() { executor.spin(); }).detach();

  // 开始教程
  //
  // 设置
  // ^^^^^
  //
  // MoveIt操作于名为“规划组”的关节集合,并将其存储在一个名为
  // ``JointModelGroup``的对象中。在MoveIt中,“规划组”和“关节模型组”
  // 这两个术语是可以互换使用的。
  static const std::string PLANNING_GROUP = "panda_arm";

  // 可以使用规划组的名称轻松设置
  // :moveit_codedir:`MoveGroupInterface<moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h>`
  // 类来控制和规划该组的动作。
  moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);

  // 我们将使用
  // :moveit_codedir:`PlanningSceneInterface<moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h>`
  // 类在我们的“虚拟世界”场景中添加和移除碰撞对象
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // 原始指针通常用于引用规划组以提高性能。
  const moveit::core::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // 可视化
  // ^^^^^^^^^^^^^
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "panda_link0", "move_group_tutorial",
                                                      move_group.getRobotModel());

  visual_tools.deleteAllMarkers();

  /* 远程控制是一个自省工具,允许用户通过按钮和键盘快捷键在RViz中逐步执行高级脚本 */
  visual_tools.loadRemoteControl();

  // RViz提供了许多类型的标记,在这个演示中,我们将使用文本、圆柱体和球体
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.0;
  visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE);

  // 批量发布用于减少发送到RViz的消息数量,以便进行大规模可视化
  visual_tools.trigger();

  // 获取基本信息
  // ^^^^^^^^^^^^^^^^^
  //
  // 我们可以打印机器人的参考框架名称。
  RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());

  // 我们还可以打印该组的末端执行器链路名称。
  RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // 我们可以获得机器人中的所有组的列表：
  RCLCPP_INFO(LOGGER, "Available Planning Groups:");
  std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
            std::ostream_iterator<std::string>(std::cout, ", "));

  // 开始演示
  // ^^^^^^^^^^^^^^^^^
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

  // .. _move_group_interface-planning-to-pose-goal:
  //
  // 规划到姿势目标
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // 我们可以为此组规划到末端执行器的期望姿势。
  geometry_msgs::msg::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.28;
  target_pose1.position.y = -0.2;
  target_pose1.position.z = 0.5;
  move_group.setPoseTarget(target_pose1);

  // 现在,我们调用规划器来计算计划并进行可视化。
  // 注意,我们只是规划,并没有要求move_group
  // 实际移动机器人。
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

  RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

  // 可视化计划
  // ^^^^^^^^^^^^^^^^^
  // 我们还可以将计划可视化为RViz中的一条线。
  RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line");
  visual_tools.publishAxisLabeled(target_pose1, "pose1");
  visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // 移动到姿势目标
  // ^^^^^^^^^^^^^^^^^^^^^
  //
  // 移动到姿势目标类似于上面的步骤
  // 除了现在我们使用``move()``函数。注意
  // 我们之前设置的姿势目标仍然是有效的
  // 因此,机器人将尝试移动到该目标。我们将
  // 在此教程中不使用该函数,因为它是
  // 阻塞函数,需要控制器处于活动状态
  // 并在执行轨迹时报告成功。

  /* 当使用实际机器人时取消注释下面的行 */
  /* move_group.move(); */

  // 规划到关节空间目标
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 让我们设置一个关节空间目标并朝着它移动。这将替换
  // 我们之前设置的姿势目标。
  //
  // 首先,我们将创建一个指针来引用当前机器人的状态。
  // RobotState是包含所有当前位置/速度/加速度数据的对象。
  moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10);
  //
  // 接下来获取该组的当前关节值集合。
  std::vector<double> joint_group_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  // 现在,让我们修改其中一个关节,规划到新的关节空间目标,并可视化计划。
  joint_group_positions[0] = -1.0;  // 弧度
  bool within_bounds = move_group.setJointValueTarget(joint_group_positions);
  if (!within_bounds)
  {
    RCLCPP_WARN(LOGGER, "Target joint position(s) were outside of limits, but we will plan and clamp to the limits ");
  }

  // 我们将允许的最大速度和加速度降低到最大值的5%。
  // 默认值是10%(0.1)。
  // 在机器人的moveit_config的joint_limits.yaml文件中设置您的首选默认值
  // 或在代码中设置显式因子,如果需要您的机器人移动更快。
  move_group.setMaxVelocityScalingFactor(0.05);
  move_group.setMaxAccelerationScalingFactor(0.05);

  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");

  // 在RViz中可视化计划：
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // 使用路径约束进行规划
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // 可以轻松地为机器人上的一个链路指定路径约束。
  // 首先定义路径约束。
  moveit_msgs::msg::OrientationConstraint ocm;
  ocm.link_name = "panda_link7";  // 约束应用于链路 "panda_link7"
  ocm.header.frame_id = "panda_link0";  // 参考坐标系为 "panda_link0"
  ocm.orientation.w = 1.0;  // 目标方向的四元数分量
  ocm.absolute_x_axis_tolerance = 0.1;  // x 轴方向的绝对容差
  ocm.absolute_y_axis_tolerance = 0.1;  // y 轴方向的绝对容差
  ocm.absolute_z_axis_tolerance = 0.1;  // z 轴方向的绝对容差
  ocm.weight = 1.0;  // 约束的权重

  // 现在,将其设置为组的路径约束。
  moveit_msgs::msg::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(test_constraints);

  // 强制在关节空间进行规划
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // 根据规划问题,MoveIt 在关节空间和笛卡尔空间之间进行选择。
  // 在 ompl_planning.yaml 文件中设置组参数 “enforce_joint_model_state_space:true”
  // 强制在所有计划中使用 “关节空间”。
  // 默认情况下,带有方向路径约束的规划请求会在“笛卡尔空间”中进行采样
  // 从而使逆运动学作为生成采样器。
  // 强制使用“关节空间”时,规划过程将使用拒绝采样来找到有效请求。
  // 请注意,这可能会显著增加规划时间。
  // 我们将重用之前的目标,并计划到它。
  // 注意,这只有在当前状态已经满足路径约束时才有效。
  // 因此,我们需要将起始状态设置为新姿势。
  moveit::core::RobotState start_state(*move_group.getCurrentState());
  geometry_msgs::msg::Pose start_pose2;
  start_pose2.orientation.w = 1.0;
  start_pose2.position.x = 0.55;
  start_pose2.position.y = -0.05;
  start_pose2.position.z = 0.8;
  start_state.setFromIK(joint_model_group, start_pose2);  // 使用逆运动学设置起始状态
  move_group.setStartState(start_state);

  // 现在,我们将从刚刚创建的新起始状态中规划到先前的姿势目标。
  move_group.setPoseTarget(target_pose1);

  // 带有约束的规划可能会很慢,因为每个样本都必须调用逆运动学求解器。
  // 让我们将规划时间从默认的 5 秒增加,以确保规划器有足够的时间来成功。
  move_group.setPlanningTime(10.0);

  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

  // 在 RViz 中可视化计划：
  visual_tools.deleteAllMarkers();
  visual_tools.publishAxisLabeled(start_pose2, "start");
  visual_tools.publishAxisLabeled(target_pose1, "goal");
  visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // 使用完路径约束后,请务必清除它。
  move_group.clearPathConstraints();

  // 笛卡尔路径
  // ^^^^^^^^^
  // 你可以通过指定一系列目标点来直接规划笛卡尔路径。
  // 请注意,我们从上述新的起始状态开始。
  // 初始姿势(起始状态)不需要添加到路径点列表中,但添加它可以帮助可视化。
  std::vector<geometry_msgs::msg::Pose> waypoints;
  waypoints.push_back(start_pose2);

  geometry_msgs::msg::Pose target_pose3 = start_pose2;

  target_pose3.position.z -= 0.2;
  waypoints.push_back(target_pose3);  // 向下

  target_pose3.position.y -= 0.2;
  waypoints.push_back(target_pose3);  // 向右

  target_pose3.position.z += 0.2;
  target_pose3.position.y += 0.2;
  target_pose3.position.x -= 0.2;
  waypoints.push_back(target_pose3);  // 向上和向左

  // 我们希望笛卡尔路径以 1 cm 的分辨率插值
  // 这就是为什么我们将笛卡尔平移的最大步长指定为 0.01。
  // 我们将跳跃阈值指定为 0.0,有效地禁用它。
  // 警告 - 禁用跳跃阈值可能会导致冗余关节的大幅度不可预测运动,这可能是一个安全问题
  moveit_msgs::msg::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;
  const double eef_step = 0.01;
  double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
  RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);

  // 在 RViz 中可视化计划
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
  for (std::size_t i = 0; i < waypoints.size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // 笛卡尔运动通常应该较慢,例如在接近物体时。
  // 笛卡尔计划的速度目前无法通过 maxVelocityScalingFactor 设置,而是需要手动调整轨迹时间,如 `这里 <https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4>`_ 中所述。
  // 欢迎提交拉取请求。
  // 
  // 你可以像这样执行轨迹。
  // /* move_group.execute(trajectory); */

  // 向环境中添加对象
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // 首先,让我们规划到另一个简单的目标,没有障碍物阻挡。
  move_group.setStartState(*move_group.getCurrentState());
  geometry_msgs::msg::Pose another_pose;
  another_pose.orientation.w = 0;
  another_pose.orientation.x = -1.0;
  another_pose.position.x = 0.7;
  another_pose.position.y = 0.0;
  another_pose.position.z = 0.59;
  move_group.setPoseTarget(another_pose);

  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");

  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishAxisLabeled(another_pose, "goal");
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // 结果可能如下所示：
  // 
  // .. image:: ./move_group_interface_tutorial_clear_path.gif
  //    :alt: 动画显示机器人臂相对直接地移动到目标位置
  // 
  // 现在,我们定义一个碰撞对象的 ROS 消息,以便机器人避开。
  moveit_msgs::msg::CollisionObject collision_object;
  collision_object.header.frame_id = move_group.getPlanningFrame();

  // 对象的 ID 用于识别它。
  collision_object.id = "box1";

  // 定义一个盒子以添加到世界中。
  shape_msgs::msg::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[primitive.BOX_X] = 0.1;
  primitive.dimensions[primitive.BOX_Y] = 1.5;
  primitive.dimensions[primitive.BOX_Z] = 0.5;

  // 为盒子定义一个姿势(相对于 frame_id)。
  geometry_msgs::msg::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 0.48;
  box_pose.position.y = 0.0;
  box_pose.position.z = 0.25;

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
  collision_objects.push_back(collision_object);

  // 现在,将碰撞对象添加到世界中
  // (使用一个可以包含其他对象的向量)
  RCLCPP_INFO(LOGGER, "Add an object into the world");
  planning_scene_interface.addCollisionObjects(collision_objects);

  // 在RViz中显示状态文本,并等待MoveGroup接收和处理碰撞物体消息
  visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();
  visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到碰撞物体出现在RViz中");

  // 现在,当我们规划轨迹时,它将避开障碍物。
  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "可视化计划 6(姿态目标绕立方体移动) %s", success ? "" : "失败");
  visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到计划完成");

  // 结果可能如下所示：
  //
  // .. image:: ./move_group_interface_tutorial_avoid_path.gif
  //    :alt: 显示手臂避开新障碍物的动画
  //
  // 将物体附加到机器人
  // ^^^^^^^^^^^^^^^^^^^^
  //
  // 您可以将一个物体附加到机器人,使其随机器人几何体一起移动。
  // 这模拟了拾取物体以便进行操作的过程。
  // 运动规划也应该避免物体之间的碰撞。
  moveit_msgs::msg::CollisionObject object_to_attach;
  object_to_attach.id = "cylinder1";

  shape_msgs::msg::SolidPrimitive cylinder_primitive;
  cylinder_primitive.type = primitive.CYLINDER;
  cylinder_primitive.dimensions.resize(2);
  cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20;
  cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;

  // 我们定义这个圆柱体的坐标系/姿态,使其出现在夹爪中。
  object_to_attach.header.frame_id = move_group.getEndEffectorLink();
  geometry_msgs::msg::Pose grab_pose;
  grab_pose.orientation.w = 1.0;
  grab_pose.position.z = 0.2;

  // 首先,我们将物体添加到世界中(不使用向量)。
  object_to_attach.primitives.push_back(cylinder_primitive);
  object_to_attach.primitive_poses.push_back(grab_pose);
  object_to_attach.operation = object_to_attach.ADD;
  planning_scene_interface.applyCollisionObject(object_to_attach);

  // 然后,我们将物体“附加”到机器人上。它使用 frame_id 来确定附加到哪个机器人连杆。
  // 我们还需要告诉MoveIt允许物体与夹爪的指尖连杆发生碰撞。
  // 您也可以使用 applyAttachedCollisionObject 直接将物体附加到机器人上。
  RCLCPP_INFO(LOGGER, "将物体附加到机器人上");
  std::vector<std::string> touch_links;
  touch_links.push_back("panda_rightfinger");
  touch_links.push_back("panda_leftfinger");
  move_group.attachObject(object_to_attach.id, "panda_hand", touch_links);

  visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* 等待MoveGroup接收和处理附加碰撞物体消息 */
  visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到新物体附加到机器人上");

  // 重新规划,但现在手中有物体。
  move_group.setStartStateToCurrentState();
  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "可视化计划 7(与圆柱体一起移动绕立方体) %s", success ? "" : "失败");
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到计划完成");

  // 结果可能如下所示：
  //
  // .. image:: ./move_group_interface_tutorial_attached_object.gif
  //    :alt: 显示物体附加后手臂运动的不同动画
  //
  // 分离和移除物体
  // ^^^^^^^^^^^^^^^
  //
  // 现在,让我们将圆柱体从机器人的夹爪中分离。
  RCLCPP_INFO(LOGGER, "将物体从机器人上分离");
  move_group.detachObject(object_to_attach.id);

  // 在RViz中显示状态文本
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* 等待MoveGroup接收和处理已分离的碰撞物体消息 */
  visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到新物体从机器人上分离");

  // 现在,让我们从世界中移除物体。
  RCLCPP_INFO(LOGGER, "从世界中移除物体");
  std::vector<std::string> object_ids;
  object_ids.push_back(collision_object.id);
  object_ids.push_back(object_to_attach.id);
  planning_scene_interface.removeCollisionObjects(object_ids);

  // 在RViz中显示状态文本
  visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* 等待MoveGroup接收和处理已移除的碰撞物体消息 */
  visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到碰撞物体消失");

  // 教程代码结束
  visual_tools.deleteAllMarkers();
  visual_tools.trigger();

  rclcpp::shutdown();
  return 0;
}

4.任务分解并执行

4.1 规划到姿态目标

• 使用 move_group.setPoseTarget(target_pose1) 设置目标姿态。
• 调用 move_group.plan(my_plan) 计算规划。
• 通过 RViz 进行可视化。

4.2 规划到关节空间目标

• 获取当前的机器人状态并修改关节值。
• 使用 move_group.setJointValueTarget(joint_group_positions) 设置关节目标。
• 调整最大速度和加速度。
• 进行规划并在 RViz 中可视化。

4.3 带路径约束的规划

• 定义并设置路径约束（如方向约束）。
• 修改起始状态以满足路径约束。
• 设置新的目标姿态并规划。
• 通过 RViz 进行可视化。

4.4 笛卡尔路径规划

• 指定一系列路径点。
• 使用 move_group.computeCartesianPath() 计算笛卡尔路径。
• 在 RViz 中可视化路径。

4.5 环境中的障碍物

• 先进行没有障碍物的规划。
• 定义并添加碰撞对象到环境中。
• 再次进行规划，并确保机器人避开障碍物。

4.6 将物体附加到机器人

• 定义一个要附加到机器人的物体，并将其添加到世界中。
• 使用 move_group.attachObject() 将物体附加到机器人上。
• 进行新的规划并在 RViz 中可视化。

4.7 从机器人上分离和移除物体

• 使用 move_group.detachObject() 从机器人上分离物体。
• 使用 planning_scene_interface.removeCollisionObjects() 从环境中移除物体。
• 在 RViz 中显示更新状态。

5.总结

        本文演示了如何使用MoveIt2在ROS 2中进行机器人运动规划。这包括了如何启动必要的节点，如何设置MoveGroupInterface进行运动规划，以及如何在主程序中实现不同的运动规划策略。

 参考资料

MoveIt2 官方教程