MoveIt2-humble----Planning Around Objects(a)

4.Pick and Place with MoveIt Task Constructor

本节教程会教你创建一个功能包，使用MoveIt Task Constructor规划一个抓取和放置的操作。MoveIt Task Constructor 提供了一种方式，去规划由多种不同子任务（也称为阶段）所组成的任务。

创建新功能包

ros2 pkg create --build-type ament_cmake --node-name mtc_tutorial mtc_tutorial

这会创建一个名为mtc_tutorial的新文件夹,下一步，在package.xml添加依赖，像这样：

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>mtc_tutorial</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="youremail@domain.com">user</maintainer>
<license>TODO: License declaration</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>moveit_task_constructor_core</depend>
<depend>rclcpp</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
    <build_type>ament_cmake</build_type>
</export>
</package>

同样，也要在CMakeLists.txt中添加依赖：

cmake_minimum_required(VERSION 3.8)
project(mtc_tutorial)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(moveit_task_constructor_core REQUIRED)
find_package(rclcpp REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

add_executable(mtc_tutorial src/mtc_tutorial.cpp)
ament_target_dependencies(mtc_tutorial moveit_task_constructor_core rclcpp)
target_include_directories(mtc_tutorial PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(mtc_tutorial PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

install(TARGETS mtc_tutorial
DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()

使用 MoveIt Task Constructor 建立程序

本章节教你使用 MoveIt Task Constructor 建立一个小任务

The Code

打开mtc_tutorial.cpp，添加以下代码：

#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

static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
namespace mtc = 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(); // 实例化 Task 对象
  mtc::Task task_; // 声明 Task 类指针
  rclcpp::Node::SharedPtr node_; // 声明共享指针->node
};
// 获取节点的基本接口，用于后面的计算
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}
// 构造函数，同时初始化其成员变量 node_
MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
{
}
// 建立规划场景
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);
}
// 任务执行函数
void MTCTaskNode::doTask()
{
  // 令 task_ 指向 createTask 类对象
  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;
}

mtc::Task MTCTaskNode::createTask()
{
  // 创建 task 对象
  mtc::Task task;
  task.stages()->setName("demo task"); // 设置 task 的名称
  task.loadRobotModel(node_); // 从给定节点中获取机械臂模型

  const auto& arm_group_name = "panda_arm";
  const auto& hand_group_name = "hand";
  const auto& hand_frame = "panda_hand";

  // 设置 task 属性
  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;  // Forward current_state on to grasp pose generator
#pragma GCC diagnostic pop
  // 创建一个 CurrentState 阶段
  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->setMaxVelocityScaling(1.0);
  cartesian_planner->setMaxAccelerationScaling(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)
{
  // 初始化 ROS
  rclcpp::init(argc, argv);
  // 实例化 node_options 并设置其参数
  rclcpp::NodeOptions options;
  options.automatically_declare_parameters_from_overrides(true);
  // 创建节点
  auto mtc_task_node = std::make_shared<MTCTaskNode>(options);
  // 声明一个多线程执行器
  rclcpp::executors::MultiThreadedExecutor executor;
  // 实例化一个线程对象，使用 lambda表达式来构造，在此线程中加入执行器
  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;
}

代码分析

代码顶部 include 了本章使用的 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 Task Constructor 中的不同成分
tf2_geometry_msgs/tf2_geometry_msgs.hpp 和tf2_eigen/tf2_eigen.hpp 不会在这个初始例子中使用，它们在我们向 MoveIt Task Constructor task 添加更多步骤时，用来位姿生成。

下一行取得新节点的日志记录器（logger），并为moveit::task_constructor创建一个命名空间别名mtc，以方便我们使用。

#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

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

首先，定义一个类来包含主要的 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:
  // Compose an MTC task from a series of stages.
  mtc::Task createTask();
  mtc::Task task_;
  rclcpp::Node::SharedPtr node_;
};

下面的代码定义了一个函数去获取节点的基本接口，用于后面的计算。

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

下面的代码使用指定的选项来初始化节点：

注：这里用到了构造函数初始化列表的内容，即在初始化时也同时初始化了其成员对象node_，从而完成了初始化节点mtc_node的操作。

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

下面这个方法被用来去建立例子中用到的规划场景，它创建了一个圆柱，该圆柱由object.primitives[0].dimensions来指定尺寸，由pose.position.z和pose.position.x来指定位置。你可以尝试改变这些参数来更改尺寸或移动圆柱，如果将圆柱移动到机械臂工作空间外，则规划会报错。

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 Task Constructor 任务对象连接。首先创建了一个任务，包括设置属性和添加阶段，这将在后面定义的createTask函数中进行讨论。

其次，task.init()初始化了这个任务，task.plan(5)生成了一个规划，在找到5个成功的规划后停止。

然后，发布要在 RViz 中实现可视化的解决方案，如果不在意可视化可以删掉。

最后，task.execute()用来执行规划，这个执行动作通过 RViz 插件中的一个 action server 接口来实现。

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 Task Construcor 对象并设置了一些初始属性。在这种情况下，我们设置任务的名称为“demo_task”，加载机械臂模型，定义了一些用到的坐标系名，并通过task.setProperty(property_name, value)将这些坐标系名称设置为任务的属性。

mtc::Task MTCTaskNode::createTask()
{
  // 创建 task 对象
  mtc::Task task;
  task.stages()->setName("demo task"); // 设置 task 的名称
  task.loadRobotModel(node_); // 从给定节点中获取机械臂模型

  const auto& arm_group_name = "panda_arm";
  const auto& hand_group_name = "hand";
  const auto& hand_frame = "panda_hand";

  // 设置 task 属性
  task.setProperty("group", arm_group_name);
  task.setProperty("eef", hand_group_name);
  task.setProperty("ik_frame", hand_frame);

现在，我们添加一个示例阶段到节点中。第一行创建了一个指向 Stage 类型的指针current_state_ptr，将其设置为nullptr；这样我们就能在特定场景中复用 Stage 信息。（现在我们没有用到这个功能，但是在之后有更多的阶段添加到任务中时会用到。）

现在，我们创建一个current_state阶段并将其添加到我们的任务中——这使机械臂以当前状态启动。

现在我们已经创建了CurrentState阶段（stage_state_current），我们令current_state_ptr指向stage_state_current的指针，以便后续使用。

// 取消这行的警告, 因为在本例中这个变量没有使用
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
  mtc::Stage* current_state_ptr = nullptr;  // Forward current_state on to grasp pose generator
#pragma GCC diagnostic pop
// 创建智能指针 stage_state_current ，其指向对象的类型为CurrentState
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));

为了去规划所有的机械臂动作，我们需要去指定一个 solver，对于 solver，MoveIt Task Constructor 有三个选项：

PipelinePlanner使用了 MoveIt 的规划管道（planning pipeline），该管道通常默认为 OMPL；
CartesianPath被用来在笛卡尔空间中移动末端执行器走直线；
JointInterpolation是一个简单的规划器，它在起始和目标关节状态中进行插补。因为其计算很快，通常用于简单的运动，但是不支持复杂的运动。

我们也设置一些属性指定给 Cartesian planner

// 三种规划方式
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->setMaxVelocityScaling(1.0);
cartesian_planner->setMaxAccelerationScaling(1.0);
cartesian_planner->setStepSize(.01);

现在我们已经添加了规划器，我们可以加入一个移动机械臂的阶段。下面的代码使用了一个MoveTo阶段（这是一个广播阶段）。由于打开机械手是一个相对简单的动作，所以我们使用关节插值规划器。这个阶段规划了到“open hand”位姿的运动，这个位姿是一个已经在 panda 机械臂的 SRDF 中命名好的位姿。我们返回任务（return the task）并完成 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)
{
  // 初始化 ROS
  rclcpp::init(argc, argv);
  // 实例化 node_options 并设置其参数
  rclcpp::NodeOptions options;
  options.automatically_declare_parameters_from_overrides(true);
  // 创建节点
  auto mtc_task_node = std::make_shared<MTCTaskNode>(options);
  // 声明一个多线程执行器
  rclcpp::executors::MultiThreadedExecutor executor;
  // 实例化一个线程对象，使用 lambda表达式来构造，在此线程中加入执行器
  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;
}

运行 Demo

我们需要一个 launch 文件去启动 move_group，ros2_control，static_tf，robot_state_publisher和rviz。Here is the launch file we use in the tutorials package。把这个 launch 文件添加到你功能包的 launch 文件夹里，然后在 CMakeLists 中添加：

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

为了运行 MoveIt Task Constructor 节点，我们需要第二个 launch 文件去使用适当的参数来启动 mtc_tutorial。要么加载你的 URDF，SRDF 和 OMPL 参数，要么使用 MoveIt Configs Utils 来实现。你的 launch 文件应该像下面这样：

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

def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_dict()

    # MTC Demo node
    pick_place_demo = Node(
        package="mtc_tutorial",
        executable="mtc_tutorial",
        output="screen",
        parameters=[
            moveit_config,
        ],
    )

    return LaunchDescription([pick_place_demo])

在你功能包的 launch 文件夹中保存此文件，命名为 pick_place_demo.launch.py，然后编译并source你的工作空间。

cd ~/tutorials_ws
colcon build --mixin release
source install/setup.bash

启动第一个 launch 文件，如果你想使用教程中的这个 launch 文件，输入以下指令：

ros2 launch moveit2_tutorials mtc_demo.launch.py

如果你使用自己的 launch 文件，我们需要配置 RViz。如果使用教程的则不用，已经配置好了。

Viz 配置

为了在 RViz 中看见你的机械臂和 MoveIt Task Constructor 结果，我们需要改变一些 RViz 的配置。首先，启动 RViz，下面的步骤将会教你如何去设置 RViz 来让 MoveIt Task Constructor 的结果可视化：

如果 MotionPlanning 显示已经激活，取消选中来暂时隐藏它；
在 Global Options 中，将 Fixed Frame 改为 panda_link0；
在窗口左下角，点击 Add 按钮；
在 moveit_task_constructor_visualization 中选择Motion Planning Tasks 并点击 OK。Motion Planning Tasks 应该在左下角出现显示；
在 Displays 中的 Motion Planning Tasks 下，将 Task Solution Topic 改为/solution。

你应该在主视角中看见 panda 机械臂，以及左下角的 Motion Planning Tasks 的空显示面板。在启动mtc_tutorial节点后，你的 MTC 任务将在这个面板中显示。

启动 Demo

启动你的mtc_tutorial节点：

ros2 launch mtc_tutorial pick_place_demo.launch.py

你将会看见机械臂执行单独阶段的任务——打开机械手，以及显示其面前的绿色圆柱，像这样：

如果你没做你自己的功能包，可以直接打开教程写好的文件来显示：

ros2 launch moveit2_tutorials mtc_demo_minimal.launch.py

添加阶段（Adding Stages）

到现在为止，我们已经创建并执行了一个简单的任务。在后半章节中，我们将开始添加抓取-放置阶段到任务中。