Ros2 Moveit2 第一个C++项目
本教程将指导您使用 MoveIt 编写第一个 C++ 应用程序。
警告：MoveIt 中的大多数功能将无法正常工作，因为完整的 Move Group 功能需要附加参数。如需完整设置，请继续阅读Move Group C++ 接口教程。
先决条件
如果您还没有这样做，请确保您已经完成入门指南中的步骤。
本教程假设您了解 ROS 2 的基础知识。为此做好准备，请完成官方 ROS 2 教程，直到“编写一个简单的发布者和订阅者（C++）”。
步骤
1 创建包
打开终端并获取 ROS 2 安装，以便ros2命令能够正常工作。
导航到您在入门教程ws_moveit中创建的目录。
将目录更改为src目录，因为那是我们放置源代码的地方。
使用 ROS 2 命令行工具创建一个新包：
ros2pkgcreate\
--build-typeament_cmake\
--dependenciesmoveit_ros_planning_interfacerclcpp\
--node-namehello_moveithello_moveit
其输出将显示它在新目录中创建了一些文件。
请注意，我们添加了moveit_ros_planning_interface和作为依赖项。这将在和文件rclcpp中创建必要的更改，以便我们可以依赖这两个包。package.xmlCMakeLists.txt
在您最喜欢的编辑器中打开为您创建的新源文件ws_moveit/src/hello_moveit/src/hello_moveit.cpp。
2 创建 ROS 节点和执行器
第一段代码有点样板化，但您应该习惯从 ROS 2 教程中看到它。
#include<memory>
#include<rclcpp/rclcpp.hpp>
#include<moveit/move_group_interface/move_group_interface.h>
intmain(intargc,char*argv[])
{
// Initialize ROS and create the Node
rclcpp::init(argc,argv);
autoconstnode=std::make_shared<rclcpp::Node>(
"hello_moveit",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
// Create a ROS logger
autoconstlogger=rclcpp::get_logger("hello_moveit");
// Next step goes here
// Shutdown ROS
rclcpp::shutdown();
return0;
}
2.1 构建并运行
在继续之前，我们将构建并运行该程序以确保一切正确。
将目录更改回工作区目录ws_moveit并运行以下命令：
colconbuild--mixindebug
成功后，打开一个新终端，然后在该新终端中获取工作区环境脚本，以便我们可以运行我们的程序。
cd~/ws_moveit
sourceinstall/setup.bash
运行你的程序并查看输出。
ros2runhello_moveithello_moveit
程序应该运行并退出并且不会出现错误。
2.2 检查代码
顶部包含的标头只是一些标准 C++ 标头和我们稍后将使用的 ROS 和 MoveIt 的标头。
之后，我们通过正常调用来初始化 rclcpp，然后创建我们的 Node。
autoconstnode=std::make_shared<rclcpp::Node>(
"hello_moveit",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
第一个参数是 ROS 用来命名唯一节点的字符串。MoveIt 需要第二个参数，因为我们使用 ROS 参数的方式不同。
接下来，我们创建一个名为“hello_moveit”的记录器，以使我们的日志输出保持有序和可配置。
// Create a ROS logger
autoconstlogger=rclcpp::get_logger("hello_moveit");
最后，我们有关闭 ROS 的代码。
// Shutdown ROS
rclcpp::shutdown();
return0;
3 使用 MoveGroupInterface 进行计划和执行
在“下一步从这里开始”的注释位置添加以下代码：
// Create the MoveIt MoveGroup Interface
usingmoveit::planning_interface::MoveGroupInterface;
automove_group_interface=MoveGroupInterface(node,"manipulator");
// Set a target Pose
autoconsttarget_pose=[]{
geometry_msgs::msg::Posemsg;
msg.orientation.w=1.0;
msg.position.x=0.28;
msg.position.y=-0.2;
msg.position.z=0.5;
returnmsg;
}();
move_group_interface.setPoseTarget(target_pose);
// Create a plan to that target pose
autoconst[success,plan]=[&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Planmsg;
autoconstok=static_cast<bool>(move_group_interface.plan(msg));
returnstd::make_pair(ok,msg);
}();
// Execute the plan
if(success){
move_group_interface.execute(plan);
}else{
RCLCPP_ERROR(logger,"Planning failed!");
}
3.1 构建并运行
就像以前一样，我们需要先构建代码才能运行它。
在工作区目录中，ws_moveit运行以下命令：
colconbuild--mixindebug
成功后，我们需要重新使用上一个教程中的演示启动文件来启动 RViz 和 MoveGroup 节点。在单独的终端中，获取工作区，然后执行以下命令：
ros2launchmoveit2_tutorialsdemo.launch.py
然后在Displays下面的窗口中，取消选中该框。MotionPlanning/Planning RequestQuery Goal State
在第三个终端中，获取工作区并运行您的程序。
ros2runhello_moveithello_moveit
这应该会导致 RViz 中的机器人移动并最终处于以下姿势：
请注意，如果您在没有先启动演示启动文件的情况下运行节点hello_moveit，它将等待 10 秒，然后打印此错误并退出。
[ERROR][1644181704.350825487][hello_moveit]:Couldnotfindparameterrobot_descriptionanddidnotreceiverobot_descriptionviastd_msgs::msg::Stringsubscriptionwithin10.000000seconds.
这是因为demo.launch.py启动正在启动MoveGroup提供机器人描述的节点。MoveGroupInterface构造时，它会寻找发布带有机器人描述的主题的节点。如果在 10 秒内找不到该节点，它会打印此错误并终止程序。
3.2 检查代码
我们要做的第一件事是创建MoveGroupInterface。此对象将用于与 交互move_group，这使我们能够规划和执行轨迹。请注意，这是我们在此程序中创建的唯一可变对象。另一件需要注意的事情是MoveGroupInterface我们在此处创建的对象的第二个参数："manipulator"。这是机器人描述中定义的关节组，我们将用它对其进行操作MoveGroupInterface。
usingmoveit::planning_interface::MoveGroupInterface;
automove_group_interface=MoveGroupInterface(node,"manipulator");
然后，我们设置目标姿势和计划。请注意，仅设置目标姿势（通过setPoseTarget）。起始姿势隐式是联合状态发布者发布的位置，可以使用 MoveGroupInterface::setStartState*函数系列进行更改（但本教程中不这样做）。
关于下一节，还有一点需要注意，即使用 lambda 来构造消息类型target_pose和规划。这是现代 C++ 代码库中常见的一种模式，它能够以更具声明性的风格进行编写。有关此模式的更多信息，请参阅本教程末尾的几个链接。
// Set a target Pose
autoconsttarget_pose=[]{
geometry_msgs::msg::Posemsg;
msg.orientation.w=1.0;
msg.position.x=0.28;
msg.position.y=-0.2;
msg.position.z=0.5;
returnmsg;
}();
move_group_interface.setPoseTarget(target_pose);
// Create a plan to that target pose
autoconst[success,plan]=[&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Planmsg;
autoconstok=static_cast<bool>(move_group_interface.plan(msg));
returnstd::make_pair(ok,msg);
}();
最后，如果规划成功，我们就执行计划，否则，我们就记录错误：
// Execute the plan
if(success){
move_group_interface.execute(plan);
}else{
RCLCPP_ERROR(logger,"Planning failed!");
}
