【MoveIt 2】直接通过 C++ API 使用 MoveIt : 可视化碰撞 (launch文件过时）

最新推荐文章于 2025-10-12 15:23:15 发布

原创最新推荐文章于 2025-10-12 15:23:15 发布 · 829 阅读

12 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #开发语言

本节将引导您完成 C++示例代码，使您能够在 RViz 中移动和操作机器人手臂时可视化机器人自身与世界之间的碰撞接触点。

运行代码

直接从 moveit_tutorials 运行代码的启动文件：

您现在应该看到熊猫机器人和 2 个可拖动的交互标记。

课程

本教程的代码主要在 InteractiveRobot 类中，我们将在下面进行讲解。 InteractiveRobot 类维护一个 RobotModel 、一个 RobotState 以及关于“世界”的信息（在本例中，“世界”是一个黄色的立方体）。

InteractiveRobot 类使用 IMarker 类来维护一个交互式标记。本教程不涉及 IMarker 类（imarker.cpp）的实现，但大部分代码是从 basic_controls 教程中复制的，如果你感兴趣，可以在那里阅读更多关于交互式标记的信息。

互动

在 RViz 中，您将看到两组红/绿/蓝交互标记箭头。用鼠标拖动这些箭头。移动右臂，使其与左臂接触。您将看到标记接触点的品红色球体。如果没有看到品红色球体，请确保已按照上述说明添加了带有 interactive_robot_marray 主题的 MarkerArray 显示。还要确保将 RobotAlpha 设置为 0.3（或小于 1 的其他值），以便机器人是透明的，可以看到球体。移动右臂，使其与黄色立方体接触（您也可以移动黄色立方体）。您将看到标记接触点的品红色球体。

相关代码

整个代码可以在 moveit_tutorials GitHub 项目中看到。使用的库可以在这里找到。为了使本教程专注于碰撞接触，许多理解此演示工作原理所需的信息被省略了。要完全理解此演示，强烈建议您通读源代码。https://github.com/moveit/moveit2_tutorials/tree/main/doc/examples/visualizing_collisions

#include <ros/ros.h>  // ROS头文件
#include "interactivity/interactive_robot.h"  // 交互机器人头文件
#include "interactivity/pose_string.h"  // 位姿字符串头文件


// MoveIt
#include <moveit/robot_model/robot_model.h>  // 机器人模型头文件
#include <moveit/robot_state/robot_state.h>  // 机器人状态头文件
#include <moveit/planning_scene/planning_scene.h>  // 规划场景头文件
#include <moveit/collision_detection_fcl/collision_env_fcl.h>  // 碰撞环境头文件
#include <moveit/collision_detection/collision_tools.h>  // 碰撞工具头文件


planning_scene::PlanningScene* g_planning_scene = nullptr;  // 全局规划场景指针初始化为空
shapes::ShapePtr g_world_cube_shape;  // 全局世界立方体形状指针
ros::Publisher* g_marker_array_publisher = nullptr;  // 全局标记数组发布者指针初始化为空
visualization_msgs::MarkerArray g_collision_points;  // 碰撞点标记数组


void help()
{
  ROS_INFO("#####################################################");  // 打印帮助信息
  ROS_INFO("RVIZ 设置");  // RVIZ 设置
  ROS_INFO("----------");
  ROS_INFO("  全局选项:");  // 全局选项
  ROS_INFO("    FixedFrame = /panda_link0");  // 固定帧设置为 /panda_link0
  ROS_INFO("  添加 RobotState 显示:");  // 添加机器人状态显示
  ROS_INFO("    RobotDescription = robot_description");  // 机器人描述设置为 robot_description
  ROS_INFO("    RobotStateTopic  = interactive_robot_state");  // 机器人状态主题设置为 interactive_robot_state
  ROS_INFO("  添加 Marker 显示:");  // 添加标记显示
  ROS_INFO("    MarkerTopic = interactive_robot_markers");  // 标记主题设置为 interactive_robot_markers
  ROS_INFO("  添加 InteractiveMarker 显示:");  // 添加交互标记显示
  ROS_INFO("    UpdateTopic = interactive_robot_imarkers/update");  // 更新主题设置为 interactive_robot_imarkers/update
  ROS_INFO("  添加 MarkerArray 显示:");  // 添加标记数组显示
  ROS_INFO("    MarkerTopic = interactive_robot_marray");  // 标记数组主题设置为 interactive_robot_marray
  ROS_INFO("#####################################################");
}


void publishMarkers(visualization_msgs::MarkerArray& markers)
{
  // 删除旧标记
  if (!g_collision_points.markers.empty())
  {
    for (auto& marker : g_collision_points.markers)
      marker.action = visualization_msgs::Marker::DELETE;  // 将旧标记设置为删除操作


    g_marker_array_publisher->publish(g_collision_points);  // 发布旧标记删除消息
  }


  // 移动新标记到 g_collision_points
  std::swap(g_collision_points.markers, markers.markers);  // 交换新旧标记数组


  // 绘制新标记（如果有的话）
  if (!g_collision_points.markers.empty())
    g_marker_array_publisher->publish(g_collision_points);  // 发布新标记
}


void computeCollisionContactPoints(InteractiveRobot& robot)
{
  // 在碰撞世界中移动世界几何体
  Eigen::Isometry3d world_cube_pose;  // 世界立方体位姿
  double world_cube_size;  // 世界立方体大小
  robot.getWorldGeometry(world_cube_pose, world_cube_size);  // 获取世界几何体
  g_planning_scene->getWorldNonConst()->moveShapeInObject("world_cube", g_world_cube_shape, world_cube_pose);  // 移动形状


  // BEGIN_SUB_TUTORIAL computeCollisionContactPoints
  //
  // 碰撞请求
  // ^^^^^^^^^^^^^^^^^^
  // 我们将为Panda机器人创建一个碰撞请求
  collision_detection::CollisionRequest c_req;  // 碰撞请求
  collision_detection::CollisionResult c_res;  // 碰撞结果
  c_req.group_name = robot.getGroupName();  // 设置碰撞组名
  c_req.contacts = true;  // 启用接触点
  c_req.max_contacts = 100;  // 最大接触点数
  c_req.max_contacts_per_pair = 5;  // 每对最大接触点数
  c_req.verbose = false;  // 不显示详细信息


  // 检查碰撞
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // 我们检查机器人与自身或世界之间的碰撞
  g_planning_scene->checkCollision(c_req, c_res, *robot.robotState());  // 检查碰撞


  // 显示碰撞接触点
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // 如果存在碰撞，我们获取接触点并将其显示为标记。
  // **getCollisionMarkersFromContacts()** 是一个帮助函数，将
  // 碰撞接触点添加到MarkerArray消息中。如果您想将
  // 接触点用于其他用途，可以遍历 **c_res.contacts**，它是接触点的std::map。
  // 请查看 `collision_tools.cpp
  // <https://github.com/moveit/moveit/blob/noetic-devel/moveit_core/collision_detection/src/collision_tools.cpp>`_
  // 的实现方式。
  if (c_res.collision)
  {
    ROS_INFO("发生碰撞 contact_point_count=%d", (int)c_res.contact_count);  // 打印碰撞信息
    if (c_res.contact_count > 0)
    {
      std_msgs::ColorRGBA color;  // 定义颜色
      color.r = 1.0;  // 红色
      color.g = 0.0;  // 绿色
      color.b = 1.0;  // 蓝色
      color.a = 0.5;  // 透明度
      visualization_msgs::MarkerArray markers;  // 定义标记数组


      /* 获取接触点并将其显示为标记 */
      collision_detection::getCollisionMarkersFromContacts(markers, "panda_link0", c_res.contacts, color,
                                                           ros::Duration(),  // 保留直到删除
                                                           0.01);            // 半径
      publishMarkers(markers);  // 发布标记
    }
  }
  // END_SUB_TUTORIAL
  else
  {
    ROS_INFO("无碰撞");  // 打印无碰撞信息


    // 删除旧的碰撞点标记
    visualization_msgs::MarkerArray empty_marker_array;  // 定义空的标记数组
    publishMarkers(empty_marker_array);  // 发布空的标记数组
  }
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "visualizing_collisions_tutorial");  // 初始化ROS节点
  ros::NodeHandle nh;  // 创建节点句柄


  // BEGIN_TUTORIAL
  //
  // 初始化规划场景和标记
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // 在本教程中，我们使用 :codedir:`InteractiveRobot <interactivity/src/interactive_robot.cpp>`
  // 对象作为包装器，它将机器人模型与立方体和交互标记结合在一起。我们还创建了一个
  // 规划场景用于碰撞检测。如果您还没有学习过
  // :doc:`规划场景教程 </doc/examples/planning_scene/planning_scene_tutorial>`，建议您先学习。
  InteractiveRobot robot;  // 创建交互机器人对象
  g_planning_scene = new planning_scene::PlanningScene(robot.robotModel());  // 初始化规划场景


  // 向规划场景中添加几何体
  Eigen::Isometry3d world_cube_pose;  // 世界立方体位姿
  double world_cube_size;  // 世界立方体大小
  robot.getWorldGeometry(world_cube_pose, world_cube_size);  // 获取世界几何体
  g_world_cube_shape.reset(new shapes::Box(world_cube_size, world_cube_size, world_cube_size));  // 创建立方体形状
  g_planning_scene->getWorldNonConst()->addToObject("world_cube", g_world_cube_shape, world_cube_pose);  // 添加到对象


  // CALL_SUB_TUTORIAL computeCollisionContactPoints
  // END_TUTORIAL


  // 创建一个标记数组发布者用于发布接触点
  g_marker_array_publisher =
      new ros::Publisher(nh.advertise<visualization_msgs::MarkerArray>("interactive_robot_marray", 100));  // 初始化发布者


  robot.setUserCallback(computeCollisionContactPoints);  // 设置用户回调


  help();  // 调用帮助信息函数


  ros::spin();  // 进入事件循环


  delete g_planning_scene;  // 删除规划场景指针
  delete g_marker_array_publisher;  // 删除发布者指针


  ros::shutdown();  // 关闭ROS
  return 0;  // 返回
}