Ros2 Moveit2 第一个C++项目

标签：C++ hello msg moveit interface ROS Moveit2 Ros2 MoveGroupInterface

本教程将指导您使用 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!");
}

本教程将指导您使用 MoveIt 编写第一个 C++ 应用程序。警告：MoveIt 中的大多数功能将无法正常工作，因为完整的 Move Group 功能需要附加参数。如需完整设置，请继续阅读Move Group C++ 接口教程。
先决条件如果您还没有这样做，请确保您已经完成入门指南中的步骤。
本教程假设您了解 ROS 2 的基础知识。为此做好准备，请完成官方 ROS 2 教程，直到“编写一个简单的发布者和订阅者（C++）”。
步骤1 创建包打开终端并获取 ROS 2 安装，以便ros2命令能够正常工作。
导航到您在入门教程ws_moveit中创建的目录。
将目录更改为src目录，因为那是我们放置源代码的地方。
使用 ROS 2 命令行工具创建一个新包：
ros2 pkg create \ --build-type ament_cmake \ --dependencies moveit_ros_planning_interface rclcpp \ --node-name hello_moveit hello_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>
int main(int argc, char * argv[]){ // Initialize ROS and create the Node rclcpp::init(argc, argv); auto const node = std::make_shared<rclcpp::Node>( "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) );
// Create a ROS logger auto const logger = rclcpp::get_logger("hello_moveit");
// Next step goes here
// Shutdown ROS rclcpp::shutdown(); return 0;}2.1 构建并运行在继续之前，我们将构建并运行该程序以确保一切正确。
将目录更改回工作区目录ws_moveit并运行以下命令：
colcon build --mixin debug成功后，打开一个新终端，然后在该新终端中获取工作区环境脚本，以便我们可以运行我们的程序。
cd ~/ws_moveitsource install/setup.bash运行你的程序并查看输出。
ros2 run hello_moveit hello_moveit程序应该运行并退出并且不会出现错误。
2.2 检查代码顶部包含的标头只是一些标准 C++ 标头和我们稍后将使用的 ROS 和 MoveIt 的标头。
之后，我们通过正常调用来初始化 rclcpp，然后创建我们的 Node。
auto const node = std::make_shared<rclcpp::Node>( "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));第一个参数是 ROS 用来命名唯一节点的字符串。MoveIt 需要第二个参数，因为我们使用 ROS 参数的方式不同。
接下来，我们创建一个名为“hello_moveit”的记录器，以使我们的日志输出保持有序和可配置。
// Create a ROS loggerauto const logger = rclcpp::get_logger("hello_moveit");最后，我们有关闭 ROS 的代码。
// Shutdown ROSrclcpp::shutdown();return 0;3 使用 MoveGroupInterface 进行计划和执行在“下一步从这里开始”的注释位置添加以下代码：
// Create the MoveIt MoveGroup Interfaceusing moveit::planning_interface::MoveGroupInterface;auto move_group_interface = MoveGroupInterface(node, "manipulator");
// Set a target Poseauto 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);
// Create a plan to that target poseauto 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 planif(success) { move_group_interface.execute(plan);} else { RCLCPP_ERROR(logger, "Planning failed!");}3.1 构建并运行就像以前一样，我们需要先构建代码才能运行它。
在工作区目录中，ws_moveit运行以下命令：
colcon build --mixin debug成功后，我们需要重新使用上一个教程中的演示启动文件来启动 RViz 和 MoveGroup 节点。在单独的终端中，获取工作区，然后执行以下命令：
ros2 launch moveit2_tutorials demo.launch.py然后在Displays下面的窗口中，取消选中该框。MotionPlanning/Planning RequestQuery Goal State
../../../_图像/rviz_1.png在第三个终端中，获取工作区并运行您的程序。
ros2 run hello_moveit hello_moveit这应该会导致 RViz 中的机器人移动并最终处于以下姿势：
../../../_图像/rviz_2.png请注意，如果您在没有先启动演示启动文件的情况下运行节点hello_moveit，它将等待 10 秒，然后打印此错误并退出。
[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我们在此处创建的对象的第二个参数："manipulator"。这是机器人描述中定义的关节组，我们将用它对其进行操作MoveGroupInterface。
using moveit::planning_interface::MoveGroupInterface;auto move_group_interface = MoveGroupInterface(node, "manipulator");然后，我们设置目标姿势和计划。请注意，仅设置目标姿势（通过setPoseTarget）。起始姿势隐式是联合状态发布者发布的位置，可以使用 MoveGroupInterface::setStartState*函数系列进行更改（但本教程中不这样做）。
关于下一节，还有一点需要注意，即使用 lambda 来构造消息类型target_pose和规划。这是现代 C++ 代码库中常见的一种模式，它能够以更具声明性的风格进行编写。有关此模式的更多信息，请参阅本教程末尾的几个链接。
// Set a target Poseauto 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);
// Create a plan to that target poseauto 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 planif(success) { move_group_interface.execute(plan);} else { RCLCPP_ERROR(logger, "Planning failed!");}概括您创建了一个 ROS 2 包并使用 MoveIt 编写了您的第一个程序。
您学习了如何使用 MoveGroupInterface 来规划和执行移动。
这是本教程末尾的完整 hello_moveit.cpp 源代码的副本。
进一步阅读我们使用 lambda 来将对象初始化为常量。这被称为 IIFE 技术。请阅读 C++ Stories 以了解有关此模式的更多信息。
我们还将所有内容声明为 const。请在此处了解有关 const 的更多用途。
下一步在下一个教程“在 RViz 中进行可视化”中，您将扩展在此处构建的程序，以创建可视化标记，从而更容易理解 MoveIt 正在做什么。

标签：C++,hello,msg,moveit,interface,ROS,Moveit2,Ros2,MoveGroupInterface
From： https://www.cnblogs.com/ai-ldj/p/18342278