Ros2- Moveit2 - Visualizing Collisions(可视化碰撞)

本节将引导您了解 C++ 示例代码，该代码可让您在 RViz 中移动和与机器人手臂交互时可视化机器人本身与世界之间的碰撞接触点。

入门

运行代码

使用 Roslaunch 启动文件直接从 moveit_tutorials 运行代码：

roslaunch moveit_tutorials visualizing_collisions_tutorial.launch

现在您应该看到 Panda 机器人有 2 个可拖动的交互式标记。

 

 

类

本教程的代码主要在 InteractiveRobot 类中，我们将详细讲解。InteractiveRobot 类维护了一个 RobotModel、一个 RobotState 和关于“世界”的信息（在这个案例中，“世界”是一个单独的黄色立方体）。

InteractiveRobot 类使用了 IMarker 类，该类维护了一个交互式标记。这个教程没有涵盖 IMarker 类（imarker.cpp）的实现，但大部分代码是从 basic_controls 教程中复制过来的。如果你感兴趣，可以阅读那个教程以了解更多关于交互式标记的内容。

交互

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

相关代码

完整代码可在 moveit_tutorials GitHub 项目中查看。使用的库可在此处找到。为使本教程重点关注碰撞接触，省略了大量理解此演示如何工作所需的信息。为了充分理解此演示，强烈建议您通读源代码。

初始化规划场景和标记

在本教程中，我们使用InteractiveRobot 对象作为包装器，将 robot_model 与立方体和交互式标记结合在一起。我们还创建了一个 PlanningScene 来进行碰撞检查。如果您尚未完成 规划场景教程，请先完成该教程。

InteractiveRobot robot;
g_planning_scene = new planning_scene::PlanningScene(robot.robotModel());

向 PlanningScene 添加几何图形

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);

碰撞请求

我们将为 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中 getCollisionMarkersFromContacts() 的实现 以了解具体方法。

 

if (c_res.collision)
{
  ROS_INFO("COLLIDING 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;

    /* Get the contact points and display them as markers */
    collision_detection::getCollisionMarkersFromContacts(markers, "panda_link0", c_res.contacts, color,
                                                         ros::Duration(),  // remain until deleted
                                                         0.01);            // radius
    publishMarkers(markers);
  }
}

启动文件

整个启动文件位于 GitHub上。本教程中的所有代码都可以从 moveit_tutorials 包中编译和运行。