二、RVIZ中可视化执行过程-moveit_visual_tools

目录

0.前言

1.前提条件

2.配置文件设置

3.编写功能代码

4.构建并运行

5.结果

 参考资料

---

0.前言

moveit_visual_tools工具可以在rviz中显示可视化效果来了解程序执行的操作

1.前提条件

完成第一个C++MoveIt2项目，控制机械臂到固定位姿

2.配置文件设置

在功能包的package.xml文件中添加

<depend>moveit_visual_tools</depend>

在功能包的CMakeLists.txt文件中添加

find_package(moveit_visual_tools REQUIRED)

ament_target_dependencies(
  hello_moveit
  "moveit_ros_planning_interface"
  "moveit_visual_tools"
  "rclcpp"
)

3.编写功能代码

在第一个项目的基础框架上添加executor，在spinner线程中处理节点通信相关内容

#include <memory>
#include "moveit_visual_tools/moveit_visual_tools.h"
#include "rclcpp/rclcpp.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "thread"
#include "moveit/planning_scene_interface/planning_scene_interface.h"

using moveit::planning_interface::MoveGroupInterface;
int main(int argc, char * argv[])
{
  // 初始化ROS并创建Node节点
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // 创建ROS日志
  auto const logger = rclcpp::get_logger("hello_moveit");
  RCLCPP_INFO(logger,"hello_moveit");

  // 启动单线程执行器，用于MoveItVisualTools与ROS节点交互
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // 创建MoveGroupInterface，传入参数：节点node与机器人臂名称"panda_arm"
  auto move_group_interface = MoveGroupInterface(node, "panda_arm");

  // 创建并初始化MoveItVisualTools实例
  auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{
      node, 
      "panda_link0",//作为参考坐标系的机器人基座名称
      rviz_visual_tools::RVIZ_MARKER_TOPIC,//rviz中用于可视化标记的默认话题名称
      move_group_interface.getRobotModel()//从move_group_interface中获取的机器人模型
    };
  moveit_visual_tools.deleteAllMarkers();
  // 调用loadRemoteControl方法，用于设置RViz的远端控制功能
  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;
      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);
      };

  // 设置末端执行器目标位姿
  auto const target_pose = []{
    geometry_msgs::msg::Pose msg;
    msg.orientation.w = 1.0;
    msg.position.x = 0.28;
    msg.position.y = 0.4;
    msg.position.z = 0.5;
    return msg;
  }();
  move_group_interface.setPoseTarget(target_pose);

  // 为机器人创建避免碰撞的物体
  auto const collision_object = [frame_id =
                                  move_group_interface.getPlanningFrame()] {
    moveit_msgs::msg::CollisionObject collision_object;
    collision_object.header.frame_id = frame_id;
    collision_object.id = "box1";
    shape_msgs::msg::SolidPrimitive primitive;

    // 以米为单位设置物体尺寸
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[primitive.BOX_X] = 0.5;
    primitive.dimensions[primitive.BOX_Y] = 0.1;
    primitive.dimensions[primitive.BOX_Z] = 0.5;

    // 设置避障物体的位姿
    geometry_msgs::msg::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.2;
    box_pose.position.y = 0.2;
    box_pose.position.z = 0.25;

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    return collision_object;
  }();

  // 将碰撞物添加到规划场景
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  planning_scene_interface.applyCollisionObject(collision_object);

  // 创建到目标位姿的运动规划
  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);
  }();

  // 执行运动规划
  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!");
  }
  // 关闭ROS，等待spinner线程结束
  rclcpp::shutdown();
  spinner.join();
  return 0;
}

4.构建并运行

构建当前功能包

cd ~/ws_moveit
source /opt/ros/humble/setup.bash
colcon build --packages-select hello_moveit --mixin debug

打开一个新终端，启动教程文件

cd ~/ws_moveit
source install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py

在原终端中执行程序

cd ~/ws_moveit2
source install/setup.bash
ros2 run hello_moveit hello_moveit

5.结果

在RvizVisualToolsGui面板中按下next

 参考资料

MoveIt2 官方教程