【Moveit2官方教程】使用 MoveIt Task Constructor (MTC) 框架来定义和执行一个机器人任务

课堂随想2024-09-10 20:26
cpp 复制代码 
#include <rclcpp/rclcpp.hpp>  // ROS 2 的核心库
#include <moveit/planning_scene/planning_scene.h>  // MoveIt 规划场景相关的头文件
#include <moveit/planning_scene_interface/planning_scene_interface.h>  // MoveIt 规划场景接口
#include <moveit/task_constructor/task.h>  // MTC 任务的头文件
#include <moveit/task_constructor/solvers.h>  // MTC 求解器相关的头文件
#include <moveit/task_constructor/stages.h>  // MTC 阶段定义的头文件
#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>
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>  // 包含用于处理 Eigen 矩阵的 TF2 转换
#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 创建别名 mtc

// 定义 MTC 任务节点类
class MTCTaskNode
{
public:
  MTCTaskNode(const rclcpp::NodeOptions& options);  // 构造函数
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();  // 获取节点基础接口
  void doTask();  // 执行任务的函数
  void setupPlanningScene();  // 设置规划场景的函数
private:
  mtc::Task createTask();  // 创建任务的函数
  mtc::Task task_;  // MTC 任务对象
  rclcpp::Node::SharedPtr node_;  // ROS 2 节点对象
};

// 获取节点基础接口的实现
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}

// 构造函数的实现，创建节点对象
MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }  // 初始化节点名称为 "mtc_node"
{
}

// 设置规划场景的实现
void MTCTaskNode::setupPlanningScene()
{
  // 创建一个碰撞对象（障碍物）
  moveit_msgs::msg::CollisionObject object;
  object.id = "object";  // 定义对象 ID
  object.header.frame_id = "world";  // 对象的参考坐标系
  object.primitives.resize(1);  // 定义对象的几何形状数组大小为 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;
  pose.orientation.w = 1.0;  // 设置无旋转的四元数
  object.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;
  }

  // 规划任务路径，最大规划尝试次数为 5
  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()
{
  mtc::Task task;  // 创建一个 MTC 任务对象
  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;  // 当前状态指针，设置但不使用
#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);  // 设置最大速度缩放因子为 1.0
  cartesian_planner->setMaxAccelerationScalingFactor(1.0);  // 设置最大加速度缩放因子为 1.0
  cartesian_planner->setStepSize(.01);  // 设置步长为 0.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);  // 初始化 ROS 2

  rclcpp::NodeOptions options;  // 创建节点选项
  options.automatically_declare_parameters_from_overrides(true);  // 自动声明节点参数

  // 创建 MTC 任务节点对象
  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();  // 关闭 ROS 2
  return 0;
}

包含的头文件

cpp 复制代码 
#include <rclcpp/rclcpp.hpp>
  • 包含 ROS 2 的核心库，用于节点的创建和管理。
cpp 复制代码 
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
  • 包含 MoveIt 的规划场景相关的头文件，允许你在场景中设置物体（如障碍物）进行规划。
cpp 复制代码 
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
  • 包含 MoveIt Task Constructor 的头文件，用于任务的创建、规划和求解。task.h 管理任务的整体流程，solvers.h 和 stages.h 用于定义和解决任务中的不同阶段。
cpp 复制代码 
#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
  • 条件包含头文件，用于处理几何消息的 TF2 转换。TF2 是 ROS 中用于处理坐标变换的工具。
cpp 复制代码 
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif
  • 类似于上一个 tf2_geometry_msgs，这个条件编译块用于包含 Eigen 库的 TF2 转换接口。

定义日志器

cpp 复制代码 
static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
  • 定义一个用于输出日志的 ROS 2 日志器，名称为 mtc_tutorial。

使用命名空间

cpp 复制代码 
namespace mtc = moveit::task_constructor;
  • 为 moveit::task_constructor 命名空间定义简短别名 mtc，便于后续使用。

定义 MTCTaskNode 类

cpp 复制代码 
class MTCTaskNode
{
public:
  MTCTaskNode(const rclcpp::NodeOptions& options);
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
  void doTask();
  void setupPlanningScene();
private:
  mtc::Task createTask();
  mtc::Task task_;
  rclcpp::Node::SharedPtr node_;
};
  • MTCTaskNode 是一个类，表示 ROS 2 节点，并包含执行任务的功能：
    • MTCTaskNode: 构造函数，接受 NodeOptions 参数用于节点配置。
    • getNodeBaseInterface: 返回节点的基础接口，供 ROS 2 的执行器使用。
    • doTask: 定义并执行一个任务。
    • setupPlanningScene: 设置规划场景，定义场景中的物体。
    • createTask: 创建任务，并添加不同阶段的规划步骤。
    • task_: MTC 任务对象。
    • node_: ROS 2 节点共享指针。

实现 getNodeBaseInterface 函数

cpp 复制代码 
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}
  • 返回节点的基础接口，用于与 ROS 执行器交互。

实现 MTCTaskNode 构造函数

cpp 复制代码 
MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
{
}
  • 初始化函数，创建一个名为 mtc_node 的 ROS 2 节点，并传入选项。
1. 构造函数定义
cpp 复制代码 
MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  • 这是 MTCTaskNode 类的构造函数，参数 options 是 rclcpp::NodeOptions 类型的对象，表示 ROS 2 节点的选项配置。
  • 当用户创建 MTCTaskNode 的实例时，构造函数会被调用，用于初始化节点对象和类中的其他成员变量。
2. 成员初始化列表
cpp 复制代码 
: node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
  • : 后面的部分是成员初始化列表，用于直接初始化类的成员变量。相比在构造函数内部赋值，成员初始化列表在效率上可能更高，特别是当成员变量是对象或者共享指针时。
  • node_ 是类中的私有成员变量，类型为 rclcpp::Node::SharedPtr，即 ROS 2 节点的共享指针。
3. std::make_shared<rclcpp::Node>()
cpp 复制代码 
std::make_shared<rclcpp::Node>("mtc_node", options)
  • std::make_shared<rclcpp::Node>() 用于创建一个新的 rclcpp::Node 对象，并返回一个指向该对象的 std::shared_ptr（共享指针）。
  • 这个 Node 对象代表一个 ROS 2 节点，节点的名称为 "mtc_node"，并且使用传入的 options 参数来配置该节点。
4. 成员变量 node_ 的赋值
  • 构造函数中的成员初始化列表将 std::make_shared<rclcpp::Node> 创建的共享指针赋值给类的成员变量 node_，这意味着 MTCTaskNode 类的实例会持有一个指向 ROS 2 节点的共享指针。
5. 节点名称和选项
  • "mtc_node" 是这个 ROS 2 节点的名称，它可以用于在 ROS 网络中唯一标识该节点。
  • options 是 rclcpp::NodeOptions 对象，允许用户配置节点的一些选项（如参数自动声明等）。
总结

这段代码的作用是在构造 MTCTaskNode 类的实例时，通过 std::make_shared 创建了一个 ROS 2 节点对象，并将其存储在成员变量 node_ 中。通过成员初始化列表 的方式，保证了对象的高效初始化，并且节点名称为 "mtc_node"，选项通过参数 options 进行配置。

实现 setupPlanningScene

cpp 复制代码 
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;
  pose.orientation.w = 1.0;
  object.pose = pose;

  moveit::planning_interface::PlanningSceneInterface psi;
  psi.applyCollisionObject(object);
}
  • 创建一个障碍物对象（圆柱体），并将其添加到规划场景中。
    • CollisionObject: 定义碰撞对象的几何形状和位置信息。
    • PlanningSceneInterface: 用于管理与规划场景的交互。

实现 doTask 函数

cpp 复制代码 
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;
}
  • 创建任务并尝试执行：
    • task_.init(): 初始化任务。
    • task_.plan(5): 规划 5 个不同的路径。
    • task_.execute(): 执行规划的解决方案。

实现 createTask 函数

cpp 复制代码 
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);

  mtc::Stage* current_state_ptr = nullptr;
  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 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;
}
  • 定义任务并设置不同的阶段：
    • CurrentState: 获取当前状态。
    • PipelinePlanner: 采样规划器。
    • MoveTo: 指定机器人末端执行器移动到某个目标状态。

main 函数

cpp 复制代码 
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;
}
  • main 函数是程序的入口点，初始化 ROS 2 并创建执行器与任务线程：
    • rclcpp::init: 初始化 ROS 2。
    • rclcpp::NodeOptions: 设置节点选项。
    • MultiThreadedExecutor: 用于多线程执行节点操作。
    • 创建并启动执行器线程来处理任务。
相关推荐
宋138102797202 小时前
Manus Xsens Metagloves虚拟现实手套
人工智能·机器人·vr·动作捕捉
禁默3 小时前
第六届机器人、智能控制与人工智能国际学术会议（RICAI 2024）
人工智能·机器人·智能控制
Robot25116 小时前
Figure 02迎重大升级！！人形机器人独角兽[Figure AI]商业化加速
人工智能·机器人·微信公众平台
FreeIPCC20 小时前
谈一下开源生态对 AI人工智能大模型的促进作用
大数据·人工智能·机器人·开源
施努卡机器视觉1 天前
电解车间铜业机器人剥片技术是现代铜冶炼过程中自动化和智能化的重要体现
运维·机器人·自动化
zhd15306915625ff1 天前
库卡机器人日常维护
网络·机器人·自动化·机器人备件
古月居GYH1 天前
ROS一键安装脚本
人工智能·机器人·ros
清流君1 天前
【运动规划】移动机器人运动规划与轨迹优化全解析 | 经典算法总结
人工智能·笔记·算法·机器人·自动驾驶·运动规划
Matlab程序猿小助手2 天前
【MATLAB源码-第218期】基于matlab的北方苍鹰优化算法(NGO)无人机三维路径规划，输出做短路径图和适应度曲线.
开发语言·嵌入式硬件·算法·matlab·机器人·无人机
xx小寂2 天前
ubuntu16.04在ros使用USB摄像头-解决could not open /dev/video0问题
ubuntu·机器人
热门推荐
01(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明02PyTorch机器学习实现液态神经网络03【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总04玄机平台应急响应—webshell查杀05Coze扣子平台完整体验和实践（附国内和国际版对比）06Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO07【目标跟踪】相机运动补偿08Unity中PICO实现 隔空取物 和 接触抓取物体09RAG 实践- Ollama+RagFlow 部署本地知识库10怎样让音频速度变慢？请跟随以下方法进行操作