系列文章目录

【附带源码】机械臂MoveIt2极简教程（一）、moveit2安装
【附带源码】机械臂MoveIt2极简教程（二）、move_group交互
【附带源码】机械臂MoveIt2极简教程（三）、URDF/SRDF介绍
【附带源码】机械臂MoveIt2极简教程（四）、第一个入门demo
【附带源码】机械臂MoveIt2极简教程（五）、第二个demo - rviz可视化

---

---

本节实现的效果就是在rviz中添加文字和机械臂运动轨迹画图。

本节demo不另外创建package包，直接在上一节的demo_moveit包中运行。做的改动包括三个

• 新建C++代码
• 修改launch文件的Node
• 修改CMakeLists.txt

1. 新建C++代码

注意代码中两个地方的修改，一个是MoveGroupInterface传入的机械臂名称叫panda_arm；另一个是MoveItVisualTools中传入的tf link名称叫panda_link0。如果这两个地方没有和机械臂本体对应上，那么rviz中无法显示你需要的文字和路径图标。

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

int main(int argc, char **argv)
{
   
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("visualize_in_rviz", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

    rclcpp::executors::SingleThreadedExecutor executors;
    executors.add_node(node);
    std::thread([&executors]()
                {
    executors.spin(); })
        .detach();
    auto const logger = rclcpp::get_logger("visualize_in_rviz");
    RCLCPP_INFO(logger, "start ");

    auto move_group = moveit::planning_interface::MoveGroupInterface(node, "panda_arm");
    auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools(node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, move_group.getRobotModel());
    moveit_visual_tools.deleteAllMarkers();
    moveit_visual_tools.loadRemoteControl();

    auto const draw_title = [&moveit_visual_tools](auto text)
    {
   
        auto const text_pose = []
        {
   
            auto msg = Eigen::Isometry3d::Identity();
            msg