【Moveit2】move_group_interface_tutorial中文注释

move_group_interface_tutorial

#include <moveit/move_group_interface/move_group_interface.h> // 包含MoveIt的移动组接口
#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> // 包含MoveIt可视化工具

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

int main(int argc, char** argv) // 主函数
{
  rclcpp::init(argc, argv); // 初始化ROS
  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); // 创建MoveGroupInterface实例

  // 我们将使用
  // :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(); // 删除所有标记

  /* 远程控制是一种可以让用户通过按钮和键盘快捷键逐步执行高层脚本的工具 */
  visual_tools.loadRemoteControl(); // 加载远程控制

  // RViz提供了许多类型的标记，在本演示中我们将使用文本、圆柱体和球体
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); // 创建文本位置的单位变换
  text_pose.translation().z() = 1.0; // 设置文本的Z轴位置
  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; // 设置目标姿态的X轴位置
  target_pose1.position.y = -0.2; // 设置目标姿态的Y轴位置
  target_pose1.position.z = 0.5; // 设置目标姿态的Z轴位置
  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()``函数。 注意
  // 我们之前设置的姿态目标仍然有效
  // 因此机器人会尝试移动到该目标。我们将
  // 在本教程中不使用该函数，因为它是
  // 阻塞函数，并且需要控制器处于活动状态
  // 并报告轨迹执行成功。

  /* Uncomment below line when working with a real robot */
  /* 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; // 将第一个关节设定为 -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); // 设置最大速度缩放因子为 5%
  move_group.setMaxAccelerationScalingFactor(0.05); // 设置最大加速度缩放因子为 5%

  // 规划并执行计划
  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; // 约束的权重为1（完全施加）

  // 现在将其设置为该组的路径约束。
  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`
  // 强制所有计划使用关节空间。
  //
  // 默认情况下，带有方向路径约束的规划请求是在笛卡尔空间中采样的，以便调用逆运动学（IK）作为生成采样器。
  //
  // 强制使用关节空间时，规划过程将使用拒绝采样找到有效请求。
  // 请注意，这可能会显著增加规划时间。
  //
  // 我们将重用之前的目标，并为其进行规划。
  // 请注意，这仅在当前状态已经满足路径约束的情况下才会起作用。
  // 所以我们需要将初始状态设置为新的姿态。
  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; // 设置姿态位置 X 轴
  start_pose2.position.y = -0.05; // 设置姿态位置 Y 轴
  start_pose2.position.z = 0.8; // 设置姿态位置 Z 轴
  start_state.setFromIK(joint_model_group, start_pose2); // 根据逆运动学将起始姿态设置为当前状态
  move_group.setStartState(start_state); // 将机器人起始状态设置为刚定义的状态

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

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

  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(); // 清除路径约束

使用航点规划笛卡尔路径

• 适合于末端执行器需要沿特定轨迹移动的场景

  // 笛卡尔路径
  // ^^^^^^^^^^^^^^^^^^^
  // 您可以直接通过指定一系列航点（waypoints）为末端执行器规划笛卡尔路径。
  // 请注意，我们是从上面定义的新起始状态开始的。起始姿态（start state）不需要添加到航点列表中，但添加它可以帮助进行可视化。

  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; // 调整目标位置的Z轴，使其向下移动0.2米
  waypoints.push_back(target_pose3); // 将这个新姿态作为第二个航点

  target_pose3.position.y -= 0.2; // 调整目标位置的Y轴，使其向右移动0.2米
  waypoints.push_back(target_pose3); // 将调整后的姿态作为第三个航点

  target_pose3.position.z += 0.2; // 调整目标位置的Z轴，使其向上移动0.2米
  target_pose3.position.y += 0.2; // 调整目标位置的Y轴，使其向左移动0.2米
  target_pose3.position.x -= 0.2; // 调整目标位置的X轴，使其向后移动0.2米
  waypoints.push_back(target_pose3); // 将这个新的姿态作为第四个航点

  // 我们希望笛卡尔路径以1厘米的分辨率进行插值，因此我们将0.01指定为笛卡尔平移中的最大步长。
  // 我们将跳跃阈值指定为0.0，有效地禁用了它。
  // 警告：在操作真实硬件时禁用跳跃阈值可能会导致冗余关节的不规则运动，可能带来安全问题。
  moveit_msgs::msg::RobotTrajectory trajectory; // 创建轨迹对象
  const double jump_threshold = 0.0; // 设置跳跃阈值为0（禁用）
  const double eef_step = 0.01; // 设置末端执行器的步长为1厘米
  double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); 
  // 计算笛卡尔路径并获取完成路径的比例（fraction表示路径的完成度）

  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); // 在RViz中发布航点路径，使用绿色标记
  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 设置，
  // 需要手动调整路径的时间。
  // 您可以使用如下方式执行轨迹。
  /* move_group.execute(trajectory); */ // 通过调用execute来执行轨迹（这里注释掉，需要时可启用）

添加物体到环境中

• 在虚拟环境中添加障碍物，并规划机器人绕过障碍物的路径

  // 将物体添加到环境中
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 首先，我们计划一个没有障碍物的简单目标。
  move_group.setStartState(*move_group.getCurrentState()); 
  // 设置当前的机器人状态为起始状态

  geometry_msgs::msg::Pose another_pose; // 创建一个新的姿态对象
  another_pose.orientation.w = 1.0; // 设置方向四元数，表示没有旋转
  another_pose.position.x = 0.7; // 设定目标姿态的X坐标
  another_pose.position.y = 0.0; // 设定目标姿态的Y坐标
  another_pose.position.z = 0.59; // 设定目标姿态的Z坐标
  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); // 在RViz中发布“清晰目标”文本
  visual_tools.publishAxisLabeled(another_pose, "goal"); // 为目标位置发布带标签的坐标轴
  visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); // 在RViz中发布轨迹线
  visual_tools.trigger(); // 触发可视化工具
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); 
  // 提示用户继续演示

  // 现在，定义一个碰撞物体的 ROS 消息，以便让机器人避免它。
  moveit_msgs::msg::CollisionObject collision_object; // 创建碰撞物体消息
  collision_object.header.frame_id = move_group.getPlanningFrame(); // 设置物体的参考坐标系为机器人规划框架

  // 物体的id用于识别它。
  collision_object.id = "box1"; // 设置碰撞物体的ID为 "box1"

  // 定义一个要添加到世界中的盒子。
  shape_msgs::msg::SolidPrimitive primitive; // 创建实心物体的原始形状
  primitive.type = primitive.BOX; // 设置物体类型为盒子
  primitive.dimensions.resize(3); // 设置盒子的三个维度
  primitive.dimensions[primitive.BOX_X] = 0.1; // X方向长度为0.1米
  primitive.dimensions[primitive.BOX_Y] = 1.5; // Y方向长度为1.5米
  primitive.dimensions[primitive.BOX_Z] = 0.5; // Z方向高度为0.5米

  // 定义盒子的姿态（相对于frame_id指定的坐标系）。
  geometry_msgs::msg::Pose box_pose; // 创建盒子的位置和方向
  box_pose.orientation.w = 1.0; // 设置方向为单位四元数
  box_pose.position.x = 0.48; // 设置盒子的X坐标
  box_pose.position.y = 0.0; // 设置盒子的Y坐标
  box_pose.position.z = 0.25; // 设置盒子的Z坐标

  // 将物体形状和位置信息添加到碰撞物体中
  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); 
  // 在RViz中显示“添加物体”文本
  visual_tools.trigger(); // 触发可视化工具
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); 
  // 提示用户等待，直到碰撞物体出现在RViz中

  // 现在，当我们规划轨迹时，它会避开障碍物。
  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); 
  RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); 
  // 规划新的路径，并检查机器人是否成功避开障碍物

  visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE); 
  // 在RViz中发布文本“障碍物目标”
  visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); 
  // 在RViz中发布新的轨迹线
  visual_tools.trigger(); // 触发可视化工具
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); 
  // 提示用户继续演示

  // 结果可能如下所示：
  //
  // .. 图像:: ./move_group_interface_tutorial_avoid_path.gif
  //    :alt: 显示机械臂避开新障碍物的动画
  //

将物体附加到机器人上

• 将物体附加到机器人上，并对机器人进行路径规划，同时考虑附加物体的碰撞检测和避障需求

  
  // 将物体附加到机器人上
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 您可以将物体附加到机器人上，使其与机器人的几何结构一起移动。
  // 这模拟了拾取物体以进行操作的过程。
  // 运动规划应该避免物体之间的碰撞。
  
  moveit_msgs::msg::CollisionObject object_to_attach; 
  // 创建一个碰撞物体对象，用于表示要附加的物体
  object_to_attach.id = "cylinder1"; 
  // 为附加的物体指定一个唯一的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; 
  // 设置圆柱体的高度为0.2米
  cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04; 
  // 设置圆柱体的半径为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; 
  // 设置物体的位置，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, "Attach the object to the robot"); 
  // 打印日志，表示物体即将附加到机器人上

  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); 
  // 将物体附加到机器人上，附加到“panda_hand”（机械手）上，并允许物体与两个指尖碰撞

  visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE); 
  // 在RViz中发布文本，提示“物体附加到机器人上”
  visual_tools.trigger(); 
  // 触发可视化工具，显示状态

  /* 等待 MoveGroup 接收并处理附加的碰撞物体消息 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); 
  // 提示用户在RViz中按下“next”按钮，直到物体成功附加到机器人

  // 重新规划，但这次物体附加在机器人上。
  move_group.setStartStateToCurrentState(); 
  // 将机器人的当前状态设置为起始状态
  success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); 
  // 重新规划路径，并检查是否成功
  RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); 
  // 输出日志，表示是否成功规划路径（机器人附带物体的情况下）

  visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); 
  // 在RViz中发布新的轨迹线，展示机器人携带物体时的运动轨迹
  visual_tools.trigger(); 
  // 触发可视化工具
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); 
  // 提示用户继续演示，直到路径规划完成
  
  // 结果可能如下所示：
  //
  // .. 图像:: ./move_group_interface_tutorial_attached_object.gif
  //    :alt: 显示附加物体后，机械臂运动不同的动画
  //

分离和移除物体

• 模拟机器人操作完成后放下物体并将其移出操作区域的场景

  
  // 分离和移除物体
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 现在，让我们从机器人的机械手中分离圆柱体。
  RCLCPP_INFO(LOGGER, "Detach the object from the robot"); 
  // 打印日志，表示我们正在从机器人上分离物体
  move_group.detachObject(object_to_attach.id); 
  // 调用 `detachObject` 函数，将之前附加的物体与机器人分离

  // 在RViz中显示状态
  visual_tools.deleteAllMarkers(); 
  // 删除所有RViz中的标记
  visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE); 
  // 发布文本 "物体已从机器人上分离" 
  visual_tools.trigger(); 
  // 触发可视化工具，显示状态更新

  /* 等待 MoveGroup 接收并处理已分离的碰撞物体消息 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); 
  // 提示用户等待，直到物体成功从机器人上分离

  // 现在，让我们从世界中移除物体。
  RCLCPP_INFO(LOGGER, "Remove the objects from the world"); 
  // 打印日志，表示我们即将从虚拟环境中移除物体
  std::vector<std::string> object_ids; 
  // 创建一个字符串向量，用来存储要移除的物体ID
  object_ids.push_back(collision_object.id); 
  // 将之前定义的碰撞物体ID "box1" 添加到待移除的物体ID列表中
  object_ids.push_back(object_to_attach.id); 
  // 将附加物体ID "cylinder1" 添加到待移除的物体ID列表中
  planning_scene_interface.removeCollisionObjects(object_ids); 
  // 调用 `removeCollisionObjects` 函数，从世界中移除这些物体

  // 在RViz中显示状态
  visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE); 
  // 发布文本 "物体已移除"
  visual_tools.trigger(); 
  // 触发可视化工具，显示状态更新

  /* 等待 MoveGroup 接收并处理已移除的碰撞物体消息 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disappears"); 
  // 提示用户等待，直到碰撞物体从RViz中消失

  // END_TUTORIAL
  visual_tools.deleteAllMarkers(); 
  // 删除所有RViz中的标记
  visual_tools.trigger(); 
  // 触发可视化工具，完成教程

  rclcpp::shutdown(); 
  // 关闭ROS节点
  return 0; 
  // 程序结束