本教程将向您介绍一个工具,通过在 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
。此函数会阻止您的程序,直到用户按下next
RViz 中的按钮。这对于在调试时逐步执行程序很有帮助。 -
最后一个绘制了我们规划的轨迹的轨迹路径。这通常有助于从轨迹的角度理解规划的轨迹。
您可能会问自己,我们为什么要创建这样的 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”插件。
![../../../_images/uncheck_motion_planning.png](https://moveit.picknik.ai/main/_images/uncheck_motion_planning.png)
![../../../_images/unchecked_motion_planning.png](https://moveit.picknik.ai/main/_images/unchecked_motion_planning.png)
要添加按钮来与我们添加到程序中的提示进行交互,请使用“面板/添加新面板”菜单打开对话框:
![../../../_images/panel_menu.png](https://moveit.picknik.ai/main/_images/panel_menu.png)
然后选择RvizVisualToolsGui
并单击“确定”。这将在左下角创建一个新面板,其中包含一个Next
我们稍后会使用的按钮。
![../../../_images/add_rviz_tools_gui.png](https://moveit.picknik.ai/main/_images/add_rviz_tools_gui.png)
![../../../_images/next_button.png](https://moveit.picknik.ai/main/_images/next_button.png)
最后,我们需要添加一个来呈现我们添加的可视化效果。单击“显示”面板中的“添加”按钮。Marker Array
![../../../_images/add_button.png](https://moveit.picknik.ai/main/_images/add_button.png)
选择并单击。Marker Array
OK
![../../../_images/marker_array.png](https://moveit.picknik.ai/main/_images/marker_array.png)
滚动到显示面板中的项目底部并编辑新标记阵列正在使用的主题/rviz_visual_tools
。
![../../../_images/marker_array_topic.png](https://moveit.picknik.ai/main/_images/marker_array_topic.png)
您现在可以运行带有可视化效果的新程序了。
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
单击Next
RViz 中的按钮并查看您的应用程序进展。
![../../../_图像/规划.png](https://moveit.picknik.ai/main/_images/planning.png)
单击下一步按钮后,您将看到您的应用程序已规划,在机器人上方添加了标题,并绘制了一条代表工具路径的线。要继续,请Next
再次按下以查看您的机器人执行计划。
![../../../_images/executing.png](https://moveit.picknik.ai/main/_images/executing.png)