官方文档上的教程，从moveit1的melodic到moveit2的foxy基本一致，但是从最新的humble开始有了很大的变化，其中之一便是 lambda表达式 的广泛使用。

本节为教程的第二节，会介绍一个工具（moveit_visual_tools），它能够通过在 rviz 中呈现可视化，从而帮助你更容易理解你的 Moveit 应用在做什么。

1 添加 moveit_visual_tools 依赖包

把下面这行添加到 hello_moveit 项目下 package.xml 中其它<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路径：

#include <moveit_visual_tools/moveit_visual_tools.h>

保存后打开一个新终端，编译看是否报错。

2 创建 ROS 执行器并在线程中循环节点

在初始化 MoveItVisualTools 之前，需要有一个执行器在 ROS 节点上不断循环。

这是必要的操作，来让MoveItVisualTools与 ROS服务和话题交互。

#include <thread>  // <---- add this to the set of includes at the top...// 创建一个 ROS loggerauto const logger = rclcpp::get_logger("hello_moveit");// 循环一个单线程执行器来让 MoveItTools 与 ROS 交互rclcpp::executors::SingleThreadedExecutor executor;executor.add_node(node);// 实例化一个线程对象，使用 lambda表达式来构造auto spinner = std::thread([&executor]() { executor.spin(); });// Create the MoveIt MoveGroup Interface...// Shutdown ROSrclcpp::shutdown();  // <--- 这将会使线程中的回调函数返回spinner.join();  // <--- 在结束前加入子线程，等待子线程结束，主进程才可以退出return 0;
}

然后重新编译工作空间确保没有语法错误。

3 创建并初始化 MoveItVisualTools

在创建 MoveGroupInterface 后创建和初始化 MoveItVisualTools：

// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");// Construct and initialize MoveItVisualTools
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();

我们传递以下参数到构造函数中：

1. ROS节点
2. 机械臂的 base link
3. 要使用的 marker 话题（稍后介绍）
4. 机器人模型 robot model（从 move_group_interface 中获得）

下一步，我们调用其成员函数去删除所有的标记，这会清除 Rviz 中所有以前运行遗留下来的绘制状态。

最后，加载远程控制。远程控制是一个简单的插件，它能在 Rviz 中提供一个按钮来与我们的程序进行交互。

4 为可视化编写闭包

在初始化后，我们创建一些闭包（闭包是引用了自由变量的函数），这样我们就能在程序的后面调用这些闭包来帮助在 Rviz 中描述可视化。

// 创建用于可视化的闭包
auto const draw_title = [&moveit_visual_tools](auto text) {auto const text_pose = [] {auto msg = Eigen::Isometry3d::Identity();msg.translation().z() = 1.0;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("panda_arm")](auto const trajectory) {moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);};

这三个闭包都引用了 moveit_visual_tools，最后一个闭包引用了一个指向我们所规划的关节组对象的指针。

这些闭包调用了一个moveit_visual_tools上的函数，改变了 Rviz 中的一些东西：

1. 第一个闭包draw_title：在 RViz 中机械臂的上方一米处添加了文本，方便看运行状态；
2. 第二个闭包prompt：这个函数会阻止程序进行，直到用户按下 RViz 中的next按钮，方便 debug；
3. 第三个闭包draw_trajectory_tool_path ：画出了我们规划的轨迹路径。

你可能会好奇我们为什么要用 lambda 表达式。原因是它能更简单地编码，便于以后阅读和理解。当你写软件时，把功能分解为命名函数，有助于复用和测试。

5 Visualize the steps of your program

// 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, "Planing failed!");
}

source install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py

在 RViz 中，取消选择 “Displays”中的 “MotionPlanning”，这节中用不到这个插件

[INFO] [1652822889.492940200] [hello_moveit.remote_control]: Waiting to continue: Press 'Next' in the RvizVisualToolsGui window to plan