系列文章目录
留空
文章目录
前言
自用
一、完整代码
#include <memory>
/*第一个C++程序*/
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
/*在 RViz 中可视化*/
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <thread>
int main(int argc, char * argv[])
{
// 1.1 初始化 ROS 并创建节点
rclcpp::init(argc, argv);
auto const node = std::make_shared<rclcpp::Node>(
"hello_moveit",
// 创建名为 "hello_moveit" 的 ROS 2 节点,并自动声明从命令行传入的参数
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
// 1.2 创建 ROS 日志记录器
auto const logger = rclcpp::get_logger("hello_moveit");
// 1.3 创建单线程执行器
// 创建一个 `SingleThreadedExecutor` 来处理 ROS 的回调函数
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
// 1.4 启动一个独立线程运行 `spin` 方法
// 使用一个独立的线程来运行 executor 的 spin 方法
// `spin` 方法会处理所有添加到 executor 的节点的回调函数,直到节点被关闭
auto spinner = std::thread([&executor]() { executor.spin(); });
// 2.1 创建 MoveGroupInterface
// 创建 MoveIt 的 `MoveGroupInterface` 接口,用于与机械臂进行交互
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");
// 2.2 初始化 MoveItVisualTools
// 构建并初始化 MoveItVisualTools 用于在 Rviz 中进行可视化
auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{
node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC,
move_group_interface.getRobotModel()};
// 删除所有旧的标记
moveit_visual_tools.deleteAllMarkers();
// 加载远程控制界面,用于用户交互
moveit_visual_tools.loadRemoteControl();
// 2.3 创建用于可视化的闭包函数
// 创建用于可视化的闭包函数(Lambda 函数)
auto const draw_title = [&moveit_visual_tools](auto text) {
auto const text_pose = [] {
auto msg = Eigen::Isometry3d::Identity(); // 设置文本的初始位置
msg.translation().z() = 1.0; // 提升文本位置以便在 Rviz 中显示
return msg;
}();
moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE,
rviz_visual_tools::XLARGE); // 发布文本标记
};
// 等待用户点击 Rviz 中的 "Next" 按钮继续执行的闭包函数
auto const prompt = [&moveit_visual_tools](auto text) {
moveit_visual_tools.prompt(text);
};
// 在 Rviz 中绘制轨迹的闭包函数
auto const draw_trajectory_tool_path =
[&moveit_visual_tools,
jmg = move_group_interface.getRobotModel()->getJointModelGroup(
"panda_arm")](auto const trajectory) {
moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); // 发布轨迹线
};
// 2.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); // 将目标位姿设置为机械臂的目标
// 2.5 创建规划并提示用户
// 创建一个计划到目标位姿的路径
prompt("Press 'Next' in the RvizVisualToolsGui window to plan"); // 提示用户继续
draw_title("Planning"); // 在 Rviz 中显示 "Planning" 字样
moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
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); // 返回规划结果和规划的路径
}();
// 2.6 执行规划路径
// 如果规划成功,执行规划
if (success) {
draw_trajectory_tool_path(plan.trajectory_); // 在 Rviz 中绘制轨迹
moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
prompt("Press 'Next' in the RvizVisualToolsGui window to execute"); // 提示用户继续
draw_title("Executing"); // 在 Rviz 中显示 "Executing" 字样
moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
move_group_interface.execute(plan); // 执行规划的路径
} else {
draw_title("Planning Failed!"); // 规划失败,在 Rviz 中显示错误信息
moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
RCLCPP_ERROR(logger, "Planning failed!"); // 记录错误日志
}
// 1.5 关闭 ROS 节点
rclcpp::shutdown(); // 这将使 `spin` 函数返回,停止执行回调
spinner.join(); // 等待线程结束,确保程序安全退出
return 0;
}
编译
打开一个终端
cd moveit2_ws
colcon build --packages-select learning_moveit2
source install/setup.sh
ros2 launch moveit2_tutorials demo.launch.py
经过这样那样变成下图(放在 四、补充 的 2)
打开另一个终端
source install/setup.sh
ros2 run learning_moveit2 hello_moveit
点击Next
再点
完美结束
二、编写步骤
1. 初始化和配置 ROS 环境
- 初始化ROS2并创建节点:初始化ROS2 系统,创建一个名为
"hello_moveit"
的 ROS 2 节点。 - 创建 ROS2 日志记录器:用于记录日志信息。
- 创建单线程执行器:用于处理 ROS2 的回调函数。
- 启动独立线程:运行执行器的
spin
方法,以便处理回调函数。 - 关闭 ROS2 节点:停止 ROS2 节点并等待线程结束。
2. 使用 MoveIt 进行机械臂控制和可视化
- 创建 MoveGroupInterface:用于与机械臂进行交互。
- 初始化 MoveItVisualTools:用于在 Rviz 中可视化机械臂的状态和轨迹。
- 定义可视化闭包函数:
- 绘制标题:在 Rviz 中显示文本标记。
- 提示用户:在 Rviz 中提示用户操作。
- 绘制轨迹:在 Rviz 中绘制机械臂的轨迹。
- 设置目标位姿:定义机械臂的目标位置和姿态。
- 创建并执行规划:
- 创建规划:计算机械臂从当前位置到目标位置的路径。
- 执行规划:沿着规划的路径移动机械臂。如果规划失败,记录错误并在 Rviz 中显示错误信息。
三、代码详细解析
1. 初始化和配置 ROS2 环境
#include <thread> // 引入线程库
int main(int argc, char * argv[])
{
// 1.1 初始化 ROS2 并创建节点
rclcpp::init(argc, argv);
auto const node = std::make_shared<rclcpp::Node>(
"hello_moveit",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
rclcpp::init(argc, argv);
: 初始化 ROS 2 系统,准备处理命令行参数。这里argc
和argv
分别表示参数的个数和具体的参数值,这些参数会传递给 ROS 进行配置。std::make_shared<rclcpp::Node>("hello_moveit", ...);
: 使用std::make_shared
创建一个共享指针,生成名为"hello_moveit"
的 ROS 2 节点。NodeOptions().automatically_declare_parameters_from_overrides(true)
设置节点的参数选项,允许从命令行传入的参数自动声明。
// 1.2 创建 ROS 日志记录器
auto const logger = rclcpp::get_logger("hello_moveit");
rclcpp::get_logger("hello_moveit");
: 创建一个日志记录器,用于输出日志信息,帮助调试和监控程序的运行。"hello_moveit"
是日志记录器的名称,用来标识日志信息的来源。
// 1.3 创建单线程执行器
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
-
rclcpp::executors::SingleThreadedExecutor executor;
: 创建一个SingleThreadedExecutor
对象,用于处理 ROS 节点的回调函数。SingleThreadedExecutor
是 ROS 2 中的一种执行器,表示在单线程中顺序处理回调函数。 -
executor.add_node(node);
: 将之前创建的节点node
添加到执行器中。这样执行器就可以管理该节点,并处理该节点的回调函数。
// 1.4 启动一个独立线程运行 `spin` 方法
auto spinner = std::thread([&executor]() { executor.spin(); });
-
std::thread([&executor]() { executor.spin(); });
: 使用std::thread
创建一个新线程,在这个线程中执行executor.spin();
方法。executor.spin()
会持续运行,处理与节点相关的回调函数。使用新线程的目的是让回调函数在后台持续运行,而主线程可以继续执行其他任务。 -
Lambda 表达式
[&executor]() { executor.spin(); }
: 这个 Lambda 表达式定义了一个无参数、无返回值的匿名函数,并捕获了外部变量executor
。在这个表达式中,executor.spin()
会在新线程中运行。
// 1.5 关闭 ROS 节点
rclcpp::shutdown(); // 停止 ROS 节点
spinner.join(); // 等待线程结束
return 0;
}
rclcpp::shutdown();
: 停止 ROS 系统,关闭所有的 ROS 节点。这个调用会使executor.spin()
方法退出,从而停止执行回调函数。spinner.join();
: 调用join()
方法等待线程spinner
结束。这样可以确保后台线程在主线程退出前已经完成其任务,避免线程未正确关闭引发的资源泄露或未定义行为。return 0;
: 结束main
函数并返回 0,表示程序成功执行完毕。
2. 使用 MoveIt 进行机械臂控制和可视化
理解了你的需求,以下是对代码的详细解释,按步骤分解并解释每部分的功能和使用的专业术语。
2.1 创建 MoveGroupInterface
// 包含 MoveIt 的运动规划接口
#include <moveit/move_group_interface/move_group_interface.h>
// 创建 MoveGroupInterface 对象,用于控制机械臂运动
auto move_group_interface = moveit::planning_interface::MoveGroupInterface(node, "panda_arm");
MoveGroupInterface
:MoveIt 提供的一个接口类,用于与机械臂进行交互。通过它,可以发送目标位姿、获取机械臂状态、规划和执行运动等。"panda_arm"
:指定要控制的机械臂组的名称。
2.2 初始化 MoveItVisualTools
// 包含 MoveIt Visual Tools,用于在 Rviz 中可视化机械臂状态和轨迹
#include <moveit_visual_tools/moveit_visual_tools.h>
// 初始化 MoveItVisualTools,设置基座链接和 Rviz 中的标记主题
auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools(
node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, move_group_interface.getRobotModel());
这段代码创建了一个名为 moveit_visual_tools
的对象,它是 MoveItVisualTools
类型的实例。这个对象的作用是在ROS2可视化工具Rviz中展示机械臂的状态和运动轨迹。具体来说:
node
: 是一个ROS节点,用于与ROS系统通信。"panda_link0"
: 是机械臂的基座链接,即机械臂的起始点。rviz_visual_tools::RVIZ_MARKER_TOPIC
: 是一个ROS主题,用于发布标记消息到Rviz中,以便在3D场景中显示机械臂的运动和规划路径。move_group_interface.getRobotModel()
: 获取当前机械臂的模型,这样可以确保在Rviz中正确显示机械臂的运动轨迹。
总之,这行代码的目的是初始化一个工具,使得机械臂在Rviz中的运动和规划结果可以直观地展示出来。
// 删除所有旧的标记
moveit_visual_tools.deleteAllMarkers();
// 加载远程控制界面,允许用户在 Rviz 中进行交互操作
moveit_visual_tools.loadRemoteControl();
这两行代码的作用是在使用 moveit_visual_tools
对象时进行一些准备工作,具体解释如下:
-
moveit_visual_tools.deleteAllMarkers();
- 这行代码调用了
deleteAllMarkers()
方法,用于删除在之前可能已经在 Rviz 中显示的所有标记。这样做可以确保在执行后续操作时,Rviz 中不会混杂之前的标记,从而保持界面的清晰度和准确性。
- 这行代码调用了
-
moveit_visual_tools.loadRemoteControl();
- 这行代码调用了
loadRemoteControl()
方法,该方法允许在 Rviz 中加载一个远程控制界面。这个界面使用户可以通过 Rviz 来交互地控制和调整机械臂的运动和显示,例如选择动作、设置目标位姿等操作。
- 这行代码调用了
这两行代码确保在使用 moveit_visual_tools
对象进行机械臂运动规划和可视化时,能够清除之前的标记,并提供一个交互界面,使用户可以方便地在 Rviz 中进行操作和观察。
2.3 创建用于可视化的闭包函数
// 定义闭包函数,用于在 Rviz 中显示标题文本
auto draw_title = [&moveit_visual_tools](const std::string& text) {
auto text_pose = Eigen::Isometry3d::Identity(); // 设置文本的初始位置
text_pose.translation().z() = 1.0; // 提升文本位置以便在 Rviz 中显示
moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); // 发布文本标记
};
// 定义闭包函数,提示用户进行下一步操作
auto prompt = [&moveit_visual_tools](const std::string& text) {
moveit_visual_tools.prompt(text); // 在 Rviz 中显示提示信息
};
// 定义闭包函数,绘制机械臂轨迹
auto draw_trajectory_tool_path = [&moveit_visual_tools,
jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]
(const moveit::planning_interface::MoveGroupInterface::Plan& plan) {
moveit_visual_tools.publishTrajectoryLine(plan.trajectory_, jmg); // 发布轨迹线
};
这段代码定义了三个闭包函数(Lambda 函数),用于在 MoveItVisualTools 中进行操作和可视化:
auto xxx = [...]
:使用 auto
关键字定义了一个闭包函数 xxx
。[&moveit_visual_tools]
:闭包函数通过引用捕获了外部的 moveit_visual_tools
对象。
- draw_title 在 Rviz 中绘制标题文本。
- 闭包函数
draw_title
接受一个字符串参数text
,用于在 Rviz 中显示标题文本。 Eigen::Isometry3d::Identity()
创建一个单位变换矩阵,用于设置文本的初始位置。text_pose.translation().z() = 1.0;
提升文本位置,使其在 Rviz 中更清晰可见。moveit_visual_tools.publishText(...);
发布文本标记,显示指定文本内容、颜色和大小。
- 闭包函数
- prompt 在 Rviz 中显示用户提示。
- 闭包函数
prompt
接受一个字符串参数text
,用于在 Rviz 中显示提示信息,让用户进行下一步操作。 moveit_visual_tools.prompt(text);
在 Rviz 中显示给定的提示文本,等待用户操作。
- 闭包函数
- draw_trajectory_tool_path 在 Rviz 中绘制机械臂的运动轨迹。
- 闭包函数
draw_trajectory_tool_path
在()
中接受一个MoveGroupInterface::Plan
对象plan
,用于在 Rviz 中绘制机械臂的规划轨迹。 [&moveit_visual_tools, jmg = ...]
:这里的&moveit_visual_tools
表示闭包函数中引用了外部的moveit_visual_tools
对象,闭包还引入了一个名为jmg
的局部变量。这个jmg
是通过move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")
获取的机械臂模型中的关节模型组 “panda_arm”。(const moveit::planning_interface::MoveGroupInterface::Plan& plan)
:这是闭包函数的参数列表,它接受一个类型为moveit::planning_interface::MoveGroupInterface::Plan
的引用参数plan
。这个plan
包含了机械臂的运动规划信息。moveit_visual_tools.publishTrajectoryLine(...);
调用moveit_visual_tools
对象的publishTrajectoryLine
方法,用于在 Rviz 中发布机械臂的运动轨迹线,使用给定的关节模型组jmg
,展示规划的运动路径。- 其中,
plan.trajectory_
:通过plan
参数访问机械臂的规划路径trajectory_
。jmg
:作为第二个参数,指定机械臂的关节模型组,确保轨迹线的正确显示和适配。
- 闭包函数
2.4 设置目标位姿
// 设置目标位姿,定义机械臂末端的目标位置和姿态
geometry_msgs::msg::Pose target_pose;
target_pose.orientation.w = 1.0; // 设置四元数,表示没有旋转
target_pose.position.x = 0.28;
target_pose.position.y = -0.2;
target_pose.position.z = 0.5;
// 将目标位姿设置为机械臂的运动目标
move_group_interface.setPoseTarget(target_pose);
geometry_msgs::msg::Pose
:定义机械臂末端执行器的位置和姿态。setPoseTarget
:设置目标位姿,机械臂将朝这个目标进行运动。
2.5 创建和执行规划
// 提示用户开始规划机械臂运动
prompt("Press 'Next' in the RvizVisualToolsGui window to plan");
draw_title("Planning");
// 触发 Rviz 更新以显示最新的可视化内容
moveit_visual_tools.trigger();
// 进行运动规划,获取规划结果
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if (success) {
// 如果规划成功,绘制运动轨迹并提示用户执行
draw_trajectory_tool_path(plan); // 调用绘制轨迹的闭包函数,显示机械臂运动路径
prompt("Press 'Next' in the RvizVisualToolsGui window to execute"); // 提示用户执行下一步操作
draw_title("Executing"); // 在 Rviz 中显示 "Executing" 字样,表示正在执行
// 触发 Rviz 更新以显示最新的可视化内容
moveit_visual_tools.trigger();
// 执行机械臂运动规划的路径
move_group_interface.execute(plan); // 调用 MoveGroupInterface 的 execute 方法执行规划好的路径
} else {
// 如果规划失败,在 Rviz 中显示错误信息
draw_title("Planning Failed!"); // 在 Rviz 中显示规划失败的提示信息
moveit_visual_tools.trigger(); // 触发 Rviz 更新显示
RCLCPP_ERROR(logger, "Failed to plan motion"); // 记录错误日志,提示运动规划失败
}
-
运动规划:
move_group_interface.plan(plan)
:调用MoveGroupInterface
对象的plan
方法,对机械臂从当前位置到目标位置的运动路径进行规划。这个方法会返回一个规划结果,其中包括规划好的运动路径。(... == moveit::planning_interface::MoveItErrorCode::SUCCESS)
:将plan
方法返回的规划结果与MoveItErrorCode::SUCCESS
进行比较。MoveItErrorCode::SUCCESS
是 MoveIt 提供的一个枚举值,表示规划成功的状态。success = (...);
:将比较的结果(布尔值)赋给success
变量。如果plan
方法返回的规划结果等于MoveItErrorCode::SUCCESS
(成功),则success
变量将为true
,表示运动规划成功;否则为false
,表示运动规划失败。
-
执行规划:
draw_trajectory_tool_path(plan)
:调用之前定义的闭包函数draw_trajectory_tool_path
,传入规划好的路径plan
,在 Rviz 中绘制机械臂的运动轨迹。move_group_interface.execute(plan)
:调用MoveGroupInterface
对象的execute
方法,执行之前规划好的机械臂运动路径。
-
错误处理:
- 如果
success
变量为false
,表示运动规划失败。 draw_title("Planning Failed!")
:在 Rviz 中显示 “Planning Failed!”,提示用户规划失败。RCLCPP_ERROR(logger, "Failed to plan motion")
:使用 ROS2 的日志系统记录错误信息,指出运动规划过程中出现问题。
- 如果
四、补充
1.MoveItVisualTools 功能和语法
MoveItVisualTools
是 是一个用于在 ROS Visualization (Rviz)
中可视化机械臂运动规划和执行过程的工具库。它主要用于帮助开发者和用户实时监测和调试机械臂的运动状态,提供了丰富的功能来简化机械臂操作的可视化和交互。
-
构造函数:
moveit_visual_tools::MoveItVisualTools visual_tools("base_link", "/moveit_visual_tools");
- 功能:初始化
MoveItVisualTools
对象。 - 参数:
"base_link"
:指定机械臂的基座链接。"/moveit_visual_tools"
:Rviz 中的标记主题,用于标记显示。
- 功能:初始化
-
删除所有旧标记:
visual_tools.deleteAllMarkers();
- 功能:清除 Rviz 中所有之前发布的标记,确保视图中只显示最新的标记。
-
发布文本标记:
visual_tools.publishText(pose, "Hello World", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
- 功能:在 Rviz 中显示文本信息。
- 参数:
pose
:文本的显示位置和姿态。"Hello World"
:要显示的文本内容。rviz_visual_tools::WHITE
:文本的颜色。rviz_visual_tools::XLARGE
:文本的大小。
-
加载远程控制界面:
visual_tools.loadRemoteControl();
- 功能:在 Rviz 中加载控制界面,允许用户通过点击按钮来控制和交互。
-
发布轨迹线:
visual_tools.publishTrajectoryLine(plan.trajectory_, joint_model_group);
- 功能:在 Rviz 中显示机械臂的运动轨迹。
- 参数:
plan.trajectory_
:机械臂的运动轨迹。joint_model_group
:机械臂的关节模型组,用于正确显示轨迹。
-
触发 Rviz 更新:
visual_tools.trigger();
- 功能:更新 Rviz 的显示,确保最新的标记和信息被渲染出来。
-
显示提示信息:
visual_tools.prompt("Press 'Next' to continue");
- 功能:在 Rviz 中显示提示信息,让用户知道下一步要做什么。
- 参数:
"Press 'Next' to continue"
:提示的内容。
这些功能和语法使 MoveItVisualTools
能够帮助用户在 Rviz 中实时查看和控制机械臂的动作,通过可视化和交互操作简化调试过程。
2.补充步骤
取消选中“Displays”选项卡中的“MotionPlanning”以隐藏它。 我们不打算在下一部分使用“MotionPlanning”插件。
要添加按钮以与我们添加到程序中的提示进行交互,请使用 “Panels/Add New Panel” 菜单打开对话框:
然后选择并单击 OK。 这将在左下角创建一个新面板,其中包含一个我们稍后将使用的按钮。RvizVisualToolsGui
Next
最后,我们需要添加 a 来呈现我们添加的可视化效果。 单击 “Displays” 面板中的 “Add” 按钮。Marker Array
选择并单击 。Marker Array
OK
滚动到 Displays 面板中项目的底部,然后编辑新 Marker Array 正在使用的主题。/rviz_visual_tools
这样那样完成!
总结
自用