可视化碰撞

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

入门

如果您尚未完成，请确保您已完成 Getting Started.

运行代码

Roslaunch 启动文件以直接从 moveit_tutorials 运行代码：:

roslaunch moveit_tutorials visualizing_collisions_tutorial.launch

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

类

本教程的代码主要在 InteractiveRobot 我们将在下面介绍该类。 InteractiveRobot 类维护一个 RobotModel, a RobotState, 以及有关“世界”的信息（在本例中，“世界”是一个黄色立方体）。

The InteractiveRobot class uses the IMarker 维护交互式标记的类。本教程不介绍 IMarker 类的实现 (imarker.cpp), 但大部分代码都是从 basic_controls 教程，如果您有兴趣，您可以在其中阅读有关交互式标记的更多信息。

交互

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

相关代码

完整代码可见 here 在 moveit_tutorials GitHub 项目中。使用的库可以在 here. 本教程省略了很多理解此演示如何工作所需的信息，以便将重点放在碰撞接触上。要充分理解此演示，强烈建议您通读源代码。

Initializing the Planning Scene and Markers

For this tutorial we use an InteractiveRobot object as a wrapper that combines a robot_model with the cube and an interactive marker. We also create a PlanningScene for collision checking. If you haven’t already gone through the planning scene tutorial, you go through that first.

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

Adding geometry to the 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);

Collision Requests

We will create a collision request for the Panda robot

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;

Checking for Collisions

We check for collisions between robot and itself or the world.

g_planning_scene->checkCollision(c_req, c_res, *robot.robotState());

Displaying Collision Contact Points

If there are collisions, we get the contact points and display them as markers. getCollisionMarkersFromContacts() is a helper function that adds the collision contact points into a MarkerArray message. If you want to use the contact points for something other than displaying them you can iterate through c_res.contacts which is a std::map of contact points. Look at the implementation of getCollisionMarkersFromContacts() in collision_tools.cpp for how.

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

启动文件

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