系列文章目录

【附带源码】机械臂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.translation().z() = 1.0; // Place text 1m above the base linkreturn 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.getRobotModel()->getJointModelGroup("panda_arm")](auto const trajectory){moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);};// Set a target Poseauto 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.setPoseTarget(target_pose);// Create a plan to that target poseprompt("Press 'Next' in the RvizVisualToolsGui window to plan");draw_title("Planning");moveit_visual_tools.trigger();auto const [success, plan] = [&move_group]{moveit::planning_interface::MoveGroupInterface::Plan msg;auto const ok = static_cast<bool>(move_group.plan(msg));return std::make_pair(ok, msg);}();// Execute the planif (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.execute(plan);}else{draw_title("Planning Failed!");moveit_visual_tools.trigger();RCLCPP_ERROR(logger, "Planning failed!");}rclcpp::shutdown();return 0;
}

2. 修改launch文件

沿用上一节的launch文件demo_moveit.launch.py

import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import xacrodef load_file(package_name, file_path):package_path = get_package_share_directory(package_name)absolute_file_path = os.path.join(package_path, file_path)try:with open(absolute_file_path, "r") as file:return file.read()except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where availablereturn Nonedef load_yaml(package_name, file_path):package_path = get_package_share_directory(package_name)absolute_file_path = os.path.join(package_path, file_path)try:with open(absolute_file_path, "r") as file:return yaml.safe_load(file)except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where availablereturn Nonedef generate_launch_description():robot_description_config = xacro.process_file(os.path.join(get_package_share_directory("moveit_resources_panda_moveit_config"),"config","panda.urdf.xacro",))robot_description = {"robot_description": robot_description_config.toxml()}robot_description_semantic_config = load_file("moveit_resources_panda_moveit_config", "config/panda.srdf")robot_description_semantic = {"robot_description_semantic": robot_description_semantic_config}kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config", "config/kinematics.yaml")# 第四节# move_group_demo = Node(#     package="demo_moveit",#     executable="demo_moveit",#     name="demo_moveit",#     output="screen",#     parameters=[robot_description, #                 robot_description_semantic, #                 kinematics_yaml,#                 ],# )# 第五节move_group_demo = Node(package="demo_moveit",executable="visualize_in_rviz",name="visualize_in_rviz",output="screen",parameters=[robot_description, robot_description_semantic, kinematics_yaml,],)return LaunchDescription([move_group_demo])

3. 修改CMakeLists.txt

依旧沿用上一节的CMakeLists.txt，只是添加了新的节点

cmake_minimum_required(VERSION 3.10.2)
project(demo_moveit)# Common cmake code applied to all moveit packages
find_package(moveit_common REQUIRED)
moveit_package()find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED system filesystem date_time thread)
find_package(ament_cmake REQUIRED)
find_package(control_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_ros_perception REQUIRED)
find_package(moveit_servo REQUIRED)
find_package(interactive_markers REQUIRED)
find_package(rviz_visual_tools REQUIRED)
find_package(moveit_visual_tools REQUIRED)
find_package(geometric_shapes REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(pluginlib REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)set(THIS_PACKAGE_INCLUDE_DIRSdoc/interactivity/include
)set(THIS_PACKAGE_INCLUDE_DEPENDSament_cmakerclcpprclcpp_actiontf2_geometry_msgstf2_rosmoveit_corerviz_visual_toolsmoveit_visual_toolsmoveit_ros_planning_interfaceinteractive_markerstf2_geometry_msgsmoveit_ros_planningpluginlibEigen3Boostcontrol_msgsmoveit_servo
)include_directories(${THIS_PACKAGE_INCLUDE_DIRS})ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}
)ament_export_include_directories(include)# -------------------------------------------------------------- executable
add_executable(demo_moveitsrc/demo_moveit.cpp)target_include_directories(demo_moveitPUBLIC include)ament_target_dependencies(demo_moveit${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)# -------------------------------------------------------------- executable
add_executable(visualize_in_rvizsrc/visualize_in_rviz.cpp)target_include_directories(visualize_in_rvizPUBLIC include)ament_target_dependencies(visualize_in_rviz${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)# -------------------------------------------------------------- installinstall(TARGETS demo_moveit visualize_in_rviz planning_around_objectsDESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY launchDESTINATION share/${PROJECT_NAME}
)ament_package()

4. 运行

• 进入工作空间，编译项目

cd ~/ws_moveitcolcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select demo_moveit

• 启动panda_arm机械臂

ros2 launch moveit2_tutorials demo.launch.py

1. 记得先把MotionPlanning关闭，因为目前用不到;
2. 添加插件RvizVisualToolsGui
3. 添加MarkerArray，topic选择/rviz_visual_tools

• 启动本节程序

ros2 launch demo_moveit demo_moveit.launch.py

• 点击RvizVisualToolsGui的Next按钮，即可看到机械臂运动动画，如文章开头的视频展示的一样。

---

觉得对您有帮助的，可以点个赞👍支持一下，谢谢各位！

因为淋过雨，所以想为别人撑把伞；因为踩过太多坑，所以想让喜欢机器人的同学们减少试错成本！