本教程将引导您创建一个使用 MoveIt 任务构造器规划抓取和放置操作的包。MoveIt 任务构造器（https://github.com/moveit/moveit_task_constructor/tree/ros2/）提供了一种为包含多个不同子任务（称为阶段）的任务进行规划的方法。如果您只想运行教程，可以按照 Docker 指南 https://moveit.picknik.ai/main/doc/how_to_guides/how_to_setup_docker_containers_in_ubuntu.html 启动一个包含完整教程的容器。

 1 基本概念 

MTC 的基本思想是复杂的运动规划问题可以分解为一组更简单的子问题。顶层规划问题被指定为任务，而所有子问题由阶段指定。阶段可以按任何任意顺序和层次排列，仅受各个阶段类型的限制。阶段的排列顺序受结果传递方向的限制。与结果流相关的阶段有三种：生成器阶段、传播器阶段和连接器阶段：

生成器独立于其相邻阶段计算结果，并在两个方向上传递，向后和向前。一个例子是几何位姿的 IK 采样器，其中接近和离开运动（相邻阶段）取决于解决方案。

传播器接收一个邻居阶段的结果，解决一个子问题，然后将结果传播到对面邻居。根据实现的不同，传播阶段可以分别向前、向后或双向传递解决方案。一个例子是基于起始状态或目标状态计算笛卡尔路径的阶段。

连接器不会传播任何结果，而是尝试弥合两个相邻状态之间的差距。一个例子是从一个给定状态到另一个状态的自由运动规划的计算。

除了顺序类型外，还有不同的层次类型允许封装从属阶段。没有从属阶段的阶段称为原始阶段，高级阶段称为容器阶段。有三种容器类型：

包装器封装单个从属阶段并修改或过滤结果。例如，仅接受其子阶段满足某个约束条件的解决方案的过滤阶段可以实现为包装器。此类型的另一个标准用法包括 IK 包装器阶段，该阶段基于带有姿态目标属性的规划场景生成逆运动学解决方案。

序列容器包含一系列从属阶段，并且仅将端到端解决方案视为结果。一个例子是由一系列连贯步骤组成的拣选动作。

并行容器结合了一组从属阶段，可用于传递最佳的替代结果、运行后备求解器或合并多个独立的解决方案。示例包括为自由运动计划运行替代规划器、用右手或左手作为后备拾取物体，或同时移动手臂和打开夹爪。

阶段不仅支持解决运动规划问题。它们还可以用于各种状态转换，例如修改规划场景。结合使用类继承的可能性，可以在仅依赖于一组结构良好的原始阶段的情况下构建非常复杂的行为。

有关 MTC 的更多详细信息，请参阅 MoveIt 任务构造器概念页面https://moveit.picknik.ai/main/doc/concepts/moveit_task_constructor/moveit_task_constructor.html 

 2 入门 

如果您还没有这样做，请确保您已完成入门中的步骤。

2.1 下载 MoveIt 任务构造器 

进入你的 colcon 工作区并拉取 MoveIt 任务构建器源代码：https://github.com/PickNikRobotics/moveit_task_constructor  （手动下载）

cd ~/ws_moveit/src
git clone git@github.com:moveit/moveit_task_constructor.git -b ros2

cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws/src$ cd ..
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ colcon build --packages-select moveit_task_constructor_demo

‍3 试用 

MoveIt 任务构建器包包含几个基本示例和一个挑选和放置演示。对于所有演示，您应该启动基本环境：

ros2 launch moveit_task_constructor_demo demo.launch.py

随后，您可以运行各个演示：

ros2 launch moveit_task_constructor_demo cartesian.launch.py
ros2 launch moveit_task_constructor_demo modular.launch.py
ros2 launch moveit_task_constructor_demo pickplace.launch.py

cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ source install/setup.bash
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch moveit_task_constructor_demo cartesian.launch.py

// 运行前 除之前的任务。rviz左下角 点击 Reset 按钮。
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch moveit_task_constructor_demo modular.launch.py

// 执行错误
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch moveit_task_constructor_demo pickplace.launch.py
……
[pick_place_demo-1] [INFO] [1722493269.128607352] [moveit_task_constructor_demo]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[pick_place_demo-1] [WARN] [1722493269.128675058] [planning_scene_interface_98096004140416.moveit.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[pick_place_demo-1] [INFO] [1722493269.128914802] [moveit_task_constructor_demo]: Planning succeded
[pick_place_demo-1] [INFO] [1722493269.128933682] [moveit_task_constructor_demo]: Execution disabled

在右侧，您应该会看到运动规划任务面板，概述任务的分层阶段结构。当您选择特定阶段时，成功和失败的解决方案列表将显示在最右侧的窗口中。选择其中一个解决方案将开始其可视化。

4 使用 MoveIt 任务构造器设置项目 

本节介绍了使用 MoveIt 任务构造器构建简单任务所需的步骤。

4.1 创建一个新包 

使用以下命令创建一个新包：

ros2 pkg create \
--build-type ament_cmake \
--dependencies moveit_task_constructor_core rclcpp \
--node-name mtc_node mtc_tutorial

这将创建一个名为 mtc_tutorial 的新包和文件夹，并依赖于 moveit_task_constructor_core ，以及在 src/mtc_node 中的 hello world 示例。

 4.2 代码 

在您选择的编辑器中打开 mtc_node.cpp ，并粘贴以下代码。

#include <rclcpp/rclcpp.hpp> // 引入ROS 2的C++客户端库
#include <moveit/planning_scene/planning_scene.h> // 引入MoveIt的规划场景库
#include <moveit/planning_scene_interface/planning_scene_interface.h> // 引入规划场景接口库
#include <moveit/task_constructor/task.h> // 引入MoveIt任务构造器的task库
#include <moveit/task_constructor/solvers.h> // 引入任务构造器的求解器库
#include <moveit/task_constructor/stages.h> // 引入任务构造器的阶段库
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>) // 条件编译，检查是否存在tf2_geometry_msgs库
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> // 引入tf2_geometry_msgs库
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> // 如果不存在，使用.h后缀
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>) // 条件编译，检查是否存在tf2_eigen库
#include <tf2_eigen/tf2_eigen.hpp> // 引入tf2_eigen库
#else
#include <tf2_eigen/tf2_eigen.h> // 如果不存在，使用.h后缀
#endifstatic const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial"); // 定义一个ROS日志记录器
namespace mtc = moveit::task_constructor; // 为moveit::task_constructor命名空间创建别名class MTCTaskNode
{
public:// 构造函数，接受节点选项作为参数MTCTaskNode(const rclcpp::NodeOptions& options);// 获取节点的基本接口rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();// 执行任务void doTask();// 设置规划场景void setupPlanningScene();private:// 创建并配置MTC任务mtc::Task createTask(); // 从一系列阶段组成一个MTC任务mtc::Task task_; // MTC任务对象rclcpp::Node::SharedPtr node_; // ROS 2节点指针
};// 构造函数实现
MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options): node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }// 初始化节点
{
}// 获取节点的基本接口实现
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{return node_->get_node_base_interface();
}// 设置规划场景
void MTCTaskNode::setupPlanningScene()
{// 创建碰撞对象moveit_msgs::msg::CollisionObject object;object.id = "object"; // 对象IDobject.header.frame_id = "world";  // 设置对象的参考坐标系object.primitives.resize(1); // 调整图元数组大小object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; // 设置图元类型为圆柱体object.primitives[0].dimensions = { 0.1, 0.02 }; // 圆柱的尺寸（高度0.1米，直径0.02米）// 设置碰撞对象的位姿geometry_msgs::msg::Pose pose; // 创建位姿对象pose.position.x = 0.5; // 设置位姿的x坐标pose.position.y = -0.25; // 设置位姿的y坐标pose.orientation.w = 1.0; // 设置位姿的四元数wobject.pose = pose; // 将位姿赋值给碰撞对象// 应用碰撞对象到规划场景moveit::planning_interface::PlanningSceneInterface psi;// 创建规划场景接口对象psi.applyCollisionObject(object); // 将碰撞对象应用到规划场景中
}// 执行任务
void MTCTaskNode::doTask()
{// 创建并初始化任务task_ = createTask();// 创建任务try{task_.init();// 初始化任务}catch (mtc::InitStageException& e){// 捕获初始化异常并记录错误RCLCPP_ERROR_STREAM(LOGGER, e);// 捕获初始化阶段异常并记录错误return;}// 规划任务if (!task_.plan(5)) // 规划任务，最多尝试5次{// 记录任务规划失败的错误RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");return;}// 发布解决方案以供调试task_.introspection().publishSolution(*task_.solutions().front());// 执行任务并检查结果auto result = task_.execute(*task_.solutions().front());// 执行任务if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS){// 记录任务执行失败的错误RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");return;}return;
}// 创建任务
mtc::Task MTCTaskNode::createTask()
{mtc::Task task; // 创建任务对象task.stages()->setName("demo task"); // 设置任务名称task.loadRobotModel(node_); // 加载机器人模型// 定义机器人运动组的名称const auto& arm_group_name = "panda_arm"; // 机械臂组名称const auto& hand_group_name = "hand"; // 手部组名称const auto& hand_frame = "panda_hand"; // 手部框架名称// 设置任务的属性task.setProperty("group", arm_group_name);task.setProperty("eef", hand_group_name);task.setProperty("ik_frame", hand_frame);// 禁用未使用变量的警告
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"mtc::Stage* current_state_ptr = nullptr;  // 用于传递当前状态给抓取姿势生成器 // 将current_state转发到抓取位姿生成器
#pragma GCC diagnostic pop// 添加当前状态阶段auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current"); // 创建当前状态阶段current_state_ptr = stage_state_current.get(); // 获取当前状态阶段的指针task.add(std::move(stage_state_current)); // 将当前状态阶段添加到任务中// 定义不同的规划器auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_); // 创建采样规划器auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>(); // 创建插值规划器auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>(); // 创建笛卡尔路径规划器cartesian_planner->setMaxVelocityScalingFactor(1.0); // 设置最大速度缩放因子cartesian_planner->setMaxAccelerationScalingFactor(1.0); // 设置最大加速度缩放因子cartesian_planner->setStepSize(.01); // 设置步长// 创建打开手的阶段auto stage_open_hand =std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner); // 创建打开手部阶段stage_open_hand->setGroup(hand_group_name); // 设置手部组stage_open_hand->setGoal("open"); // 设置目标为打开task.add(std::move(stage_open_hand)); // 将打开手部阶段添加到任务中return task; // 返回任务对象
}// 主函数
int main(int argc, char** argv)
{rclcpp::init(argc, argv); // 初始化ROSrclcpp::NodeOptions options;// 创建节点选项对象options.automatically_declare_parameters_from_overrides(true);// 自动声明覆盖的参数auto mtc_task_node = std::make_shared<MTCTaskNode>(options); // 创建MTC任务节点  // 创建MTCTaskNode对象rclcpp::executors::MultiThreadedExecutor executor; // 创建多线程执行器// 创建线程来运行执行器auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() {executor.add_node(mtc_task_node->getNodeBaseInterface()); // 将节点添加到执行器executor.spin(); // 阻塞调用，执行器开始运行executor.remove_node(mtc_task_node->getNodeBaseInterface()); // 从执行器中移除节点});mtc_task_node->setupPlanningScene(); // 设置规划场景mtc_task_node->doTask(); // 执行任务spin_thread->join(); // 等待线程结束rclcpp::shutdown(); // 关闭ROSreturn 0;
}

 4.3 代码分解 

代码顶部包含此包使用的 ROS 和 MoveIt 库。

• rclcpp/rclcpp.hpp 包含核心 ROS2 功能

• moveit/planning_scene/planning_scene.h 和 moveit/planning_scene_interface/planning_scene_interface.h 包含与机器人模型和碰撞对象接口的功能

• moveit/task_constructor/task.h 、 moveit/task_constructor/solvers.h 和 moveit/task_constructor/stages.h 包含示例中使用的 MoveIt 任务构造器的不同组件

• tf2_geometry_msgs/tf2_geometry_msgs.hpp 和 tf2_eigen/tf2_eigen.hpp 不会在这个初始示例中使用，但在我们为 MoveIt 任务构造器任务添加更多阶段时，它们将用于位姿生成。

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif

下一行获取我们新节点的记录器。我们还为 moveit::task_constructor 创建了一个命名空间别名以方便使用。

static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
namespace mtc = moveit::task_constructor;

我们首先定义一个包含主要 MoveIt 任务构造器功能的类。我们还将 MoveIt 任务构造器任务对象声明为我们类的成员变量：这对于给定的应用程序来说并不是绝对必要的，但它有助于保存任务以便以后进行可视化。我们将在下面分别探讨每个功能。

class MTCTaskNode
{
public:MTCTaskNode(const rclcpp::NodeOptions& options);rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();void doTask();void setupPlanningScene();private:// Compose an MTC task from a series of stages.mtc::Task createTask();mtc::Task task_;rclcpp::Node::SharedPtr node_;
};

这些行使用指定的选项初始化节点（这是我们 MTCTaskNode 类的构造函数）。

MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options): node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
{
}

接下来的几行定义了一个 getter 函数来获取节点基础接口，该接口将在稍后用于执行器。

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{return node_->get_node_base_interface();
}

此类方法用于设置示例中使用的规划场景。它创建一个由 object.primitives[0].dimensions 指定尺寸和由 pose.position.x 和 pose.position.y 指定位置的圆柱体。您可以尝试更改这些数字以调整圆柱体的大小和移动位置。如果我们将圆柱体移出机器人的可达范围，规划将失败。

void MTCTaskNode::setupPlanningScene()
{moveit_msgs::msg::CollisionObject object;object.id = "object";object.header.frame_id = "world";object.primitives.resize(1);object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;object.primitives[0].dimensions = { 0.1, 0.02 };geometry_msgs::msg::Pose pose;pose.position.x = 0.5;pose.position.y = -0.25;object.pose = pose;moveit::planning_interface::PlanningSceneInterface psi;psi.applyCollisionObject(object);
}

此函数与 MoveIt 任务构造器任务对象接口。它首先创建一个任务，其中包括设置一些属性和添加阶段。这将在 createTask 函数定义中进一步讨论。接下来， task.init() 初始化任务并 task.plan(5) 生成计划，在找到 5 个成功计划后停止。下一行发布解决方案以在 RViz 中可视化 - 如果您不关心可视化，可以删除此行。最后， task.execute() 执行计划。执行通过与 RViz 插件的操作服务器接口进行。

void MTCTaskNode::doTask()
{task_ = createTask();try{task_.init();}catch (mtc::InitStageException& e){RCLCPP_ERROR_STREAM(LOGGER, e);return;}if (!task_.plan(5)){RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");return;}task_.introspection().publishSolution(*task_.solutions().front());auto result = task_.execute(*task_.solutions().front());if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS){RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");return;}return;
}

如上所述，此函数创建一个 MoveIt 任务构造器对象并设置一些初始属性。在这种情况下，我们将任务名称设置为“demo_task”，加载机器人模型，定义一些有用框架的名称，并将这些框架名称设置为任务的属性，使用 task.setProperty(property_name, value) 。接下来的几个代码块将填充此函数体。

mtc::Task MTCTaskNode::createTask()
{moveit::task_constructor::Task task;task.stages()->setName("demo task");task.loadRobotModel(node_);const auto& arm_group_name = "panda_arm";const auto& hand_group_name = "hand";const auto& hand_frame = "panda_hand";// Set task propertiestask.setProperty("group", arm_group_name);task.setProperty("eef", hand_group_name);task.setProperty("ik_frame", hand_frame);

现在，我们向节点添加一个示例阶段。第一行将 current_state_ptr 设置为 nullptr ；这创建了一个指向阶段的指针，以便我们可以在特定场景中重用阶段信息。这行代码目前未使用，但在稍后添加更多阶段到任务时将会使用。接下来，我们创建一个 current_state 阶段（生成器阶段）并将其添加到我们的任务中 - 这使机器人在其当前状态下启动。现在我们已经创建了 CurrentState 阶段，我们将其指针保存在 current_state_ptr 中以备后用。

mtc::Stage* current_state_ptr = nullptr;  // Forward current_state on to grasp pose generator
auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
current_state_ptr = stage_state_current.get();
task.add(std::move(stage_state_current));

求解器用于定义机器人运动类型。MoveIt 任务构造器有三种求解器选项：

PipelinePlanner 使用 MoveIt 的规划管道，通常默认为 OMPL。

auto sampling_planner=std::make_shared<mtc::solvers::PipelinePlanner>(node_);

JointInterpolation 是一个简单的规划器，它在起始和目标关节状态之间进行插值。它通常用于简单的运动，因为它计算速度快，但不支持复杂的运动。

auto interpolation_planner=std::make_shared<mtc::solvers::JointInterpolationPlanner>();

CartesianPath 用于在笛卡尔空间中沿直线移动末端执行器。

auto cartesian_planner=std::make_shared<mtc::solvers::CartesianPath>();

请随意尝试不同的求解器，看看机器人运动如何变化。对于第一阶段，我们将使用笛卡尔规划器，它需要设置以下属性：

auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();
cartesian_planner->setMaxVelocityScalingFactor(1.0);
cartesian_planner->setMaxAccelerationScalingFactor(1.0);
cartesian_planner->setStepSize(.01);

现在我们添加了规划器，我们可以添加一个移动机器人的阶段。以下几行使用 MoveTo 阶段（传播阶段）。由于打开手是一个相对简单的动作，我们可以使用关节插值规划器。此阶段计划移动到“打开手”姿势，这是为 Panda 机器人在 SRDF 中定义的命名姿势。我们返回任务并以 createTask() 函数结束。

auto stage_open_hand =std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);stage_open_hand->setGroup(hand_group_name);stage_open_hand->setGoal("open");task.add(std::move(stage_open_hand));return task;
}

最后，我们有 main ：以下几行使用上面定义的类创建一个节点，并调用类方法来设置和执行基本的 MTC 任务。在这个例子中，我们不会在任务执行完毕后取消执行器，以保持节点活着以便在 RViz 中检查解决方案。

int main(int argc, char** argv)
{rclcpp::init(argc, argv);rclcpp::NodeOptions options;options.automatically_declare_parameters_from_overrides(true);auto mtc_task_node = std::make_shared<MTCTaskNode>(options);rclcpp::executors::MultiThreadedExecutor executor;auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() {executor.add_node(mtc_task_node->getNodeBaseInterface());executor.spin();executor.remove_node(mtc_task_node->getNodeBaseInterface());});mtc_task_node->setupPlanningScene();mtc_task_node->doTask();spin_thread->join();rclcpp::shutdown();return 0;
}

5 运行演示 

 5.1 启动文件 

我们将需要一个启动文件来启动 move_group 、 ros2_control 、 static_tf 、 robot_state_publisher 和 rviz 节点，这些节点为我们提供了运行演示的环境。我们将在此示例中使用的启动文件可以在这里找到。https://github.com/moveit/moveit2_tutorials/blob/main/doc/tutorials/pick_and_place_with_moveit_task_constructor/launch/mtc_demo.launch.py

import os
from launch import LaunchDescription  # 导入LaunchDescription类，用于定义启动文件的描述
from launch.actions import ExecuteProcess  # 导入ExecuteProcess类，用于启动外部进程
from launch_ros.actions import Node  # 导入Node类，用于定义ROS节点
from ament_index_python.packages import get_package_share_directory  # 获取包共享目录
from moveit_configs_utils import MoveItConfigsBuilder  # MoveIt配置生成工具def generate_launch_description():# 配置规划上下文moveit_config = (MoveItConfigsBuilder("moveit_resources_panda")  # 指定机器人资源包.robot_description(file_path="config/panda.urdf.xacro")  # 机器人描述文件路径.trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")  # 轨迹执行控制器配置文件路径.planning_pipelines(pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"])  # 规划管道配置.to_moveit_configs()  # 生成MoveIt配置)# 加载ExecuteTaskSolutionCapability，以便在仿真中执行找到的解决方案move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"}# 启动move_group节点/动作服务器run_move_group_node = Node(package="moveit_ros_move_group",  # 指定包名称executable="move_group",  # 可执行文件名称output="screen",  # 输出到屏幕parameters=[moveit_config.to_dict(),  # 使用MoveIt配置move_group_capabilities,  # Move Group功能],)# RViz 可视化配置rviz_config_file = (get_package_share_directory("moveit2_tutorials") + "/launch/mtc.rviz")  # 获取RViz配置文件路径rviz_node = Node(package="rviz2",  # 指定RViz包名称executable="rviz2",  # 可执行文件名称name="rviz2",  # 节点名称output="log",  # 输出到日志arguments=["-d", rviz_config_file],  # 传递配置文件参数parameters=[moveit_config.robot_description,  # 机器人描述参数moveit_config.robot_description_semantic,  # 语义描述参数moveit_config.robot_description_kinematics,  # 运动学描述参数],)# 静态TF变换static_tf = Node(package="tf2_ros",  # tf2_ros包executable="static_transform_publisher",  # 静态变换发布器name="static_transform_publisher",  # 节点名称output="log",  # 输出到日志arguments=["--frame-id", "world", "--child-frame-id", "panda_link0"],  # 参数：父坐标系、子坐标系)# 发布机器人状态TFrobot_state_publisher = Node(package="robot_state_publisher",  # robot_state_publisher包executable="robot_state_publisher",  # 可执行文件名称name="robot_state_publisher",  # 节点名称output="both",  # 输出到屏幕和日志parameters=[moveit_config.robot_description,  # 机器人描述参数],)# 使用FakeSystem作为硬件的ros2_controlros2_controllers_path = os.path.join(get_package_share_directory("moveit_resources_panda_moveit_config"),"config","ros2_controllers.yaml",)  # 获取ros2控制器配置文件路径ros2_control_node = Node(package="controller_manager",  # 控制器管理包executable="ros2_control_node",  # ros2控制节点parameters=[ros2_controllers_path],  # 配置文件路径参数remappings=[("/controller_manager/robot_description", "/robot_description"),],  # 重映射话题output="both",  # 输出到屏幕和日志)# 加载控制器load_controllers = []for controller in ["panda_arm_controller",  # 机械臂控制器"panda_hand_controller",  # 手爪控制器"joint_state_broadcaster",  # 关节状态广播器]:load_controllers += [ExecuteProcess(cmd=["ros2 run controller_manager spawner {}".format(controller)],shell=True,  # 使用shell执行命令output="screen",  # 输出到屏幕)]return LaunchDescription([rviz_node,  # 添加RViz节点static_tf,  # 添加静态TF节点robot_state_publisher,  # 添加机器人状态发布节点run_move_group_node,  # 添加move_group节点ros2_control_node,  # 添加ros2_control节点]+ load_controllers  # 添加控制器加载命令)

要运行 MoveIt 任务构造器节点，我们将使用第二个启动文件以适当的参数启动 mtc_tutorial 可执行文件。在这里，我们可以加载 URDF、SRDF 和 OMPL 参数，或者使用 MoveIt 配置工具来完成。您的启动文件应类似于此教程包中的文件(https://github.com/moveit/moveit2_tutorials/blob/main/doc/tutorials/pick_and_place_with_moveit_task_constructor/launch/pick_place_demo.launch.py)（请特别注意下面的 package 和 executable 参数，因为它们与链接的启动文件不同）：

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_dict()  # 创建MoveIt配置构建器对象并转换为字典# MTC Demo node MTC演示节点pick_place_demo = Node(package="mtc_tutorial",  # 设置包名为mtc_tutorialexecutable="mtc_node",  # 设置可执行文件名为mtc_nodeoutput="screen",  # 设置输出到屏幕parameters=[moveit_config,  # 设置MoveIt配置参数],)return LaunchDescription([pick_place_demo])  # 返回包含pick_place_demo节点的启动描述

将启动文件保存为 pick_place_demo.launch.py 或下载到包的启动目录。确保编辑 CMakeLists.txt ，以便通过添加以下行来包含启动文件夹：

install(DIRECTORY launchDESTINATION share/${PROJECT_NAME})

现在我们可以构建和源化 colcon 工作区。

cd ~/ws_moveit
colcon build --mixin release
source ~/ws_moveit/install/setup.bash
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ colcon build --packages-select mtc_tutorial  --mixin release

首先启动第一个启动文件。如果您想使用教程提供的文件：

ros2 launch moveit2_tutorials mtc_demo.launch.py
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch moveit2_tutorials mtc_demo.launch.py

RViz 现在将加载。如果您使用自己的启动文件并且没有包含这样的 rviz 配置，则需要在显示任何内容之前配置 RViz。如果您使用教程包中的启动文件，RViz 将已经为您配置好，您可以跳到下一节的末尾。

 5.2 RViz 配置 

如果您没有使用提供的 RViz 配置，我们将不得不对 RViz 配置进行一些更改，以查看您的机器人和 MoveIt 任务构造器解决方案。首先，启动 RViz。以下步骤将介绍如何设置 RViz 以进行 MoveIt 任务构造器解决方案的可视化。

1. 如果 MotionPlanning 显示处于活动状态，请取消选中以暂时隐藏它。

2. 在全局选项下 Global Options，如果尚未完成，请将固定框架Fixed Frame 从 map 更改为 panda_link0 。

3. 在窗口的左下角，点击添加按钮。

4. 在 moveit_task_constructor_visualization 下选择运动规划任务Motion Planning Tasks并点击确定。运动规划任务显示应出现在左下角。

5. 在显示中，在运动规划任务下，将Task Solution Topic 任务解决方案主题更改为 /solution

您应该在主视图中看到熊猫手臂，左下角打开运动规划任务显示，里面没有任何内容。一旦您启动 mtc_tutorial 节点，您的 MTC 任务将显示在此面板中。如果您使用教程中的 mtc_demo.launch.py ，请返回此处。

5.3 启动演示 

启动 mtc_tutorial 节点与

ros2 launch mtc_tutorial pick_place_demo.launch.py
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch mtc_tutorial pick_place_demo.launch.py

您应该看到机械臂执行任务，单级打开手，前面有绿色的气缸。它应该看起来像这样：

如果你还没有制作自己的包，但仍然想看看它是什么样子，你可以从教程中启动这个文件：

ros2 launch moveit2_tutorials mtc_demo_minimal.launch.py

 6 添加阶段 

到目前为止，我们已经完成了创建和执行一个简单任务的过程，该任务运行但没有做太多事情。现在，我们将开始向任务中添加拾取和放置阶段。下图显示了我们将在任务中使用的各个阶段的概述。

我们将在现有的开放手阶段之后开始添加阶段。打开 mtc_node.cpp 并找到以下几行：

auto stage_open_hand =std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);  // 创建打开手部阶段// clang-format onstage_open_hand->setGroup(hand_group_name);  // 设置手部组stage_open_hand->setGoal("open");  // 设置目标为打开task.add(std::move(stage_open_hand));  // 添加打开手部阶段到任务

6.1 拣选阶段

我们需要将手臂移动到可以拾取物体的位置。这是通过一个 Connect 阶段完成的，顾名思义，这是一个连接器阶段。这意味着它试图在前后阶段的结果之间架起桥梁。这个阶段用一个名称 move_to_pick 和一个指定规划组和规划器的 GroupPlannerVector 初始化。然后我们为该阶段设置超时，设置阶段的属性，并将其添加到我们的任务中。

auto stage_move_to_pick = std::make_unique<mtc::stages::Connect>("move to pick",mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner } });  // 创建移动到抓取位置阶段// clang-format onstage_move_to_pick->setTimeout(5.0);  // 设置超时时间为 5 秒stage_move_to_pick->properties().configureInitFrom(mtc::Stage::PARENT);  // 从父阶段初始化属性task.add(std::move(stage_move_to_pick));  // 添加移动到抓取位置阶段到任务

接下来，我们创建一个指向 MoveIt 任务构造器阶段对象的指针，并将其暂时设置为 nullptr 。稍后，我们将使用它来保存一个阶段。

mtc::Stage* attach_object_stage =nullptr;  // Forward attach_object_stage to place pose generator

这段代码创建了一个 SerialContainer 。这是一个可以添加到我们任务中的容器，可以包含多个子阶段。在这种情况下，我们创建了一个串行容器，其中包含与拣选操作相关的阶段。我们不会将阶段添加到任务中，而是将相关阶段添加到串行容器中。我们使用 exposeTo() 从父任务中声明新串行容器的任务属性，并使用 configureInitFrom() 初始化它们。这允许包含的阶段访问这些属性。

{auto grasp = std::make_unique<mtc::SerialContainer>("pick object");  // 创建串行容器阶段task.properties().exposeTo(grasp->properties(), { "eef", "group", "ik_frame" });  // 公开任务属性到串行容器// clang-format offgrasp->properties().configureInitFrom(mtc::Stage::PARENT,{ "eef", "group", "ik_frame" });  // 从父阶段初始化属性

然后我们创建一个阶段来接近对象。这个阶段是一个 MoveRelative 阶段，它允许我们指定从当前位置的相对移动。 MoveRelative 是一个传播阶段：它接收来自相邻阶段的解决方案并将其传播到下一个或上一个阶段。使用 cartesian_planner 找到一个涉及以直线移动末端执行器的解决方案。我们设置属性，并设置移动的最小和最大距离。现在我们创建一个 Vector3Stamped 消息来指示我们想要移动的方向 - 在这种情况下，从手框架的 Z 方向。最后，我们将这个阶段添加到我们的串行容器中。

{// clang-format offauto stage =std::make_unique<mtc::stages::MoveRelative>("approach object", cartesian_planner);  // 创建相对移动阶段// clang-format onstage->properties().set("marker_ns", "approach_object");  // 设置标记命名空间stage->properties().set("link", hand_frame);  // 设置链接为手部参考坐标系stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });  // 从父阶段初始化属性stage->setMinMaxDistance(0.1, 0.15);  // 设置最小和最大距离// 设置手部前进方向geometry_msgs::msg::Vector3Stamped vec;vec.header.frame_id = hand_frame;  // 设置参考坐标系vec.vector.z = 1.0;  // 设置方向向量stage->setDirection(vec);  // 设置方向grasp->insert(std::move(stage));  // 插入阶段到串行容器}

现在，创建一个阶段来生成抓取姿势。这是一个生成器阶段，因此它在计算结果时不考虑之前和之后的阶段。第一个阶段， CurrentState 也是一个生成器阶段 - 要连接第一个阶段和这个阶段，必须使用一个连接阶段，我们已经在上面创建了这个连接阶段。此代码设置阶段属性，设置抓取前的位姿、角度增量和监控阶段。角度增量是 GenerateGraspPose 阶段的一个属性，用于确定要生成的位姿数量；在生成解决方案时，MoveIt 任务构造器将尝试从许多不同的方向抓取对象，方向之间的差异由角度增量指定。增量越小，抓取方向之间的距离就越近。在定义当前阶段时，我们设置了 current_state_ptr ，现在用于将有关对象姿势和形状的信息转发给逆运动学求解器。这个阶段不会像以前那样直接添加到串行容器中，因为我们仍然需要对它生成的姿势进行逆运动学计算。

{// 采样抓取姿态auto stage = std::make_unique<mtc::stages::GenerateGraspPose>("generate grasp pose");  // 创建生成抓取姿态阶段stage->properties().configureInitFrom(mtc::Stage::PARENT);  // 从父阶段初始化属性stage->properties().set("marker_ns", "grasp_pose");  // 设置标记命名空间stage->setPreGraspPose("open");  // 设置预抓取姿态stage->setObject("object");  // 设置对象stage->setAngleDelta(M_PI / 12);  // 设置角度增量stage->setMonitoredStage(current_state_ptr);  // 监控当前状态阶段  将有关对象姿势和形状的信息转发给逆运动学求解器。

在计算上述生成的姿态的逆运动学之前，我们首先需要定义框架。这可以通过来自 PoseStamped 的消息 geometry_msgs 来完成，或者在这种情况下，我们使用 Eigen 变换矩阵和相关连杆的名称来定义变换。在这里，我们定义了变换矩阵。

// 这是从对象框架到末端执行器框架的变换Eigen::Isometry3d grasp_frame_transform;Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) *Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());  // 设置四元数grasp_frame_transform.linear() = q.matrix();  // 设置线性变换grasp_frame_transform.translation().z() = 0.1;  // 设置平移变换

现在，我们创建 ComputeIK 阶段，并将其命名为 generate pose IK 以及上面定义的 generate grasp pose 阶段。某些机器人对于给定的姿态有多个逆运动学解 - 我们将解决方案的数量限制为最多 8 个。我们还设置了最小解距离，这是对不同解决方案必须有多大差异的阈值：如果一个解中的关节位置与之前的解太相似，它将被标记为无效。接下来，我们配置一些附加属性，并将 ComputeIK 阶段添加到串行容器中。

// 计算 IK// clang-format offauto wrapper =std::make_unique<mtc::stages::ComputeIK>("grasp pose IK", std::move(stage));  // 创建计算 IK 阶段// clang-format onwrapper->setMaxIKSolutions(8);  // 设置最大 IK 解数量wrapper->setMinSolutionDistance(1.0);  // 设置最小解距离wrapper->setIKFrame(grasp_frame_transform, hand_frame);  // 设置 IK 框架wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });  // 从父阶段初始化属性wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });  // 从接口初始化属性grasp->insert(std::move(wrapper));  // 插入阶段到串行容器}

要拾取物体，我们必须允许手和物体之间发生碰撞。这可以通过 ModifyPlanningScene 阶段完成。 allowCollisions 函数允许我们指定要禁用的碰撞。 allowCollisions 可以与名称容器一起使用，因此我们可以使用 getLinkModelNamesWithCollisionGeometry 获取手组中具有碰撞几何体的所有链接的名称。

{// clang-format offauto stage =std::make_unique<mtc::stages::ModifyPlanningScene>("allow collision (hand,object)");  // 创建修改规划场景阶段stage->allowCollisions("object",task.getRobotModel()->getJointModelGroup(hand_group_name)->getLinkModelNamesWithCollisionGeometry(),true);  // 允许手部和对象碰撞// clang-format ongrasp->insert(std::move(stage));  // 插入阶段到串行容器}

允许碰撞后，我们现在可以关闭手。这是通过 MoveTo 阶段完成的，类似于上面的 open hand 阶段，只是移动到 SRDF 中定义的 close 位置。

{auto stage = std::make_unique<mtc::stages::MoveTo>("close hand", interpolation_planner);  // 创建关闭手部阶段stage->setGroup(hand_group_name);  // 设置手部组stage->setGoal("close");  // 设置目标为关闭grasp->insert(std::move(stage));  // 插入阶段到串行容器}

我们现在再次使用 ModifyPlanningScene 阶段，这次使用 attachObject 将对象附加到手上。与我们对 current_state_ptr 所做的类似，我们获得了该阶段的指针，以便在生成对象的放置姿势时使用。

{auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attach object");  // 创建附加对象阶段stage->attachObject("object", hand_frame);  // 附加对象到手部attach_object_stage = stage.get();  // 获取附加对象阶段指针  以便在生成对象的放置姿势时使用。grasp->insert(std::move(stage));  // 插入阶段到串行容器}

接下来，我们用 MoveRelative 阶段提升物体，类似于 approach_object 阶段。

{// clang-format offauto stage =std::make_unique<mtc::stages::MoveRelative>("lift object", cartesian_planner);  // 创建提升对象阶段// clang-format onstage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });  // 从父阶段初始化属性stage->setMinMaxDistance(0.1, 0.3);  // 设置最小和最大距离stage->setIKFrame(hand_frame);  // 设置 IK 框架stage->properties().set("marker_ns", "lift_object");  // 设置标记命名空间// 设置向上方向geometry_msgs::msg::Vector3Stamped vec;vec.header.frame_id = "world";  // 设置参考坐标系vec.vector.z = 1.0;  // 设置方向向量stage->setDirection(vec);  // 设置方向grasp->insert(std::move(stage));  // 插入阶段到串行容器}

有了这个，我们就有了拾取物体所需的所有阶段。现在，我们将串行容器（及其所有子阶段）添加到任务中。如果按原样构建包，您可以看到机器人计划拾取物体。

task.add(std::move(grasp));  // 添加串行容器到任务}

要测试新创建的阶段，请构建代码并执行：

ros2 launch mtc_tutorial pick_place_demo.launch.py

 6.2 放置阶段 

现在定义拾取的阶段已经完成，我们继续定义放置物体的阶段。接着我们之前的工作，我们添加一个 Connect 阶段来连接两者，因为我们很快将使用生成器阶段来生成放置物体的姿势。

{// clang-format offauto stage_move_to_place = std::make_unique<mtc::stages::Connect>("move to place",mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner },{ hand_group_name, interpolation_planner } });  // 创建移动到放置位置阶段// clang-format onstage_move_to_place->setTimeout(5.0);  // 设置超时时间为 5 秒stage_move_to_place->properties().configureInitFrom(mtc::Stage::PARENT);  // 从父阶段初始化属性task.add(std::move(stage_move_to_place));  // 添加移动到放置位置阶段到任务}

我们还为放置阶段创建了一个串行容器。这与选择串行容器类似。接下来的阶段将被添加到串行容器中，而不是任务中。

{auto place = std::make_unique<mtc::SerialContainer>("place object");  // 创建放置对象串行容器task.properties().exposeTo(place->properties(), { "eef", "group", "ik_frame" });  // 公开任务属性到串行容器// clang-format offplace->properties().configureInitFrom(mtc::Stage::PARENT,{ "eef", "group", "ik_frame" });  // 从父阶段初始化属性

此下一个阶段生成用于放置对象的姿势并计算这些姿势的逆运动学 - 它与从拾取串行容器的 generate grasp pose 阶段有些相似。我们首先创建一个阶段来生成姿势并继承任务属性。我们使用来自 geometry_msgs 的 PoseStamped 消息指定我们要放置对象的姿势 - 在这种情况下，我们选择 "object" 框架中的 y = 0.5 。然后，我们将目标姿势传递给带有 setPose 的阶段。接下来，我们使用 setMonitoredStage 并将其指针传递给早期的 attach_object 阶段。这允许阶段知道对象是如何附加的。然后我们创建一个 ComputeIK 阶段并传递我们的 GeneratePlacePose 阶段 - 其余的逻辑与上面的拾取阶段相同。

{// 采样放置姿态auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose");  // 创建生成放置姿态阶段stage->properties().configureInitFrom(mtc::Stage::PARENT);  // 从父阶段初始化属性stage->properties().set("marker_ns", "place_pose");  // 设置标记命名空间stage->setObject("object");  // 设置对象geometry_msgs::msg::PoseStamped target_pose_msg;target_pose_msg.header.frame_id = "object";  // 设置参考坐标系target_pose_msg.pose.position.y = 0.5;  // 设置 y 坐标target_pose_msg.pose.orientation.w = 1.0;  // 设置方向stage->setPose(target_pose_msg);  // 设置姿态stage->setMonitoredStage(attach_object_stage);  // 监控附加对象阶段// 计算 IK// clang-format offauto wrapper =std::make_unique<mtc::stages::ComputeIK>("place pose IK", std::move(stage));  // 创建计算 IK 阶段// clang-format onwrapper->setMaxIKSolutions(2);  // 设置最大 IK 解数量wrapper->setMinSolutionDistance(1.0);  // 设置最小解距离wrapper->setIKFrame("object");  // 设置 IK 框架wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });  // 从父阶段初始化属性wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });  // 从接口初始化属性place->insert(std::move(wrapper));  // 插入阶段到串行容器}

现在我们准备放置对象，我们用 MoveTo 阶段和关节插值规划器打开手。

{auto stage = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);  // 创建打开手部阶段stage->setGroup(hand_group_name);  // 设置手部组stage->setGoal("open");  // 设置目标为打开place->insert(std::move(stage));  // 插入阶段到串行容器}

我们也可以重新启用与对象的碰撞，因为我们不再需要持有它。这是使用 allowCollisions 完成的，几乎与禁用碰撞的方式完全相同，只是将最后一个参数设置为 false 而不是 true 。

{// clang-format offauto stage =std::make_unique<mtc::stages::ModifyPlanningScene>("forbid collision (hand,object)");  // 创建一个ModifyPlanningScene对象，命名为“forbid collision (hand,object)”stage->allowCollisions("object",task.getRobotModel()->getJointModelGroup(hand_group_name)->getLinkModelNamesWithCollisionGeometry(),false);  // 禁止“object”与手部关节组中的所有带有碰撞几何体的链接发生碰撞// clang-format onplace->insert(std::move(stage));  // 将stage插入到place中
}

现在，我们可以使用 detachObject 分离对象。

{// clang-format offauto stage =std::make_unique<mtc::stages::ModifyPlanningScene>("detach object");  // 创建分离对象阶段// clang-format onstage->detachObject("object", hand_frame);  // 分离对象place->insert(std::move(stage));  // 插入阶段到串行容器}

我们使用 MoveRelative 阶段从对象撤退，这与 approach object 和 lift object 阶段类似。

{auto stage = std::make_unique<mtc::stages::MoveTo>("retreat after place", cartesian_planner);  // 创建放置后撤退阶段stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });  // 从父阶段初始化属性stage->setMinMaxDistance(0.1, 0.25);  // 设置最小和最大距离stage->setIKFrame(hand_frame);  // 设置 IK 框架stage->properties().set("marker_ns", "retreat_after_place");  // 设置标记命名空间// 设置撤退方向geometry_msgs::msg::Vector3Stamped vec;vec.header.frame_id = "world";  // 设置参考坐标系vec.vector.z = -1.0;  // 设置方向向量stage->setDirection(vec);  // 设置方向place->insert(std::move(stage));  // 插入阶段到串行容器}

我们完成了我们的序列容器并将其添加到任务中。

task.add(std::move(place));
}

最后一步是返回home：我们使用一个 MoveTo 阶段并传递 ready 的目标姿态，这是在 Panda SRDF 中定义的姿态。

{auto stage = std::make_unique<mtc::stages::MoveTo>("return home", interpolation_planner);  // 创建一个MoveTo对象，命名为“return home”，使用插值规划器stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });  // 从父级配置初始化属性，设置“group”stage->setGoal("ready");  // 设置目标位置为“ready”task.add(std::move(stage));  // 将stage插入到task中
}

所有这些阶段应添加到这些行上方。

// Stages all added to the task above this linereturn task;
}

恭喜！您现在已经使用 MoveIt 任务构造器定义了一个拾取和放置任务！要尝试一下，请构建代码并执行：

ros2 launch mtc_tutorial pick_place_demo.launch.py

 7 进一步讨论 

每个包含阶段的任务显示在运动规划任务窗格中。点击一个阶段，关于该阶段的附加信息将显示在右侧。右侧窗格显示不同的解决方案及其相关成本。根据阶段类型和机器人配置，可能只显示一个解决方案。

点击其中一个解决方案成本以查看机器人按照该阶段计划执行的动画。点击窗格右上角的“执行”按钮以执行动作。

要运行包含在 MoveIt 教程中的完整 MoveIt 任务构造器示例：

ros2 launch moveit2_tutorials mtc_demo.launch.py
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch moveit2_tutorials mtc_demo.launch.py

并在第二个终端中：

ros2 launch moveit2_tutorials pick_place_demo.launch.py
cxy@cxy-Ubuntu2404:~/moveit_tutorials_ws$ ros2 launch mtc_tutorial pick_place_demo.launch.py

7.1 打印到终端的调试信息 

运行 MTC 时，它会在终端打印出这样的图表：

[demo_node-1]     1  - ←   1 →   -  0 / initial_state
[demo_node-1]     -  0 →   0 →   -  0 / move_to_home

此示例^显示了两个阶段。第一个阶段（“initial_state”）是 CurrentState 类型的阶段，它初始化 PlanningScene 并捕获当时存在的任何碰撞对象。可以使用指向此阶段的指针来检索机器人的状态。由于 CurrentState 继承自 Generator ，它向前和向后传播解决方案。这由两个方向的箭头表示。

• 第一个 1 表示一个解决方案已成功向后传播到前一个阶段。

• 第二个 1 ，在箭头之间，表示生成了一个解决方案。

• 0 表示解决方案未能成功传播到下一阶段，因为下一阶段失败了。

第二阶段（“move_to_home”）是一个 MoveTo 类型的阶段。它继承了前一个阶段的传播方向，所以两个箭头都指向前方。 0 表示这个阶段完全失败。从左到右， 0 的意思是：

• 阶段未从上一个阶段收到解决方案

• 舞台没有生成解决方案

• 阶段未将解决方案传播到下一个阶段

在这种情况下，我们可以确定“move_to_home”是故障的根本原因。问题在于一个处于碰撞状态的 home。定义一个新的、无碰撞的 home 位置解决了这个问题。

 7.2 阶段 

有关各个阶段的信息可以从任务中检索。例如，在这里我们检索一个阶段的唯一 ID：

uint32_t const unique_stage_id = task_.stages()->findChild(stage_name)->introspectionId();

一种 CurrentState 类型的阶段不仅仅是检索机器人的当前状态。它还初始化一个 PlanningScene 对象，捕获当时存在的任何碰撞对象。

MTC 阶段可以正向和反向传播。您可以通过 RViz GUI 中的箭头轻松检查阶段传播的方向。反向传播时，许多操作的逻辑是相反的。例如，要在 ModifyPlanningScene 阶段允许与对象碰撞，您需要调用 allowCollisions(false) 而不是 allowCollisions(true) 。这里有一个讨论可以阅读。

附录：完整代码

这段代码是一个使用ROS 2和MoveIt!的C++程序，主要功能是通过任务构造器（MoveIt! Task Constructor）实现机器人抓取和放置物体的任务。以下是代码的总结：

1. 包含头文件：

• 包含ROS 2的C++客户端库、MoveIt!的规划场景和任务构造器相关的头文件，以及tf2的几何消息和Eigen头文件。

定义日志记录器和命名空间别名：

• 定义一个日志记录器LOGGER和一个命名空间别名mtc。

定义MTCTaskNode类：

• 包含构造函数、获取节点基础接口、执行任务和设置规划场景的成员函数。

• 私有成员函数createTask用于创建任务。

构造函数：

• 初始化节点对象。

setupPlanningScene函数：

• 设置规划场景，定义一个碰撞对象并应用到规划场景中。

doTask函数：

• 创建任务并初始化。

• 规划任务并发布解决方案。

• 执行任务解决方案。

createTask函数：

• 创建任务对象并设置任务属性。

• 添加各个阶段到任务中，包括当前状态、打开手部、移动到抓取位置、生成抓取姿态、附加对象、提升对象、移动到放置位置、生成放置姿态、分离对象和返回初始位置等阶段。

主函数：

• 初始化rclcpp库，创建节点选项对象和MTCTaskNode对象。

• 创建多线程执行器对象并启动一个新的线程。

• 设置规划场景并执行任务。

• 等待线程结束并关闭rclcpp库。

这段代码展示了如何使用MoveIt!任务构造器实现一个简单的机器人抓取和放置任务。

#include <rclcpp/rclcpp.hpp>  // 包含 ROS 2 的 C++ 客户端库
#include <moveit/planning_scene/planning_scene.h>  // 包含 MoveIt! 的规划场景头文件
#include <moveit/planning_scene_interface/planning_scene_interface.h>  // 包含 MoveIt! 的规划场景接口头文件
#include <moveit/task_constructor/task.h>  // 包含 MoveIt! 任务构造器的任务头文件
#include <moveit/task_constructor/solvers.h>  // 包含 MoveIt! 任务构造器的求解器头文件
#include <moveit/task_constructor/stages.h>  // 包含 MoveIt! 任务构造器的阶段头文件
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>  // 包含 tf2 的几何消息头文件（如果存在）
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>  // 包含 tf2 的几何消息头文件（备用）
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>  // 包含 tf2 的 Eigen 头文件（如果存在）
#else
#include <tf2_eigen/tf2_eigen.h>  // 包含 tf2 的 Eigen 头文件（备用）
#endif