MoveIt2-humble】入门教程----第一个 C++ MoveIt 程序
四节教程会手把手带你写一个完整的 Moveit 控制程序，包括轨迹规划、RViz可视化、添加碰撞物体、抓取和放置。
1 创建依赖包
进入到教程所在工作空间下的src目录，创建一个新的依赖包。
ros2 pkg create \
--build-type ament_cmake \
--dependencies moveit_ros_planning_interface rclcpp \
--node-name hello_moveit hello_moveit
在新建的这个包中添加hello_moveit.cpp，准备开始编码
2 创建ROS节点和执行器
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char * argv[])
{
// 初始化 ROS 并创建节点
rclcpp::init(argc, argv);
auto const node = std::make_shared<rclcpp::Node>(
"hello_moveit",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
// 创建一个 ROS logger
auto const logger = rclcpp::get_logger("hello_moveit");
// Next step goes here
// 创建 MoveIt 的 MoveGroup 接口
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");
// 设置目标位姿
auto const target_pose = []{
geometry_msgs::msg::Pose msg;
msg.orientation.w = 1.0;
msg.position.x = 0.28;
msg.position.y = -0.2;
msg.position.z = 0.5;
return msg;
}();
move_group_interface.setPoseTarget(target_pose);
// 创建一个到目标位姿的规划
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan msg;
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
// 计算这个规划
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planing failed!");
}
// 关闭 ROS
rclcpp::shutdown();
return 0;
}
2.1 编译和运行
我们可以尝试编译并运行一下，看看有无出现问题。
2.2 代码说明
最上面的是标准C++头文件，随后是为使用 ROS 和 Moveit 所添加的头文件。
在这之后，我们初始化 rclcpp，并创建了节点。
auto const node = std::make_shared<rclcpp::Node>(
"hello_moveit",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
第一个参数是节点名称。第二个参数对于 Moveit 很重要，因为我们要使用 ROS 参数。
最后，关闭这个节点。
3 使用MoveGroupInterface来规划和执行
在代码中“Next step goes here,”的后面添加以下节点：
// 创建 MoveIt 的 MoveGroup 接口
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");
// 设置目标位姿
auto const target_pose = []{
geometry_msgs::msg::Pose msg;
msg.orientation.w = 1.0;
msg.position.x = 0.28;
msg.position.y = -0.2;
msg.position.z = 0.5;
return msg;
}();
move_group_interface.setPoseTarget(target_pose);
// 创建一个到目标位姿的规划
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan msg;
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
// 计算这个规划
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planing failed!");
}
3.1 编译和运行
打开教程中的launch文件去启动rviz和 MoveGroup 节点，在另一个终端中source这个工作空间并执行：
ros2 launch moveit2_tutorials demo.launch.py
然后在Displays窗口下面的MotionPlanning/Planning Request中，取消选择Query Goal State。
在第三个终端中 source 并允许我们本节的程序：
ros2 run hello_moveit hello_moveit
注：
如果你没有运行launch文件就运行了 hello_moveit 节点，等待10s后将会报如下错误：
[ERROR] [1644181704.350825487] [hello_moveit]: Could not find parameter robot_description and did not receive robot_description via std_msgs::msg::String subscription within 10.000000 seconds.
这是因为demo.launch.py启动了MoveGroup节点，其提供了机器人描述信息。当MoveGroupInterface被构建的时候，会寻找发布机器人描述话题的节点，如果在10秒内没找到，就会打印错误信息并结束程序。
3.2 代码说明
首先要创建 MoveGroupInterface，这个对象用来与 move_group 交互，从而是我们能够去规划和执行轨迹。注意这是我们在程序中创建的唯一的可变对象。
其次是我们创建的 MoveGroupInterface 的第二个接口 "panda_arm"，这是在机器人描述中定义的关节组，我们将通过 MoveGroupInterface 去操作它。
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");
之后，设置目标位姿和规划。起始点位姿通过 joint state publisher 来发布，它能够使用MoveGroupInterface::setStartState*中的函数来被改变（本教程无）
通过使用 lambda 表达式来构建信息类型 target_pose与规划。
// 利用 lambda 函数来创建目标位姿
auto const target_pose = []{
geometry_msgs::msg::Pose msg;
msg.orientation.w = 1.0;
msg.position.x = 0.28;
msg.position.y = -0.2;
msg.position.z = 0.5;
return msg;
}();
move_group_interface.setPoseTarget(target_pose);
// 利用 lambda 函数来创建到目标位姿的规划
// 注：这里使用了 C++ 20标准的新特性
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan msg;
// 强制类型转换
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
最后，如果成功规划则计算这个规划，如果规划失败则返回错误信息。
// Execute the plan
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planning failed!");
}