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 loggerauto 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!");
}// 关闭 ROSrclcpp::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!");
}