【MoveIt 2】示例:移动组 C++ 接口

最新推荐文章于 2024-08-02 17:37:12 发布

十年一梦实验室

最新推荐文章于 2024-08-02 17:37:12 发布

阅读量1k

点赞数 17

本文链接：https://blog.csdn.net/cxyhjl/article/details/140942395

版权

在 MoveIt 中，最简单的用户界面是通过 MoveGroupInterface 类https://github.com/moveit/moveit2/blob/main/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h 。它为用户可能想要执行的大多数操作提供了易于使用的功能，特别是设置关节或姿态目标、创建运动计划、移动机器人、向环境中添加对象以及从机器人附加/分离对象。该接口通过 ROS 主题、服务和操作与 MoveGroup 节点https://github.com/moveit/moveit2/blob/main/moveit_ros/move_group/src/move_group.cpp 进行通信。

观看这个快速的 YouTube 视频https://www.youtube.com/watch?v=_5siHkFQPBQ&feature=youtu.be 演示，了解移动组界面的强大功能！

视频演示

入门

如果您还没有这样做，请确保您已完成入门中的步骤。https://moveit.picknik.ai/main/doc/tutorials/getting_started/getting_started.html

运行代码

打开两个终端。在第一个终端中，启动 RViz 并等待所有内容加载完成：

ros2 launch moveit2_tutorials move_group.launch.py

在第二个 shell 中，运行启动文件：

ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py

片刻之后，RViz 窗口应该会出现，并且看起来与此页面顶部的窗口类似。要通过每个演示步骤，请按屏幕底部的 RvizVisualToolsGui 面板中的 Next 按钮，或在屏幕顶部的 Tools 面板中选择 Key Tool，然后在 RViz 聚焦时按键盘上的 0。

预期输出

请参阅本教程顶部的 YouTube 视频以了解预期输出。在 RViz 中，我们应该能够看到以下内容：

机器人将其手臂移动到其前方的姿势目标。
机器人将其手臂移动到其侧面的关节目标。
机器人在保持末端执行器水平的同时，将手臂移回到一个新的姿态目标。
机器人沿着所需的笛卡尔路径移动其手臂（一个三角形向下，向右，向上+向左）。
机器人将其手臂移动到一个没有障碍物的简单目标。
一个箱子对象被添加到环境中，位于机械臂的右侧。
机器人将其手臂移动到目标姿势，避免与箱子碰撞。
物体附着在手腕上（其颜色将变为紫色/橙色/绿色）。
机器人移动其带有附加物的手臂到达目标姿势，避免与箱子碰撞。
物体从手腕上脱落（其颜色将变回绿色）。
对象已从环境中移除。

整个代码

整个代码可以在 MoveIt GitHub 项目中https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp 看到。接下来，我们逐步解释代码的功能。

设置

MoveIt 在称为“规划组 planning groups ”的关节集上运行，并将它们存储在称为 JointModelGroup 的对象中。在 MoveIt 中，“规划组”和“关节模型组”这两个术语可以互换使用。

static const std::string PLANNING_GROUP = "panda_arm";

MoveGroupInterface 类可以通过使用您想要控制和规划的规划组的名称轻松设置。

moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);

我们将使用 PlanningSceneInterface 类在我们的“虚拟世界”场景中添加和移除碰撞对象

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


/* 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 提供多种类型的标记，在此演示中我们将使用文本、圆柱体和球体

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

规划一个位姿目标

我们可以为该组规划一个运动，以使末端执行器达到所需的姿势。

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() 函数。请注意，我们之前设置的姿态目标仍然是活动的，因此机器人将尝试移动到该目标。在本教程中我们不会使用该函数，因为它是一个阻塞函数，需要控制器处于活动状态并报告轨迹执行的成功。

/* 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;  // radians
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";
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;

现在，将其设置为组的路径约束。

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

在关节空间中执行规划

根据规划问题，MoveIt 在 joint space 和 cartesian space 之间选择问题表示。 在 ompl_planning.yaml 文件中设置组参数 enforce_joint_model_state_space:true 强制对所有规划使用 joint space 。

默认情况下，具有方向路径约束的规划请求在 cartesian space 中采样，以便调用 IK 作为生成采样器。

通过执行 joint space ，规划过程将使用拒绝采样来找到有效请求。请注意，这可能会大大增加规划时间。

我们将重新使用我们之前的目标并规划抵达它。请注意，这只有在当前状态已经满足路径约束时才会有效。因此，我们需要将起始状态设置为新的姿态。

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

我们希望笛卡尔路径以 1 厘米的分辨率进行插值，这就是为什么我们将最大步长指定为 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 设置，但需要您手动计时轨迹，如此处所述。欢迎提交拉取请求。

你可以执行这样的轨迹。

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

结果可能如下所示：

现在，让我们定义一个机器人要避开的碰撞对象 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("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in 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);
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");

结果可能如下所示：

将物体附加到机器人

您可以将物体附加到机器人上，使其随机器人几何形状移动。这模拟了拾取物体以便操纵它。运动规划还应避免物体之间的碰撞。

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

重新规划，但现在手头有对象。

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

结果可能看起来像这样：

分离和移除对象

现在，让我们从机器人的夹持器上拆下气缸。

RCLCPP_INFO(LOGGER, "Detach the object from the robot");
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();


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

现在，让我们从世界上移除这些对象。

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

在 RViz 中显示状态文本

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

启动文件

整个启动文件https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/move_group_interface/launch/move_group_interface_tutorial.launch.py 在 GitHub 上。本教程中的所有代码都可以从您作为 MoveIt 设置一部分的 moveit2_tutorials 包中运行。

from launch import LaunchDescription  # 从launch模块导入LaunchDescription类
from launch_ros.actions import Node  # 从launch_ros.actions模块导入Node类
from moveit_configs_utils import MoveItConfigsBuilder  # 从moveit_configs_utils模块导入MoveItConfigsBuilder类


def generate_launch_description():  # 定义生成启动描述的函数
    moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()  # 使用MoveItConfigsBuilder创建MoveIt配置


    # MoveGroupInterface演示可执行文件
    move_group_demo = Node(
        name="move_group_interface_tutorial",  # 节点名称为move_group_interface_tutorial
        package="moveit2_tutorials",  # 节点所属包为moveit2_tutorials
        executable="move_group_interface_tutorial",  # 可执行文件为move_group_interface_tutorial
        output="screen",  # 输出到屏幕
        parameters=[
            moveit_config.robot_description,  # 机器人描述参数
            moveit_config.robot_description_semantic,  # 机器人语义描述参数
            moveit_config.robot_description_kinematics,  # 机器人运动学描述参数
        ],
    )


    return LaunchDescription([move_group_demo])  # 返回包含move_group_demo节点的启动描述

关于设置公差的说明

请注意，MoveGroupInterface 的 setGoalTolerance() 和相关方法设置的是规划的容差，而不是执行的容差。

如果要配置执行公差，您将不得不编辑 controller.yaml 文件（如果使用 FollowJointTrajectory 控制器），或者手动将其添加到规划器生成的轨迹消息中。

附录：完整代码

#include <moveit/move_group_interface/move_group_interface.h> // 包含MoveIt的移动组接口头文件
#include <moveit/planning_scene_interface/planning_scene_interface.h> // 包含MoveIt的规划场景接口头文件


#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(); // 启动一个线程来运行执行器


  // BEGIN_TUTORIAL
  //
  // 设置
  // ^^^^^
  //
  // MoveIt操作一组称为“规划组”的关节，并将它们存储在一个称为JointModelGroup的对象中。
  // 在MoveIt中，“规划组”和“关节模型组”这两个术语可以互换使用。
  static const std::string PLANNING_GROUP = "panda_arm"; // 定义规划组名称


  // MoveGroupInterface类可以通过规划组的名称轻松设置，用于控制和规划。
  moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);


  // 我们将使用PlanningSceneInterface类在“虚拟世界”场景中添加和移除碰撞对象
  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; // 定义rviz_visual_tools命名空间的别名
  moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "panda_link0", "move_group_tutorial",
                                                      move_group.getRobotModel()); // 创建MoveItVisualTools对象


  visual_tools.deleteAllMarkers(); // 删除所有标记


  /* 远程控制是一个自省工具，允许用户通过按钮和键盘快捷键在RViz中逐步执行高级脚本 */
  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");


  // 规划到姿态目标
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // 我们可以为此组规划一个运动到末端执行器的目标姿态。
  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"; // 设置链接名称
  ocm.header.frame_id = "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”以强制在所有计划中使用“关节空间”。
  //
  // 默认情况下，带有方向路径约束的规划请求在“笛卡尔空间”中采样，以便调用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;
  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厘米的分辨率插值，这就是为什么我们将最大步长指定为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设置，
  // 但需要您手动计时轨迹，如此处所述。拉取请求是受欢迎的。
  //
  // 您可以像这样执行轨迹。
  /* 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"; // 设置碰撞对象的ID


  // 定义一个要添加到世界中的盒子。
  shape_msgs::msg::SolidPrimitive primitive; // 定义一个实心原语
  primitive.type = primitive.BOX; // 设置原语类型为盒子
  primitive.dimensions.resize(3); // 调整尺寸数组大小为3
  primitive.dimensions[primitive.BOX_X] = 0.1; // 设置盒子的X尺寸
  primitive.dimensions[primitive.BOX_Y] = 1.5; // 设置盒子的Y尺寸
  primitive.dimensions[primitive.BOX_Z] = 0.5; // 设置盒子的Z尺寸


  // 定义盒子的姿态（相对于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"); // 提示用户继续演示


  // 现在，当我们规划轨迹时，它将避开障碍物。
  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); // 发布轨迹线
  visual_tools.trigger(); // 触发批量发布
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); // 提示用户继续演示
//
  // 结果可能如下所示：
  //
  // .. image:: ./move_group_interface_tutorial_avoid_path.gif
  //    :alt: 动画显示手臂避开新障碍物移动
  //
  // 将对象附加到机器人
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 您可以将对象附加到机器人上，使其随机器人几何体移动。
  // 这模拟了拾取对象以便操纵它。
  // 运动规划应避免对象之间的碰撞。
  moveit_msgs::msg::CollisionObject object_to_attach; // 定义要附加的碰撞对象
  object_to_attach.id = "cylinder1"; // 设置碰撞对象的ID


  shape_msgs::msg::SolidPrimitive cylinder_primitive; // 定义一个实心圆柱原语
  cylinder_primitive.type = primitive.CYLINDER; // 设置原语类型为圆柱
  cylinder_primitive.dimensions.resize(2); // 调整尺寸数组大小为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; // 设置抓取姿态的Z位置


  // 首先，我们将对象添加到世界中（不使用向量）。
  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); // 将对象附加到机器人上


  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"); // 提示用户继续演示


  // 重新规划，但现在手中有对象。
  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"); // 提示用户继续演示


  // 结果可能如下所示：
  //
  // .. image:: ./move_group_interface_tutorial_attached_object.gif
  //    :alt: 动画显示手臂在附加对象后移动不同
  //
  // 分离和移除对象
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 现在，让我们从机器人的夹持器上分离圆柱体。
  RCLCPP_INFO(LOGGER, "Detach the object from the robot"); // 打印信息，从机器人上分离对象
  move_group.detachObject(object_to_attach.id); // 分离对象


  // 在RViz中显示状态文本
  visual_tools.deleteAllMarkers(); // 删除所有标记
  visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE); // 在RViz中发布文本
  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添加到向量中
  object_ids.push_back(object_to_attach.id); // 将附加对象的ID添加到向量中
  planning_scene_interface.removeCollisionObjects(object_ids); // 从规划场景中移除碰撞对象


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


  /* 等待MoveGroup接收和处理附加的碰撞对象消息 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disappears"); // 提示用户继续演示


  // 结束教程
  visual_tools.deleteAllMarkers(); // 删除所有标记
  visual_tools.trigger(); // 触发批量发布


  rclcpp::shutdown(); // 关闭rclcpp
  return 0; // 返回0
}