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
: 用于多线程执行节点操作。- 创建并启动执行器线程来处理任务。