本教程将向您介绍一个工具，通过在 RViz 中呈现可视化效果，该工具可以帮助您更轻松地了解 MoveIt 应用程序正在做什么。
先决条件
如果你还没有完成，请确保你已经完成了你的第一个项目hello_moveit中的步骤。这个项目假设你从上一个教程结束的地方开始。
步骤
1 添加依赖 moveit_visual_tools
package.xml在项目中的hello_moveit其他语句后面添加此行<depend>：
<depend>moveit_visual_tools</depend>
然后在语句CMakeLists.txt部分中添加此行find_package：
find_package(moveit_visual_tools REQUIRED)
在文件的下方扩展ament_target_dependencies宏调用以包含新的依赖项，如下所示：
ament_target_dependencies( hello_moveit "moveit_ros_planning_interface" "moveit_visual_tools" "rclcpp" )
为了验证您是否正确添加了依赖项，请将所需的包含内容添加到源文件中hello_moveit.cpp：
#include <moveit_visual_tools/moveit_visual_tools.h>
为了测试这一切是否有效，请在工作区目录中打开一个终端（记住在 opt 中获取 ROS 安装的源代码），然后使用 colcon 进行构建：
cd ~/ws_moveit colcon build --mixin debug
2 创建 ROS 执行器并在线程上旋转节点
在初始化 MoveItVisualTools 之前，我们需要在 ROS 节点上启动一个执行器。这是必要的，因为 MoveItVisualTools 与 ROS 服务和主题交互。首先，将线程库添加到顶部的包含中。
#include <thread> // <---- add this to the set of includes at the top
通过创建和命名记录器，我们可以保持程序日志井然有序。
// Create a ROS logger
auto const logger = rclcpp::get_logger("hello_moveit");
接下来，在创建 MoveIt MoveGroup 接口之前添加执行器。
// Spin up a SingleThreadedExecutor for MoveItVisualTools to interact with ROS
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
auto spinner = std::thread([&executor]() { executor.spin(); });
// Create the MoveIt MoveGroup Interface
...
最后，退出之前请确保加入线程。
// Shutdown ROS rclcpp::shutdown(); // <--- This will cause the spin function in the thread to return spinner.join(); // <--- Join the thread before exiting return 0;
进行这些更改后，重建工作区以确保没有任何语法错误。
3 创建并初始化 MoveItVisualTools
接下来我们在构造MoveGroupInterface之后构造并初始化MoveItVisualTools。
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "manipulator");
// Construct and initialize MoveItVisualTools
auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{
node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC,
move_group_interface.getRobotModel()};
moveit_visual_tools.deleteAllMarkers();
moveit_visual_tools.loadRemoteControl();
我们将以下内容传递到构造函数中：ROS 节点、机器人的基本链接、要使用的标记主题（稍后会详细介绍）以及机器人模型（我们从 move_group_interface 中获取）。接下来，我们调用删除所有标记。这将清除 RViz 中我们之前运行遗留的任何渲染状态。最后，我们加载远程控制。远程控制是一个非常简单的插件，它让我们在 RViz 中有一个按钮来与我们的程序交互。
4 为可视化编写闭包
在我们构造和初始化之后，我们现在创建一些闭包（可以访问当前范围内的变量的函数对象），我们可以稍后在程序中使用这些闭包来帮助在 RViz 中呈现可视化效果。
// Create closures for visualization
auto const draw_title = [&moveit_visual_tools](auto text) {
auto const text_pose = [] {
auto msg = Eigen::Isometry3d::Identity();
msg.translation().z() = 1.0; // Place text 1m above the base link
return msg;
}();
moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE,
rviz_visual_tools::XLARGE);
};
auto const prompt = [&moveit_visual_tools](auto text) {
moveit_visual_tools.prompt(text);
};
auto const draw_trajectory_tool_path =
[&moveit_visual_tools,
jmg = move_group_interface.getRobotModel()->getJointModelGroup(
"manipulator")](auto const trajectory) {
moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
};
三个闭包中的每一个都moveit_visual_tools通过引用捕获，最后一个闭包捕获指向我们正在规划的联合模型组对象的指针。它们中的每一个都调用一个函数moveit_visual_tools来更改 RViz 中的某些内容。
-
第一个，
draw_title在机器人底座上方一米处添加文本。这是一种从高层次展示程序状态的有效方法。 -
第二个调用一个名为的函数
prompt。此函数会阻止您的程序，直到用户按下nextRViz 中的按钮。这对于在调试时逐步执行程序很有帮助。 -
最后一个绘制了我们规划的轨迹的轨迹路径。这通常有助于从轨迹的角度理解规划的轨迹。
您可能会问自己，我们为什么要创建这样的 lambda，原因很简单，就是为了让后面的代码更易于阅读和理解。在编写软件时，将功能分解为命名函数通常会很有帮助，这些函数可以轻松重复使用并自行测试。您将在下一节中看到我们如何使用我们创建的这些函数。
5 可视化你的程序步骤
现在我们将扩充程序中间的代码。更新规划和执行代码以包含以下新功能：
// Set a target Pose
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);
// Create a plan to that target pose
prompt("Press 'Next' in the RvizVisualToolsGui window to plan");
draw_title("Planning");
moveit_visual_tools.trigger();
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) {
draw_trajectory_tool_path(plan.trajectory);
moveit_visual_tools.trigger();
prompt("Press 'Next' in the RvizVisualToolsGui window to execute");
draw_title("Executing");
moveit_visual_tools.trigger();
move_group_interface.execute(plan);
} else {
draw_title("Planning Failed!");
moveit_visual_tools.trigger();
RCLCPP_ERROR(logger, "Planning failed!");
}
您很快就会注意到，每次调用后，我们都必须调用一个方法，trigger以moveit_visual_tools更改 RViz 中呈现的内容。这样做的原因是，发送到 RViz 的消息会被分批发送，并在您调用时发送，trigger以减少标记主题的带宽。
最后，再次构建您的项目以确保所有代码添加都是正确的。
cd ~/ws_moveit source /opt/ros/rolling/setup.bash colcon build --mixin debug
6 在 RViz 中启用可视化
打开一个新终端，获取工作区，然后启动打开 RViz 的演示启动文件。
cd ~/ws_moveit source install/setup.bash ros2 launch moveit2_tutorials demo.launch.py
取消选中“Displays”选项卡中的“MotionPlanning”以将其隐藏。我们将不会在下一部分中使用“MotionPlanning”插件。
要添加按钮来与我们添加到程序中的提示进行交互，请使用“面板/添加新面板”菜单打开对话框：
然后选择RvizVisualToolsGui并单击“确定”。这将在左下角创建一个新面板，其中包含一个Next我们稍后会使用的按钮。
最后，我们需要添加一个来呈现我们添加的可视化效果。单击“显示”面板中的“添加”按钮。Marker Array
选择并单击。Marker ArrayOK
滚动到显示面板中的项目底部并编辑新标记阵列正在使用的主题/rviz_visual_tools。
您现在可以运行带有可视化效果的新程序了。
7 运行程序
在新终端中，转到工作区，获取工作区并运行hello_moveit：
cd ~/ws_moveit source install/setup.bash ros2 run hello_moveit hello_moveit
您会注意到您的程序已停止并显示如下日志：
[INFO] [1652822889.492940200] [hello_moveit.remote_control]: Waiting to continue: Press 'Next' in the RvizVisualToolsGui window to plan
单击NextRViz 中的按钮并查看您的应用程序进展。
单击下一步按钮后，您将看到您的应用程序已规划，在机器人上方添加了标题，并绘制了一条代表工具路径的线。要继续，请Next再次按下以查看您的机器人执行计划。
