Moveit2 Move Group C++ 接口

系列文章目录

留空

文章目录

系列文章目录
前言
一、完整代码
二、编写步骤
三、代码分析
- 1. 引入必要的头文件
- 2. 初始化和配置 ROS2 环境
- 3. 设置 MoveIt 规划组和场景
- 4. 可视化
- 5. 获取基本信息
- 6. 开始演示
- 7. 规划姿态目标
- 8. 可视化计划路径
- 9. 移动到姿势目标
- 10. 规划关节空间目标
- 11. 使用路径约束进行规划
- 12. 在关节空间中强制规划
- 13. 笛卡尔路径
- 14. 将对象添加到环境中
- 15. 将对象附加到机器人
- 16. 分离和删除对象
四、补充
- 1. 规划组（Planning Group）
- 2. 关节模型组
总结

前言

自用

一、完整代码

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, SRI International
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of SRI International nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */

#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>

// All source files that use ROS logging should define a file-specific
// static const rclcpp::Logger named LOGGER, located at the top of the file
// and inside the namespace with the narrowest scope (if there is one)
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);

  // We spin up a SingleThreadedExecutor for the current state monitor to get information
  // about the robot's state.
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(move_group_node);
  std::thread([&executor]() { executor.spin(); }).detach();

  // BEGIN_TUTORIAL
  //
  // Setup
  // ^^^^^
  //
  // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
  // the ``JointModelGroup``. Throughout MoveIt, the terms "planning group" and "joint model group"
  // are used interchangeably.
  static const std::string PLANNING_GROUP = "panda_arm";

  // The
  // :moveit_codedir:`MoveGroupInterface<moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h>`
  // class can be easily set up using just the name of the planning group you would like to control and plan for.
  moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);

  // We will use the
  // :moveit_codedir:`PlanningSceneInterface<moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h>`
  // class to add and remove collision objects in our "virtual world" scene
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // Raw pointers are frequently used to refer to the planning group for improved performance.
  const moveit::core::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // Visualization
  // ^^^^^^^^^^^^^
  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();

  /* Remote control is an introspection tool that allows users to step through a high level script */
  /* via buttons and keyboard shortcuts in RViz */
  visual_tools.loadRemoteControl();

  // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.0;
  visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE);

  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
  visual_tools.trigger();

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // We can print the name of the reference frame for this robot.
  RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // We can get a list of all the groups in the robot:
  RCLCPP_INFO(LOGGER, "Available Planning Groups:");
  std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
            std::ostream_iterator<std::string>(std::cout, ", "));

  // Start the demo
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

  // .. _move_group_interface-planning-to-pose-goal:
  //
  // Planning to a Pose goal
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // We can plan a motion for this group to a desired pose for the
  // end-effector.
  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);

  // Now, we call the planner to compute the plan and visualize it.
  // Note that we are just planning, not asking move_group
  // to actually move the robot.
  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");

  // Visualizing plans
  // ^^^^^^^^^^^^^^^^^
  // We can also visualize the plan as a line with markers in 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");

  // Moving to a pose goal
  // ^^^^^^^^^^^^^^^^^^^^^
  //
  // Moving to a pose goal is similar to the step above
  // except we now use the ``move()`` function. Note that
  // the pose goal we had set earlier is still active
  // and so the robot will try to move to that goal. We will
  // not use that function in this tutorial since it is
  // a blocking function and requires a controller to be active
  // and report success on execution of a trajectory.

  /* Uncomment below line when working with a real robot */
  /* move_group.move(); */

  // Planning to a joint-space goal
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // Let's set a joint space goal and move towards it.  This will replace the
  // pose target we set above.
  //
  // To start, we'll create an pointer that references the current robot's state.
  // RobotState is the object that contains all the current position/velocity/acceleration data.
  moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10);
  //
  // Next get the current set of joint values for the group.
  std::vector<double> joint_group_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  // Now, let's modify one of the joints, plan to the new joint space goal, and visualize the plan.
  joint_group_positions[0] = -1.0;  // radians
  move_group.setJointValueTarget(joint_group_positions);

  // We lower the allowed maximum velocity and acceleration to 5% of their maximum.
  // The default values are 10% (0.1).
  // Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config
  // or set explicit factors in your code if you need your robot to move faster.
  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");

  // Visualize the plan in 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");

  // Planning with Path Constraints
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // Path constraints can easily be specified for a link on the robot.
  // Let's specify a path constraint and a pose goal for our group.
  // First define the path constraint.
  moveit_msgs::msg::OrientationConstraint ocm;
  ocm.link_name = "panda_link7";
  ocm.header.frame_id = "panda_link0";
  ocm.orientation.w = 1.0;
  ocm.absolute_x_axis_tolerance = 0.1;
  ocm.absolute_y_axis_tolerance = 0.1;
  ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 1.0;

  // Now, set it as the path constraint for the group.
  moveit_msgs::msg::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(test_constraints);

  // Enforce Planning in Joint Space
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // Depending on the planning problem MoveIt chooses between
  // ``joint space`` and ``cartesian space`` for problem representation.
  // Setting the group parameter ``enforce_joint_model_state_space:true`` in
  // the ompl_planning.yaml file enforces the use of ``joint space`` for all plans.
  //
  // By default, planning requests with orientation path constraints
  // are sampled in ``cartesian space`` so that invoking IK serves as a
  // generative sampler.
  //
  // By enforcing ``joint space``, the planning process will use rejection
  // sampling to find valid requests. Please note that this might
  // increase planning time considerably.
  //
  // We will reuse the old goal that we had and plan to it.
  // Note that this will only work if the current state already
  // satisfies the path constraints. So we need to set the start
  // state to a new pose.
  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);

  // Now, we will plan to the earlier pose target from the new
  // start state that we just created.
  move_group.setPoseTarget(target_pose1);

  // Planning with constraints can be slow because every sample must call an inverse kinematics solver.
  // Let's increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
  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");

  // Visualize the plan in 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");

  // When done with the path constraint, be sure to clear it.
  move_group.clearPathConstraints();

  // Cartesian Paths
  // ^^^^^^^^^^^^^^^
  // You can plan a Cartesian path directly by specifying a list of waypoints
  // for the end-effector to go through. Note that we are starting
  // from the new start state above.  The initial pose (start state) does not
  // need to be added to the waypoint list but adding it can help with visualizations
  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);  // down

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

  target_pose3.position.z += 0.2;
  target_pose3.position.y += 0.2;
  target_pose3.position.x -= 0.2;
  waypoints.push_back(target_pose3);  // up and left

  // We want the Cartesian path to be interpolated at a resolution of 1 cm
  // which is why we will specify 0.01 as the max step in Cartesian
  // translation.  We will specify the jump threshold as 0.0, effectively disabling it.
  // Warning - disabling the jump threshold while operating real hardware can cause
  // large unpredictable motions of redundant joints and could be a safety issue
  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);

  // Visualize the plan in 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");

  // Cartesian motions should often be slow, e.g. when approaching objects. The speed of Cartesian
  // plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time
  // the trajectory manually, as described `here <https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4>`_.
  // Pull requests are welcome.
  //
  // You can execute a trajectory like this.
  /* move_group.execute(trajectory); */

  // Adding objects to the environment
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // First, let's plan to another simple goal with no objects in the way.
  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");

  // The result may look like this:
  //
  // .. image:: ./move_group_interface_tutorial_clear_path.gif
  //    :alt: animation showing the arm moving relatively straight toward the goal
  //
  // Now, let's define a collision object ROS message for the robot to avoid.
  moveit_msgs::msg::CollisionObject collision_object;
  collision_object.header.frame_id = move_group.getPlanningFrame();

  // The id of the object is used to identify it.
  collision_object.id = "box1";

  // Define a box to add to the world.
  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;

  // Define a pose for the box (specified relative to 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);

  // Now, let's add the collision object into the world
  // (using a vector that could contain additional objects)
  RCLCPP_INFO(LOGGER, "Add an object into the world");
  planning_scene_interface.addCollisionObjects(collision_objects);

  // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message
  visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

  // Now, when we plan a trajectory it will avoid the obstacle.
  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);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");

  // The result may look like this:
  //
  // .. image:: ./move_group_interface_tutorial_avoid_path.gif
  //    :alt: animation showing the arm moving avoiding the new obstacle
  //
  // Attaching objects to the robot
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // You can attach an object to the robot, so that it moves with the robot geometry.
  // This simulates picking up the object for the purpose of manipulating it.
  // The motion planning should avoid collisions between objects as well.
  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;

  // We define the frame/pose for this cylinder so that it appears in the gripper.
  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;

  // First, we add the object to the world (without using a vector).
  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);

  // Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to.
  // We also need to tell MoveIt that the object is allowed to be in collision with the finger links of the gripper.
  // You could also use applyAttachedCollisionObject to attach an object to the robot directly.
  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);

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

  /* Wait for MoveGroup to receive and process the attached collision object message */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot");

  // Replan, but now with the object in hand.
  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);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");

  // The result may look something like this:
  //
  // .. image:: ./move_group_interface_tutorial_attached_object.gif
  //    :alt: animation showing the arm moving differently once the object is attached
  //
  // Detaching and Removing Objects
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // Now, let's detach the cylinder from the robot's gripper.
  RCLCPP_INFO(LOGGER, "Detach the object from the robot");
  move_group.detachObject(object_to_attach.id);

  // Show text in RViz of status
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for MoveGroup to receive and process the attached collision object message */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot");

  // Now, let's remove the objects from the world.
  RCLCPP_INFO(LOGGER, "Remove the objects from the world");
  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);

  // Show text in RViz of status
  visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for MoveGroup to receive and process the attached collision object message */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disappears");

  // END_TUTORIAL
  visual_tools.deleteAllMarkers();
  visual_tools.trigger();

  rclcpp::shutdown();
  return 0;
}

编译
两个终端

ros2 launch moveit2_tutorials move_group.launch.py

ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py

片刻之后，RViz 窗口应出现，其外观与此页面顶部的窗口类似。
在这里插入图片描述
要完成每个演示步骤，请按屏幕底部 RvizVisualToolsGui 面板中的“下一步”按钮，或在屏幕顶部的“工具”面板中选择“键工具”，然后在 RViz 聚焦时按键盘上的 0。

在这里插入图片描述
后面不放图了。。

二、编写步骤

1.引入必要的头文件
2.初始化和配置 ROS2 环境
3.设置 Movelt 规划组和场景
4.可视化
5.获取基本信息
6.开始演示
7.规划姿态目标
8.可视化计划路径
9.移动到姿势目标
10.规划关节空间目标
11.使用路径约束进行规划
12.在关节空间中强制规划
13.笛卡尔路径
14.将对象添加到环境中
15.将对象附加到机器人
16.分离和删除对象

三、代码分析

1. 引入必要的头文件

#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>

这些头文件是 MoveIt! 机器人运动规划框架的核心组件，它们提供了一系列的功能，使得开发者可以在 ROS 环境中实现高效、灵活的机器人控制和操作。以下是每个头文件的主要用途以及它们如何支持机器人应用的开发的详细总结：

核心接口

moveit/move_group_interface/move_group_interface.h

这个头文件中定义的 MoveGroupInterface 类是 MoveIt! 最核心的类之一。它为开发者提供了一个高级的接口来进行机器人的运动规划和执行。通过这个接口，你可以：
- 设定机器人的目标位置或姿态。
- 执行运动规划算法来找到从当前状态到目标状态的路径。
- 执行计算出的路径。
- 配置机器人的运动参数，如速度和加速度。

环境管理

moveit/planning_scene_interface/planning_scene_interface.h

PlanningSceneInterface 类提供了一个接口来管理和更新机器人周围的环境。这对于安全地规划机器人的路径至关重要，尤其是在复杂或动态变化的环境中。主要功能包括：
- 向场景中添加、移除或更新碰撞对象。
- 管理场景中的物体，例如定义哪些物体是机器人可以互动的。
- 查询和修改环境的状态。

可视化和调试

moveit_msgs/msg/display_robot_state.hpp

这个头文件包含的消息类型用于在 ROS 系统中发布机器人的当前状态信息。这样的信息通常用于：
- 在 RViz 或其他可视化工具中显示机器人的实时状态。
- 调试和监控机器人的运行状态。
- 作为其他 ROS 节点的输入，用于决策支持或进一步处理。
- 包含消息类型，用于在 ROS 系统中发布机器人的当前状态信息。
- 发布的状态信息通常用于在 RViz 或其他可视化工具中显示，用于调试和监控机器人的运行状态。
moveit_msgs/msg/display_trajectory.hpp

此消息类型用于发布机器人的规划轨迹，允许开发者和最终用户：
- 在 RViz 中可视化预计的运动路径。
- 检查和验证路径是否符合预期，确保安全性和有效性。
- 进行调试，优化运动规划结果。

碰撞管理

moveit_msgs/msg/attached_collision_object.hpp 和 moveit_msgs/msg/collision_object.hpp

这两个头文件定义的消息类型用于处理与机器人互动的物体（碰撞对象）。它们允许：
- 定义、更新或删除机器人操作环境中的物体。
- 管理机器人与这些物体的交互，如抓取或搬运。
- 确保机器人在运动时不会与这些物体发生碰撞，提高操作的安全性。

交互式工具

moveit_visual_tools/moveit_visual_tools.h

MoveItVisualTools 类提供了一系列工具用于在 RViz 中进行各种运动规划相关的可视化，包括：
- 显示文本、控制标记和轨迹。
- 交互式工具，如按钮和菜单，用于控制和调整机器人的运动。
- 辅助开发者和用户理解机器人的运动规划和执行情况。

这些组件共同构成了 MoveIt! 的强大功能，使其成为一个全面的解决方案，用于机器人的运动规划、操作和环境交互。MoveIt! 提供了必要的工具和接口，支持从基本的机器人运动控制到复杂的交互和环境管理，非常适合在工业自动化、研究或服务机器人领域中使用。无论是在简单的应用还是复杂的多机器人系统中，MoveIt! 都能提供强大的支持。

2. 初始化和配置 ROS2 环境

初始化 ROS 2:
```
rclcpp::init(argc, argv);
```
- 这一步是所有 ROS 2 程序的必要步骤，用于设置ROS客户端库的初始化。
- argc 和 argv 是传统C++程序的标准参数，包含了启动程序时从命令行传入的参数。在 ROS 2 中，这些参数可以用来设置节点的命名空间、节点名称或其他ROS参数。
配置节点选项:
```
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
```
- NodeOptions 类提供了多种配置节点行为的方法。
- 这里使用的 automatically_declare_parameters_from_overrides 方法确保所有从命令行或其他源传入并覆盖的参数都被自动声明为节点的参数，这对于动态配置节点非常有用，特别是在需要根据不同的环境或测试场景调整参数时。
创建节点:
```
auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);
```
- 使用前面配置的node_options，此行代码构造一个名为 "move_group_interface_tutorial" 的新节点。
- make_shared 是一个智能指针帮助函数，它在堆上创建对象并返回一个 std::shared_ptr，这有助于管理节点对象的内存，确保在程序结束时自动清理资源。
启动执行器管理节点:
```
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread([&executor]() { executor.spin(); }).detach();
```
- 创建执行器: SingleThreadedExecutor 是一个简单的执行器，适合运行不需要并发处理多个回调的应用。此执行器将处理节点上的所有活动，如订阅消息和服务请求。
- 添加节点到执行器: add_node 函数将节点添加到执行器的管理中，使得该节点上的回调可以被执行器处理。
- 启动执行器在新线程中运行: 通过创建一个新的线程来调用执行器的 spin 方法，该方法循环处理事件队列中的事件。使用 detach() 使新线程在后台运行，允许主线程继续执行而不阻塞在 spin 调用上，这是处理长时间运行或需要响应用户交互的节点的典型模式。

3. 设置 MoveIt 规划组和场景

在使用 MoveIt 进行机器人运动规划时，首先需要设置规划组（Planning Group），这是一组关节（Joints）或链接（Links），用于描述机器人的一部分，例如机器人的手臂或者其他运动组件。

以下是关于设置规划组和场景的详细说明：

（1）设置规划组

定义规划组名称:

MoveIt 中的规划组定义了需要进行运动规划和控制的具体部分。在代码中，规划组的名称被定义为 panda_arm，这是一个典型的示例名称，用于描述机器人手臂的关节或链接集合。
```
static const std::string PLANNING_GROUP = "panda_arm";
```
PLANNING_GROUP 是一个静态常量，其值被设置为 "panda_arm"。静态常量意味着在程序运行期间其值不会改变。
使用 MoveGroupInterface 设置规划组:

MoveGroupInterface 类是 MoveIt 中用于与规划组交互的主要接口之一。通过传入节点和规划组名称，可以轻松地设置一个 MoveGroupInterface 对象，用于后续的运动规划和控制操作。
```
moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
```
在这里，move_group_node 是之前创建的 MoveIt 节点对象，PLANNING_GROUP 是要操作和规划的规划组名称。

（2）管理场景和碰撞对象

使用 PlanningSceneInterface 管理场景:

PlanningSceneInterface 类允许在 MoveIt 的 “虚拟世界” 场景中添加、删除和管理碰撞对象。这些碰撞对象可以是静态物体、障碍物或者其他影响机器人运动的对象。
```
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
```
这个接口允许你动态地更新机器人周围的环境，以便在进行路径规划时考虑到这些场景变化。这行代码创建了一个名为 planning_scene_interface 的对象，它可以用来添加或移除碰撞对象，或者更新场景中的其他信息。
获取规划组的关节模型组:

在进行运动规划时，通常需要操作特定的一组关节，这通过 JointModelGroup 来实现。你可以从当前机器人状态中获取这个组：
```
const moveit::core::JointModelGroup* joint_model_group =
    move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
```
- JointModelGroup 表示一组关节，用于运动规划。
- getCurrentState() 获取当前机器人的状态。
- getJointModelGroup() 根据规划组名称获取对应的关节模型组。
这里，move_group.getCurrentState() 获取当前机器人的状态，包括所有关节的当前位置和状态。而 getJointModelGroup(PLANNING_GROUP) 则获取与你的规划组（如 “panda_arm”）相关联的关节模型组。这个组包含了规划过程中需要考虑的所有关节。

4. 可视化

在你的代码示例中，你展示了如何使用 MoveItVisualTools 类来进行可视化操作。下面是关于代码的详细解释和补充说明：

1. 初始化可视化工具

moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "panda_link0", "move_group_tutorial",move_group.getRobotModel());

move_group_node: 是之前创建的MoveIt节点。
"panda_link0": 是机器人中的参考坐标系，用于可视化中的参考点。
"move_group_tutorial": 是可视化工具的命名空间，用于在RViz中标识这些标记。
move_group.getRobotModel(): 获取当前机器人的模型，这是为了正确地渲染机器人模型和其关联的部件。

2. 删除所有标记

visual_tools.deleteAllMarkers();

这行代码用于清除之前发布的所有标记，以确保每次运行时都从一个干净的状态开始。

3. 加载远程控制功能

visual_tools.loadRemoteControl();

这个方法加载了RViz中的远程控制工具，允许用户通过按钮和键盘快捷键来控制和交互高级脚本，便于调试和可视化调整。

4. 发布文本标记

Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.0;
visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE);

Eigen::Isometry3d::Identity() 创建一个单位位姿，即一个位于原点的默认位姿。
text_pose.translation().z() = 1.0; 设置文本标记在z轴方向上的位移为1.0。这意味着文本标记将被放置在z坐标为1.0的位置。
visual_tools.publishText(...): 这个方法用于在RViz中发布文本标记。

这段代码发布了一个文本标记，显示为 "MoveGroupInterface_Demo"，我们修改了单位位姿的位置部分，将文本标记沿着z轴方向移动了1.0个单位。因此，text_pose 表示的位姿是一个位于x轴和y轴为0，z轴为1.0的位置的位姿。位于机器人的指定高度上（z = 1.0），使用白色（rvt::WHITE）和大号字体（rvt::XLARGE）。

5. 触发标记发布

visual_tools.trigger();

这个方法实际触发了所有之前发布的标记一起发送到RViz，减少了发送到RViz的消息数量，提高了效率。

总结

通过使用 MoveItVisualTools 类，你可以方便地在RViz中添加和管理多种类型的标记，从简单的文本到复杂的形状，帮助理解和调试机器人的运动规划和执行过程。这些工具极大地提升了开发和调试机器人应用程序的效率和便捷性。

5. 获取基本信息

当使用 MoveIt! 的 MoveGroupInterface 进行机器人运动规划时，获取机器人的基本信息是非常重要的。下面是如何通过 MoveIt! 提供的方法获取和打印机器人的基本信息：

获取规划框架名称:
```
RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());
```
- getPlanningFrame() 方法返回用于运动规划的参考坐标系的名称。
- 这个坐标系通常是机器人的基座坐标系，所有的运动路径规划都是相对于这个坐标系进行的。
获取末端执行器链接名称:
```
RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());
```
- getEndEffectorLink() 方法返回规划组末端执行器（工具或末端部件）的链接名称。
- 末端执行器链接是机器人末端执行器实际连接的机械结构部件。
获取所有规划组名称列表:
```
RCLCPP_INFO(LOGGER, "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
          std::ostream_iterator<std::string>(std::cout, ", "));
```
- getJointModelGroupNames() 方法返回一个向量，其中包含所有定义的规划组的名称。
- 这些规划组通常对应于机器人的各个功能单元或关节组合。
- std::copy():
  
  这是 C++ 标准库中的算法，用于将一个容器中的元素复制到另一个容器中。在这里，std::copy 从 move_group.getJointModelGroupNames() 返回的向量的开始到结束，将每个元素以字符串形式输出到输出流中。
  
  std::ostream_iteratorstd::string(std::cout, ", "):
  
  std::ostream_iterator 是一个输出迭代器，用于将输出流中的元素以特定的格式输出到目标流中。
  
  std::cout 是标准输出流，这里用于输出到控制台。", " 是指定输出元素之间的分隔符，这里是逗号加空格。
  
  假设机器人中有两个规划组：“panda_arm” 和 “panda_hand”，则上述代码的输出将会是：
```
Available Planning Groups: panda_arm, panda_hand,
```
名词解释：

规划框架（Planning Frame）:
- 规划框架是用于描述运动规划相对位置和方向的坐标系。它通常是机器人基座坐标系，所有运动路径的规划都以此为参考。
末端执行器链接（End Effector Link）:
- 末端执行器链接是机器人末端执行器（如手部或工具）实际连接的机械部件的名称。它决定了末端执行器的位置和姿态。
规划组名称列表:
- 规划组是一组关节或链接，它们可以一起被规划和控制。机器人通常有多个规划组，例如不同的机械臂部分或不同功能的关节组合。

6. 开始演示

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

这行代码用于在RViz中的可视化工具界面（RvizVisualToolsGui）上显示提示信息，提示用户按下’next’按钮来启动演示。

visual_tools:
- 这是一个 MoveItVisualTools 类的对象，用于在RViz中进行可视化操作，比如添加标记、显示文本等。
prompt() 方法:
- prompt() 是 MoveItVisualTools 类中的一个方法，用于在RViz中显示一个提示消息框。
- 参数 "Press 'next' in the RvizVisualToolsGui window to start the demo" 是要显示的提示信息内容。

功能说明

当这行代码执行时，它会在RViz的可视化工具界面上弹出一个消息框，提示用户进行下一步操作。
提示信息通常用于指导用户在RViz中进行互动，比如点击按钮、移动物体等，以启动或控制演示过程。

使用场景

在机器人控制和规划的演示过程中，通常会需要用户在RViz中进行一些操作来触发演示的不同阶段或动作。
prompt() 方法允许程序员在需要时向用户提供交互式的提示和指导，以确保演示过程能够按照预期进行。

总结

以上解释了如何使用 visual_tools.prompt() 方法在RViz中显示提示消息，以及这种功能在机器人控制和演示中的应用场景。希望这能帮助理解代码的功能和用法。如有更多问题或需要进一步解释，请随时提问！

7. 规划姿态目标

这段代码演示了如何使用MoveIt!规划一个姿态目标（Pose goal）并可视化规划结果。

（1）设置姿态目标

首先，我们定义一个目标姿态 target_pose1，并将其设置为规划组的目标：

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);

target_pose1:
- 这是一个 geometry_msgs::msg::Pose 类型的变量，用于定义目标姿态。
- orientation.w = 1.0 设置了姿态的四元数，表示不旋转（单位四元数）。
- position.x, position.y, position.z 分别设置了姿态的位置坐标。
move_group.setPoseTarget(target_pose1):
- setPoseTarget() 方法将 target_pose1 设置为规划组的姿态目标。

（2）计划运动

接下来，我们调用规划器来计算规划，并将结果可视化，但不实际移动机器人：

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");

move_group.plan(my_plan):
- plan() 方法用于计算规划，将计划存储在 my_plan 变量中。
- 返回的 MoveItErrorCode 枚举值用于指示规划是否成功。
success 变量:
- 根据 plan() 方法的返回值判断规划是否成功。
- 如果成功 (success 为 true)，则打印 "Visualizing plan 1 (pose goal) "，表示规划成功。
- 如果失败，则打印 "Visualizing plan 1 (pose goal) FAILED"。

总结

以上代码片段展示了如何设置姿态目标并计划运动，使用MoveIt!进行机器人运动规划的基本流程。这种方法允许在实际执行之前预览和验证运动计划，确保机器人可以安全和有效地执行任务。如果有任何进一步的问题或需要更多解释，请随时提问！

8. 可视化计划路径

这段代码演示了如何使用 MoveIt! 和 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");

1. 打印信息

RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line");

这行代码使用 RCLCPP_INFO 打印日志信息到控制台或日志文件，表示将要在 RViz 中将计划可视化为一条轨迹线。

2. 发布姿态标记

visual_tools.publishAxisLabeled(target_pose1, "pose1");

publishAxisLabeled() 方法用于在 RViz 中发布一个带有标签的坐标轴，以展示目标姿态 target_pose1 的位置和方向。

3. 发布文本标记

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

publishText() 方法用于在 RViz 中发布文本标记，显示在指定的 text_pose 位置上，标签为 "Pose_Goal"，白色字体，大小为 XLARGE。

4. 发布轨迹线

visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);

publishTrajectoryLine() 方法将计划好的轨迹路径 my_plan.trajectory_ 发布到 RViz 中。
joint_model_group 是关节模型组的指针，用于正确显示轨迹。

5. 触发更新

visual_tools.trigger();

trigger() 方法触发 RViz 的更新，确保之前发布的所有标记和轨迹线都被正确显示。

6. 提示用户操作

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

prompt() 方法显示一个提示信息，等待用户在 RViz 中点击 'next' 来继续演示。

以上代码片段展示了如何使用 MoveIt! 和 RViz 将规划的运动路径可视化为一条轨迹线，并在 RViz 中添加轴标记和文本标记以增强可视化效果。这种方法有助于验证规划的正确性和机器人路径的有效性。

9. 移动到姿势目标

在本教程中不使用该函数，因为它是阻塞功能，需要控制器处于活动状态并在执行轨迹时报告 Success。

/* Uncomment below line when working with a real robot */
/* move_group.move(); */

move() 函数在实际运行时用于执行规划出的路径。它是一个阻塞式调用，会等待机器人完成移动，并返回移动的成功或失败状态。
在实际使用中，需要确保机器人处于能够执行运动的状态，包括控制器处于活动状态并且硬件能够按照规划的轨迹执行动作。

10. 规划关节空间目标

让我们详细地讨论如何在 MoveIt! 中规划关节空间目标，并进行可视化。

（1）获取当前机器人状态

首先，我们需要获取当前机器人的状态，包括所有关节的当前位置、速度和加速度信息。

moveit::core::RobotStatePtr current_state = move_group.getCurrentState();

这将返回一个指向 RobotState 对象的指针，其中包含了当前的关节状态。

（2）获取关节模型组并设置目标关节值

接下来，我们从当前状态中获取目标关节模型组，并设置新的关节空间目标。

// 获取规划组的关节模型组
const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLAN_GROUP);

// 获取当前关节值集
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

// 设置新的关节空间目标
joint_group_positions[0] = -1.0;  // 设置第一个关节的目标值为 -1.0 弧度
move_group.setJointValueTarget(joint_group_positions);

获取规划组的关节模型组

current_state->getJointModelGroup(PLAN_GROUP)：使用机器人当前状态对象 current_state 的方法 getJointModelGroup() 获取指定名称（PLAN_GROUP）的关节模型组。
const moveit::core::JointModelGroup* joint_model_group：将获取到的关节模型组指针保存在 joint_model_group 变量中，以便后续使用。

获取当前关节值集

current_state->copyJointGroupPositions(joint_model_group, joint_group_positions)：使用机器人当前状态对象 current_state 的方法 copyJointGroupPositions()

将关节模型组 joint_model_group 中的所有关节当前位置复制到向量 joint_group_positions 中。
joint_group_positions：这是一个 std::vector 类型的向量，它存储了规划组中所有关节的当前位置。

设置新的关节空间目标

joint_group_positions[0] = -1.0;：修改 joint_group_positions 向量中的第一个元素，即第一个关节的目标位置，设为 -1.0 弧度。
move_group.setJointValueTarget(joint_group_positions);：使用 move_group 对象的 setJointValueTarget() 方法，将修改后的关节空间目标设置为 joint_group_positions。

（3）设置速度和加速度因子

为了控制机器人执行路径规划时的速度和加速度，我们可以设置最大速度和加速度的缩放因子。

// 设置最大速度和加速度的缩放因子（这里将其设置为默认值的 5%）
move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);

这里的缩放因子控制了机器人在执行轨迹时允许的最大速度和加速度，以确保运动的平滑性和安全性。

setMaxVelocityScalingFactor(0.05)：这个方法设置了运动规划时允许的最大速度缩放因子。在这里，0.05 表示将运动规划中使用的最大速度限制缩放到当前允许的 5%。
setMaxAccelerationScalingFactor(0.05)：这个方法设置了运动规划时允许的最大加速度缩放因子。同样，0.05 表示将运动规划中使用的最大加速度限制缩放到当前允许的 5%。

（4）计划运动并可视化

接下来，我们执行路径规划，并将规划的轨迹可视化在 RViz 中。

// 执行路径规划
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool 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");

执行路径规划

move_group.plan(my_plan): 这一行代码调用了MoveIt中的路径规划器来计划从当前机器人状态到设置的关节空间目标的路径。my_plan 是一个 MoveGroupInterface::Plan 结构体，它将保存规划后的路径信息。
moveit::core::MoveItErrorCode::SUCCESS: 这是一个枚举值，表示规划是否成功。如果规划成功，plan() 方法返回 SUCCESS，否则返回其他错误代码。
RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"): 这一行将根据规划的成功与否，在控制台打印相应的信息。如果规划成功，将打印 “Visualizing plan 2 (joint space goal)”，否则打印 “FAILED”。

在 RViz 中可视化计划

visual_tools.deleteAllMarkers(): 这个方法清除之前在RViz中发布的所有标记，确保新的可视化不会受到之前可视化的影响。
visual_tools.publishText(..): 这行代码发布了一个文本标记，显示为 “Joint_Space_Goal”，位于 text_pose 指定的位置，并使用白色文字和大字体尺寸。
visual_tools.publishTrajectoryLine(..): 这行代码发布了路径规划的轨迹线，将 my_plan.trajectory_ 中的轨迹可视化出来。参数 joint_model_group 用于指定轨迹线的关节模型组，确保正确地显示轨迹。
visual_tools.trigger(): 这一行触发了所有已发布可视化消息的显示，确保它们被立即渲染到RViz中。
visual_tools.prompt(".."): 最后一行显示了一个提示，告诉用户在RViz中按下 “next” 按钮以继续演示。

通过这些步骤，你可以在 MoveIt! 中有效地规划和执行关节空间的运动目标，并在 RViz 中进行实时可视化，帮助理解和调试机器人的路径规划行为。

11. 使用路径约束进行规划

1. 定义路径约束对象

首先，我们创建一个 moveit_msgs::msg::OrientationConstraint 对象来描述姿态约束的具体要求。以下是各个属性的详细解释：

moveit_msgs::msg::OrientationConstraint ocm;
ocm.link_name = "panda_link7";
ocm.header.frame_id = "panda_link0";
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;

ocm.link_name = "panda_link7";
- 指定需要约束的机器人链接名称。在这里，我们选择了机器人末端执行器的链接，通常是机械臂末端的关节或者工具链接。
ocm.header.frame_id = "panda_link0";
- 设置参考坐标系的名称。姿态约束将相对于该坐标系来描述。
姿态四元数/姿态容差部分：
- w 分量设置为 1.0 表示期望的姿态四元数为 (0, 0, 0, 1)，即无旋转，与参考坐标系的方向一致。
- absolute_x_axis_tolerance, absolute_y_axis_tolerance, absolute_z_axis_tolerance 分别设置了在 x、y、z 轴方向上允许的姿态偏差范围。这些值表示机器人在执行路径时，可以在这些轴向上偏离期望姿态的最大量。
权重部分：
- weight 属性指定了姿态约束的重要性或优先级。在规划过程中，具有更高权重的约束将更加严格地被遵循。

2. 设置路径约束

接下来，将定义好的姿态约束对象应用到机器人运动规划中。这需要使用 moveit_msgs::msg::Constraints 对象来封装所有的约束条件，并将其传递给 MoveIt 的规划接口。

moveit_msgs::msg::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);

创建 moveit_msgs::msg::Constraints 对象 test_constraints，用于存储所有约束条件。
将之前定义的姿态约束 ocm 添加到 orientation_constraints 列表中，表示我们希望机器人在规划路径时遵循这些姿态限制。

3. 应用路径约束

最后，将路径约束应用到 MoveIt 的运动规划器中，确保机器人在执行轨迹规划时遵循指定的姿态约束。

move_group.setPathConstraints(test_constraints);

使用 move_group.setPathConstraints(test_constraints) 将 test_constraints 对象应用到 move_group 对象（通常是 MoveIt 的 MoveGroupInterface 实例）的运动规划器中。

通过以上步骤，你可以详细地定义和应用姿态约束，以确保机器人在执行路径规划时遵循特定的姿态要求。这种方法可以用于各种需要精确控制机器人末端执行器姿态的应用场景，例如避障、精确抓取和复杂操作路径规划等。

12. 在关节空间中强制规划

这段代码演示了如何在关节空间中使用 MoveIt 进行路径规划，并利用路径约束来指定起始姿态和目标姿态。

我们将重用我们曾经的旧目标并为其制定计划。请注意，这仅在当前 state 已经满足 path constraints。所以我们需要设置开始 state 设置为新姿势。

在关节空间中强制规划涉及使用MoveIt进行路径规划，并应用路径约束的详细步骤如下：

1. 定义和应用路径约束

首先，定义路径约束和起始状态：

// 定义起始状态
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);
//这个函数的作用是告诉MoveIt规划器，机器人需要移动到一个特定的姿态，由target_pose1指定。

// 设置路径约束
moveit_msgs::msg::Constraints path_constraints;
move_group.setPathConstraints(path_constraints);  
// 这里将 path_constraints 设置为空，表示在关节空间中强制规划

2. 设置规划时间和执行规划

// 设置规划时间
move_group.setPlanningTime(10.0);
//这行代码设置了规划器（planner）的最大规划时间为10秒。
//规划时间的设置是为了确保规划器有足够的时间来尝试找到一条有效的路径。

// 执行路径规划
moveit::planning_interface::MoveItErrorCode success = move_group.plan(my_plan);
//这行代码调用了move_group对象的plan函数来执行路径规划。

// 输出规划结果
RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

3. 可视化规划结果

在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");

4. 清除路径约束

完成路径规划后，清除已应用的路径约束：

move_group.clearPathConstraints();

这些步骤详细说明了如何在MoveIt中使用关节空间强制规划路径，应用路径约束，并在RViz中可视化和清除路径约束。

13. 笛卡尔路径

当你使用MoveIt进行笛卡尔路径规划时：

（1）定义航点列表和目标姿态

// 定义航点列表和目标姿态
std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(start_pose2);

geometry_msgs::msg::Pose target_pose3 = start_pose2;

// 将末端执行器向下移动0.2米
target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3);  // down

// 将末端执行器向右移动0.2米
target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3);  // right

// 将末端执行器向上左移动0.2米
target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3);  // up and left

这段代码定义了一个名为waypoints的向量，用于存储笛卡尔路径中的航点（目标姿态）。
start_pose2被添加为起始航点，然后通过修改target_pose3的位置，逐步构建了末端执行器移动的路径。

（2）计算笛卡尔路径

// 计算笛卡尔路径
moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;  // 跳跃阈值，设置为0.0禁用跳跃
const double eef_step = 0.01;       // 末端执行器步长，每步0.01米

// 使用MoveIt计算笛卡尔路径
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);

创建了一个名为 trajectory 的变量，类型为 moveit_msgs::msg::RobotTrajectory。这是MoveIt中用于存储机器人轨迹规划结果的消息类型。
jump_threshold ，它被设置为 0.0。它表示路径规划中的跳跃阈值，设置为 0.0 表示禁用跳跃，即路径中不允许有跳跃的运动。

eef_step ，它被设置为 0.01。在这里，它表示末端执行器（End Effector，通常是机器人手臂末端的工具或夹具）在笛卡尔路径规划中每一步的步长，即每次移动的距离为 0.01 米。
computeCartesianPath函数计算从当前机器人状态到目标航点列表waypoints的笛卡尔路径。
fraction表示规划的路径实现的百分比。成功时，这个值接近1.0。
"Visualizing plan 4 (Cartesian path) (%.2f%% achieved)": 这是日志消息的格式字符串。在这里，%.2f 是一个格式化占位符，用来插入浮点数。具体来说，它将用 fraction * 100.0 的值替换 %f，并且保留两位小数显示，表示已达成的笛卡尔路径规划的百分比。

（3）可视化路径规划

// 在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);

// 触发RViz中的更新并等待用户确认
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

这段代码用于在RViz中可视化规划的笛卡尔路径。
visual_tools.publishText发布一个文本消息，用于标记路径的类型。
visual_tools.publishPath发布路径，使用绿色标记以示区别。
visual_tools.publishAxisLabeled在每个航点处发布带标签的坐标系，帮助理解路径中的方向变化。
visual_tools.trigger用于触发RViz中的更新。
visual_tools.prompt等待用户在RViz界面中点击’next’按钮，以便继续演示。

（4）执行路径

// 执行笛卡尔路径规划得到的轨迹（取消注释以实际执行）
/* move_group.execute(trajectory); */

这段代码展示了如何执行计算出的笛卡尔路径规划得到的轨迹。
注释部分move_group.execute(trajectory);如果取消注释，将使机器人按照计划好的笛卡尔路径执行移动。

以上解释希望能帮助您理解每个步骤在MoveIt中执行笛卡尔路径规划的过程及其实现方式。

14. 将对象添加到环境中

在这部分中，我们将演示如何在 MoveIt 中将碰撞对象添加到机器人运动规划的环境中，并进行路径规划以避开这些障碍物。这对于在实际应用中确保机器人安全运行至关重要。

1. 规划无障碍物的目标路径

首先，我们演示如何规划一个简单的目标路径，此时环境中没有障碍物。这部分的代码如下：

// 设置起始状态为当前机器人的当前状态
move_group.setStartState(*move_group.getCurrentState());
// 定义目标姿态的末端执行器姿态
geometry_msgs::msg::Pose target_pose;
target_pose.orientation.w = 0;       // 设置四元数姿态的实部为0，表示与x轴反向对齐
target_pose.orientation.x = -1.0;    // 设置四元数姿态的虚部x为-1.0，表示绕x轴旋转180度
target_pose.position.x = 0.7;         // 设置目标位置x坐标为0.7
target_pose.position.y = 0.0;         // 设置目标位置y坐标为0.0
target_pose.position.z = 0.59;        // 设置目标位置z坐标为0.59
// 将目标姿态设置为运动组的目标
move_group.setPoseTarget(target_pose);


// 进行路径规划
success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");

// 在RViz中可视化规划结果
visual_tools.deleteAllMarkers// 清除所有的标记物体
visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE);// 在指定的姿态位置上发布文本标签，用于显示目标信息
visual_tools.publishAxisLabeled(target_pose, "goal");// 在指定的姿态位置上发布带有标签的坐标轴，用于显示目标位置
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);// 发布规划的路径轨迹线，使用给定的关节模型群组显示
visual_tools.trigger();// 触发RViz中的可视化更新
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");// 在RVizVisualToolsGui窗口中显示提示信息，等待用户操作继续

设置起始状态 (setStartState): 使用当前机器人的状态作为起点，这可以确保规划的路径从当前机器人的姿态开始。
设置目标姿态 (setPoseTarget): 定义一个新的目标姿态，这里使用了一个具体的位姿 target_pose。
路径规划 (plan): 使用 MoveIt 进行路径规划，并将结果存储在 my_plan 中。
可视化 (publishText, publishAxisLabeled, publishTrajectoryLine): 在 RViz 中展示规划的结果，包括目标姿态、规划路径的轨迹等。

2. 添加碰撞对象

接下来，我们定义一个长方体碰撞对象，并将其添加到机器人运动规划的环境中。这部分代码如下：

moveit_msgs::msg::CollisionObject collision_object;

// 设置碰撞对象的参考坐标系为规划框架
collision_object.header.frame_id = move_group.getPlanningFrame();

// 设置碰撞对象的唯一标识符
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; // 高度

// 定义长方体的姿态（位置和方向）
geometry_msgs::msg::Pose box_pose;
box_pose.orientation.w = 1.0;   // 设置四元数姿态的实部为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);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the collision object appears in RViz");

碰撞对象的定义 (CollisionObject): 使用 moveit_msgs::CollisionObject 类型定义一个碰撞对象，设置其所属的参考坐标系、形状类型、尺寸和姿态。
添加到规划场景 (addCollisionObjects): 将定义好的碰撞对象添加到 MoveIt 规划场景中，这使得后续的路径规划可以考虑到这个碰撞对象。
可视化 (publishText): 在 RViz 中显示状态文本，指示碰撞对象已经添加到规划场景中，等待 MoveGroup 处理。

3. 规划避开碰撞对象的路径

最后，我们再次进行路径规划，这时机器人会避开新添加的碰撞对象。以下是相关代码：

// 执行轨迹规划，并检查是否成功
success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
// 输出规划结果到日志
RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED");
// 在RViz中发布文本以显示目标状态
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();
// 提示用户在RViz中手动完成后续步骤
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");

路径规划 (plan): 使用 MoveIt 再次进行路径规划，这次规划会考虑到新添加的碰撞对象，确保机器人可以安全地避开障碍物。
可视化 (publishText, publishTrajectoryLine): 在 RViz 中展示规划的结果，包括避开碰撞对象后的路径轨迹。

通过以上详细说明，我们展示了如何在 MoveIt 中操作碰撞对象，以及如何进行路径规划以确保机器人在运动过程中避开障碍物，这对于实际机器人应用中的安全性至关重要。

15. 将对象附加到机器人

（1）创建要附加的碰撞对象和定义形状与位置

// 创建要附加到机器人的碰撞对象
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;  // 圆柱体高度为 20cm
cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;   // 圆柱体半径为 4cm

// 定义圆柱体在末端执行器坐标系下的姿态
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;      // 圆柱体位于末端执行器坐标系下的高度 0.2m

在这部分中，我们首先创建了一个要附加到机器人的圆柱体碰撞对象 object_to_attach。
然后，我们定义了圆柱体的形状和位置。shape_msgs::SolidPrimitive 用于描述基本形状，例如圆柱体、立方体等，而 geometry_msgs::Pose 则定义了物体的位置和方向。

（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);

在这里，我们将圆柱体的形状和姿态添加到 object_to_attach 中，并使用 planning_scene_interface 的 applyCollisionObject 方法将其应用到规划场景中。

这一步骤确保了规划器能够考虑到这个对象的存在，避免路径规划时与其发生碰撞。

（3）将对象附加到机器人

// 将对象附加到机器人
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);

// 在RViz中发布文本提示对象已附加到机器人
visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

// 等待 MoveGroup 接收和处理附加的碰撞对象消息
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot");

std::vector touch_links;: 创建一个字符串向量 touch_links，用于存储允许碰撞的机器人手部链接。

touch_links.push_back("panda_rightfinger");: 将右手指链接 panda_rightfinger 添加到 touch_links 向量中。

touch_links.push_back("panda_leftfinger");: 将左手指链接 panda_leftfinger 添加到 touch_links 向量中。
move_group.attachObject(object_to_attach.id, "panda_hand", touch_links);: 使用 move_group 对象的 attachObject 方法。

将 object_to_attach 中定义的对象附加到机器人的手部（通过 "panda_hand" 指定）。

touch_links 向量指定了允许碰撞的手指链接，确保附加的对象不会与机器人手部发生碰撞。

这部分代码用于将圆柱体对象附加到机器人上。我们指定了允许发生碰撞的手指链接，确保机器人能够正确地夹持和操作这个对象。move_group.attachObject 方法用于将对象附加到机器人的手，这一步骤非常关键，因为它允许机器人在移动时保持对象的稳定性。

（4）重新规划移动路径

// 设置起始状态为当前状态
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");

// 在 RViz 中显示路径轨迹
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();

// 等待移动计划完成
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");

最后，这段代码重新规划机器人的移动路径，确保它能够在操作附加的圆柱体时避免碰撞。

move_group.setStartStateToCurrentState() 用于将机器人的当前状态设置为路径规划的起始状态。
move_group.plan(my_plan) 对象用于执行路径规划，并将结果存储在 my_plan 变量中。成功的路径规划结果将显示在 RViz 中，以便用户可以检查和验证移动路径。

这些步骤详细地解释了如何在 MoveIt 中将一个对象附加到机器人上，并进行路径规划以避免碰撞，从而模拟夹持和操作物体的过程。

16. 分离和删除对象

// 输出日志信息，表示正在从机器人上拆下对象
RCLCPP_INFO(LOGGER, "Detach the object from the robot");

// 使用 move_group 对象的 detachObject 方法，从机器人上拆下对象
move_group.detachObject(object_to_attach.id);

RCLCPP_INFO(LOGGER, "Detach the object from the robot");: 输出一个信息，表示正在从机器人上拆下对象。
move_group.detachObject(object_to_attach.id);: 使用 move_group 对象的 detachObject 方法，根据 object_to_attach.id 拆下已附加的对象。

（1）在 RViz 中显示状态的文本

// 删除所有标记并发布文本状态信息，表示对象已从机器人上拆下
visual_tools.deleteAllMarkers();
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");

visual_tools.deleteAllMarkers();: 删除 RViz 中所有的标记，确保清除之前的所有可视化效果。
visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE);: 在 RViz 中发布文本，显示对象已成功从机器人上拆下。
visual_tools.trigger();: 触发 RViz 可视化工具，确保文本提示信息和可视化效果立即显示在 RViz 中。
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot");: 在 RViz 中显示一个提示框，等待用户在 RVizVisualToolsGui 窗口中按下 ‘next’ 按钮，确认新对象已经成功从机器人上拆下。

（2）从世界中删除对象

// 输出日志信息，表示正在从世界中删除对象
RCLCPP_INFO(LOGGER, "Remove the objects from the world");

// 创建一个字符串向量，存储要从规划场景中删除的对象的 ID
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);    // 添加第一个碰撞对象的 ID
object_ids.push_back(object_to_attach.id);    // 添加要附加到机器人的对象的 ID
planning_scene_interface.removeCollisionObjects(object_ids); // 使用规划场景接口，删除指定的碰撞对象

RCLCPP_INFO(LOGGER, "Remove the objects from the world");: 输出一个信息，表示正在从世界中删除对象。
std::vector<std::string> object_ids;: 创建一个字符串向量 object_ids，用于存储要从规划场景中删除的对象的 ID。
object_ids.push_back(collision_object.id);: 将第一个碰撞对象的 ID 添加到 object_ids 向量中。
object_ids.push_back(object_to_attach.id);: 将要附加到机器人的对象的 ID 添加到 object_ids 向量中。
planning_scene_interface.removeCollisionObjects(object_ids);: 使用规划场景接口的 removeCollisionObjects 方法，根据 object_ids 向量中的 ID，从规划场景中删除指定的碰撞对象。

（3）在 RViz 中显示状态的文本

// 在 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");

visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE);: 在 RViz 中发布文本，显示对象已成功从世界中移除。
visual_tools.trigger();: 触发 RViz 可视化工具，确保文本提示信息和可视化效果立即显示在 RViz 中。
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disappears");: 在 RViz 中显示一个提示框，等待用户在 RVizVisualToolsGui 窗口中按下 ‘next’ 按钮，确认碰撞对象已经成功从世界中移除。

这些步骤确保了从机器人上拆下对象并从世界中删除对象，并通过 RViz 提供了交互确认的方式。

四、补充

1. 规划组（Planning Group）

在ROS和MoveIt!中，PLANNING_GROUP 是一个常量字符串，它指定了机器人模型中一组特定的关节。这个字符串用来引用机器人的某个特定部分，该部分可以进行独立运动规划。在MoveIt!中，规划组（planning group）通常对应于机器人的一个子集，比如一个机械臂或移动平台。

（1）规划组（Planning Group）的概念

规划组是机器人的一部分，它包含一组关节，这些关节可以一起运动以完成特定的任务。例如，一个具有多个臂和抓手的复杂机器人可能有几个规划组：

臂部：可以是主要的臂部或次要的臂部。
移动平台：可以是机器人的底座，用于移动整个机器人。
头部：如果机器人有头部，它可能包含可以独立控制的关节。

（2）为什么使用规划组

使用规划组的原因包括：

简化规划：机器人可能具有大量的关节，但并非所有关节都需要参与到每一个任务中。通过定义规划组，可以简化规划过程，只考虑与特定任务相关的关节。
提高效率：对特定组的关节进行规划通常比对整个机器人进行规划要快，这样可以提高计算效率。
灵活性：规划组允许开发者为不同的任务重用相同的机器人模型，只需切换不同的规划组。

（3）示例

PLANNING_GROUP 是一个常量字符串，它只是一个标识符，用于在你的代码中引用特定的规划组。你可以将 PLANNING_GROUP 替换为任何其他有意义的英文单词或短语，只要它符合C++变量命名规则，并且与你的机器人模型中的规划组名称相匹配。

例如，如果你的机器人有一个叫做 “manipulator” 的机械臂，你可以这样定义：

static const std::string MANIPULATOR_GROUP = "manipulator";

然后在创建 MoveGroupInterface 对象时使用这个新的常量字符串：

moveit::planning_interface::MoveGroupInterface move_group(node, MANIPULATOR_GROUP);

重要的是要确保代码中使用的字符串与机器人的URDF/XACRO文件中定义的规划组名称一致，以及与MoveIt!配置文件中定义的名称相匹配。这样，当你的程序请求规划组时，它才能正确地引用到机器人模型的相应部分。

此外，保持代码中的命名清晰和一致也很重要，这样其他阅读你代码的人（或未来的你）能够更容易理解每个规划组的用途。通常，规划组的名称会直接反映它在机器人模型中的功能或位置。

（4）总结

在ROS 2和MoveIt!中，通过定义和使用规划组，你可以更有效地控制和规划机器人的运动。每个规划组对应于机器人的一个特定部分，使得运动规划更加灵活和高效

2. 关节模型组

在MoveIt!中，关节模型组（JointModelGroup）是一组关节的集合，它们通常属于机器人的一部分，比如一个机械臂或移动平台。这些关节可以一起被规划和控制。例如，一个完整的机器人可能包括基座、臂部、手腕和手爪等多个部分，每个部分都可以定义为一个关节模型组。

获取关节模型组的步骤

获取当前状态：
首先，你需要获取机器人的当前状态，这包括了机器人所有关节的当前位置和其他相关信息。
```
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
```
获取关节模型组：
然后，你可以使用规划组的名称（例如，"panda_arm"）来获取对应的关节模型组。这个名称应该与你的机器人模型和MoveIt!配置中定义的名称相匹配。
```
const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP);
```
这里，getJointModelGroup() 方法返回一个指向关节模型组的指针，该组包含了规划组中定义的所有关节。
合并代码：

将上述两步合并，可以直接在一行代码中完成这两个操作，这在实际编程中很常见，以提高代码的简洁性。
```
const moveit::core::JointModelGroup* joint_model_group =
    move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
```