系列文章目录
留空
前言
自用
一、完整代码
#include <memory> // 引入内存管理库，用于智能指针
/*第一个C++程序*/
#include <rclcpp/rclcpp.hpp> // 引入 ROS 2 C++ 客户端库
#include <moveit/move_group_interface/move_group_interface.h> // 引入MoveIt的MoveGroup 接口库
int main(int argc, char * argv[])
{
// 1. 初始化 ROS 并创建节点
rclcpp::init(argc, argv); // 初始化 ROS 2 系统
auto const node = std::make_shared<rclcpp::Node>( // 创建一个新的 ROS 节点
"hello_moveit", // 节点的名称
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
// 设置节点选项以自动声明参数
);
// 2. 创建一个 ROS logger
auto const logger = rclcpp::get_logger("hello_moveit"); // 获取一个日志记录器用于输出日志信息
// 3. 创建 MoveIt 的 MoveGroup 接口
using moveit::planning_interface::MoveGroupInterface; // 使用 MoveGroupInterface 类
auto move_group_interface = MoveGroupInterface(node, "panda_arm"); // 创建 MoveGroupInterface 对象，指定运动组名称为 "panda_arm"
// 4. 设置目标位姿
auto const target_pose = []{
geometry_msgs::msg::Pose msg; // 创建一个 Pose 消息
msg.orientation.w = 1.0; // 设置四元数的 w 分量，表示没有旋转
msg.position.x = 0.28; // 设置目标位置的 x 坐标
msg.position.y = -0.2; // 设置目标位置的 y 坐标
msg.position.z = 0.5; // 设置目标位置的 z 坐标
return msg; // 返回设置好的目标位姿
}();
move_group_interface.setPoseTarget(target_pose); // 设置 MoveGroupInterface 的目标位姿
// 5. 创建一个到目标位姿的规划
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan msg; // 创建一个 Plan 消息
auto const ok = static_cast<bool>(move_group_interface.plan(msg)); // 调用 plan 方法，生成运动规划，并检查规划是否成功
return std::make_pair(ok, msg); // 返回成功标志和规划结果
}();
// 6. 计算并执行这个规划
if(success) {
move_group_interface.execute(plan); // 如果规划成功，执行规划结果
} else {
RCLCPP_ERROR(logger, "Planning failed!"); // 如果规划失败，输出错误日志
}
// 7. 关闭 ROS
rclcpp::shutdown(); // 关闭 ROS 2 系统
return 0; // 返回 0 表示程序正常退出
}
编译
打开终端
cd moveit2_ws
colcon build --packages-select learning_moveit2
ros2 launch moveit2_tutorials demo.launch.py
打开第二个终端
source install/setup.sh
ros2 run learning_moveit2 hello_moveit
把橙色移开
二、编写步骤
这段代码的目的是使用 ROS 2 和 MoveIt 控制一个机械臂到达指定的位置。主要步骤包括：
- 初始化 ROS 系统。
- 创建一个 ROS 节点。
- 创建一个 MoveIt 的 MoveGroup 接口对象。
- 设置机械臂的目标位置。
- 进行运动规划并执行。
- 关闭 ROS 并结束程序。
三、代码详细解析
1. 引入头文件
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <memory>: 引入智能指针相关的头文件（std::shared_ptr等）。#include <rclcpp/rclcpp.hpp>: 引入 ROS 2 的 C++ 客户端库。#include <moveit/move_group_interface/move_group_interface.h>: 引入 MoveIt 的 MoveGroup 接口库，用于机器人运动规划。
2. main 函数
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");
}
rclcpp::init(argc, argv);: 初始化 ROS 2 系统。std::make_shared<rclcpp::Node>("hello_moveit", ...): 创建一个 ROS 节点，名字叫"hello_moveit"。rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true): 设置节点选项，自动声明参数。
auto const logger = rclcpp::get_logger("hello_moveit");: 创建一个用于记录日志的对象。
3. 创建 MoveIt 的 MoveGroup 接口
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");
using moveit::planning_interface::MoveGroupInterface;: 引入 MoveGroupInterface 类。auto move_group_interface = MoveGroupInterface(node, "panda_arm");: 创建 MoveGroupInterface 对象，"panda_arm"是机械臂的名称。这个对象用于与机械臂进行交互和控制。
4. 设置目标位姿
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 target_pose = [] { ... }();: 使用 Lambda 表达式创建目标位姿。geometry_msgs::msg::Pose用于表示位置和姿态。msg.orientation.w = 1.0;: 设置姿态为无旋转（单位四元数）。msg.position.x,msg.position.y,msg.position.z: 设置目标的位置坐标。- Lambda表达式结束之后，加上了
()，这表示立即调用这个lambda表达式，并将返回的msg赋值给target_pose。
move_group_interface.setPoseTarget(target_pose);: 将目标位姿设置到 MoveGroupInterface 对象中。
5. 创建和执行运动规划
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!");
}
这个部分的代码用于进行路径规划，并获取规划结果，检查路径规划是否成功。如果成功，则执行路径；如果失败，则打印错误信息。
-
调用
plan方法进行路径规划：moveit::planning_interface::MoveGroupInterface::Plan msg;创建了一个名为msg的对象，用来存储机器人路径规划的结果。其中MoveGroupInterface::Plan是MoveIt中的一个类型，专门用于存储路径规划的结果。move_group_interface.plan(msg)尝试为机器人规划一条路径，并将结果存储在msg中。static_cast<bool>：将plan方法的返回值转换为布尔值。plan方法返回一个MoveItErrorCode类型的结果，用于指示路径规划的成功或失败。通过static_cast<bool>，我们将这个结果转换为一个简单的true或false。auto const ok= 返回值被转换为布尔值ok，表示路径规划是否成功。
-
将结果存储在
std::pair中：static_cast<bool>：将plan方法的返回值转换为布尔值。plan方法返回一个MoveItErrorCode类型的结果，用于指示路径规划的成功或失败。通过static_cast<bool>，我们将这个结果转换为一个简单的true或false。std::make_pair(ok, msg)组合布尔值ok和msg对象，并返回这个组合结果。
-
解构绑定：
auto const [success, plan]：通过[ ](C++17 的结构化绑定) 将std::pair的两个元素ok和msg分别赋值给success和plan。- 其中，
success（布尔值，表示规划是否成功）和plan（规划的结果）。
-
检查路径规划
if(success)是否成功。
- 如果
success为true，表示路径规划成功，调用move_group_interface.execute(plan)执行规划的路径。 - 如果
success为false，表示路径规划失败，使用RCLCPP_ERROR打印错误信息 “Planning failed!”。
6. 关闭 ROS 并结束程序
// 关闭 ROS
rclcpp::shutdown();
return 0;
}
rclcpp::shutdown();: 关闭 ROS 系统，释放资源。return 0;: 程序正常结束，返回 0。
四、补充
1. MoveIt 的 MoveGroup 接口库
#include <moveit/move_group_interface/move_group_interface.h>
是 MoveIt 2 的一个重要头文件，它定义了 MoveGroupInterface 类，专门用于与 MoveIt 的运动规划功能交互。下面详细介绍这个库的功能和用途，以及与之相关的一些其他库。
（1） MoveGroupInterface 类
MoveGroupInterface 类是 MoveIt 2 中用于与运动组进行交互的主要接口。它允许用户在 C++ 代码中进行以下操作：
- 设置目标位置和姿态：通过设置机械臂的目标位置和姿态，定义机器人应达到的目标。
- 规划运动轨迹：基于目标位姿，计算机械臂的运动轨迹，以使机械臂从当前姿态移动到目标位置。
- 执行运动计划：将计算出的运动轨迹发送给机器人执行，实际移动机械臂。
- 获取状态信息：获取机械臂的当前状态，包括位置、姿态等。
（2）主要功能和方法
MoveGroupInterface 提供了许多功能和方法，主要包括：
-
构造函数
MoveGroupInterface(node, "group_name");node：ROS 节点，用于与 ROS 进行通信。"group_name"：机器人模型中的运动组名称，用于指定要操作的机器人部件。
-
设置目标
move_group_interface.setPoseTarget(target_pose);setPoseTarget：设置目标位姿（位置和姿态），用于规划机械臂的运动轨迹。
-
规划运动
auto const [success, plan] = move_group_interface.plan();plan：计算从当前姿态到目标位姿的运动轨迹，并返回一个包含规划结果的对象。
-
执行计划
move_group_interface.execute(plan);execute：执行之前规划出的运动轨迹，使机械臂按照计划的轨迹移动。
-
获取当前状态
auto current_pose = move_group_interface.getCurrentPose();getCurrentPose：获取机械臂当前的姿态信息。
（3） 相关库和头文件
除了 <moveit/move_group_interface/move_group_interface.h> 外，还有其他一些与 MoveIt 相关的库和头文件，它们提供了不同的功能：
<moveit/planning_scene_interface/planning_scene_interface.h>：提供与规划场景交互的功能，如添加或移除碰撞对象。<moveit/robot_model/robot_model.h>：提供机器人模型的表示和操作。<moveit/robot_state/robot_state.h>：用于获取和操作机器人当前的状态。<moveit/trajectory_processing/trajectory_processing.h>：用于处理运动轨迹的工具函数。<moveit_visual_tools/moveit_visual_tools.h>：提供在 RViz 中可视化机器人运动和规划的功能。
2. Lambda表达式
（1）什么是Lambda表达式
在C++中，Lambda表达式是一种定义匿名函数的方式，能够让代码更加简洁和灵活。它们通常用于需要短小函数或临时函数的场景，比如排序、自定义算法、事件处理等。
C++中Lambda表达式的基本语法
[capture](参数) -> 返回类型 {函数体}
[capture]: 捕获列表，决定了Lambda表达式中可以使用哪些外部变量。(参数): 参数列表，和普通函数一样，用于定义Lambda表达式的输入。-> 返回类型: （可选）用于指定Lambda表达式的返回类型。如果可以自动推断出返回类型，可以省略。{函数体}: 定义Lambda表达式的操作逻辑。
（2）基础用法
好的，我们来比较一下在C++中使用普通函数和Lambda表达式实现加法的不同。
普通函数的实现
首先是用普通函数来实现加法：
#include <iostream>
// 定义一个普通函数来计算两个数的和
int add(int x, int y) {
return x + y;
}
int main() {
int result = add(2, 3); // 调用普通函数
std::cout << "Result using function: " << result << std::endl; // 输出 5
return 0;
}
在这个例子中，我们定义了一个名为 add 的普通函数，并在 main 函数中调用它。这个方法非常清晰明了，但需要在函数外部定义函数。
Lambda表达式的实现
现在用Lambda表达式实现相同的功能：
#include <iostream>
int main() {
auto add = [](int x, int y) { return x + y; }; // 使用Lambda表达式
int result = add(2, 3); // 调用Lambda表达式
std::cout << "Result using lambda: " << result << std::endl; // 输出 5
return 0;
}
在这个例子中，add 是一个 Lambda 表达式，它直接在 main 函数内部定义。这个方法使得代码更简洁，尤其是在函数只在一个地方使用时。
3. automatically_declare_parameters_from_overrides(true)
这是ROS 2中的一个选项，用来让节点自动接收和使用你在启动时传递的参数，而不用你在代码里提前声明这些参数。
（1）背景知识
在ROS 2中，节点通常使用参数来控制其行为，比如设置更新频率、话题名称、阈值等。这些参数通常需要在节点的代码中明确声明，然后你才能在运行时动态调整它们。如果你没有在代码里声明参数，节点即使收到了参数，也不会知道该如何处理。
（2） 基础用法
- 普通用法的实现
在没有使用这个选项时，你需要手动声明所有你计划使用的参数。举个例子：
#include "rclcpp/rclcpp.hpp"
class ExampleNode : public rclcpp::Node {
public:
ExampleNode() : Node("example_node") {
// 明确声明一个参数，名字是 "speed"，默认值是10
this->declare_parameter<int>("speed", 10);
// 获取这个参数的值
int speed = this->get_parameter("speed").as_int();
RCLCPP_INFO(this->get_logger(), "Speed: %d", speed);
}
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ExampleNode>());
rclcpp::shutdown();
return 0;
}
在这个代码中，我们明确声明了一个名为 speed 的参数。如果你想在运行时修改这个参数，比如启动节点时将 speed 设置为20，你可以这样做：
ros2 run my_package example_node --ros-args -p speed:=20
但是，前提是你必须在代码中提前声明了 speed 这个参数。如果你没有声明它，节点就不知道如何处理 speed，会报错。
- 使用后实现
为了避免手动声明参数，你可以使用 automatically_declare_parameters_from_overrides(true) 这个选项。这个选项的作用是让节点自动识别和使用任何从外部传递进来的参数，即使这些参数没有在代码里提前声明。
#include "rclcpp/rclcpp.hpp"
class ExampleNode : public rclcpp::Node {
public:
ExampleNode(rclcpp::NodeOptions options) : Node("example_node", options) {
// 直接获取参数值，即使没有明确声明过
int speed = this->get_parameter("speed").as_int();
RCLCPP_INFO(this->get_logger(), "Speed: %d", speed);
}
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ExampleNode>(
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
));
rclcpp::shutdown();
return 0;
}
现在，当你启动这个节点时：
ros2 run my_package example_node --ros-args -p speed:=20
即使 speed 参数没有在代码中明确声明，节点也会自动识别并使用它。这是因为我们用了 automatically_declare_parameters_from_overrides(true)，让节点自动处理从外部传入的参数。
（3）何时使用？
- 有利场景：当你希望节点能够灵活接收外部传入的参数，而不想每次都修改代码来声明新的参数时，这个选项非常有用。尤其是在开发和调试过程中，可以快速测试不同的参数组合。
- 可能的问题：虽然这个选项简化了代码，但也可能引入一些隐患。由于参数不需要提前声明，可能会导致一些参数拼写错误或误用，进而引发难以调试的问题。
总结
自用
797
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
