[For testing, do not merge] Debug Bullet self collision checks

doc/bullet_collision_checker/src/bullet_collision_checker_tutorial.cpp

@@ -94,6 +94,28 @@ void publishMarkers(visualization_msgs::MarkerArray& markers)
     g_marker_array_publisher->publish(g_collision_points);
 }
 
+void publishCollisionMarkers(collision_detection::CollisionResult& c_res)
+{
+  if (c_res.collision)
+  {
+    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 ponts 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);
+    }
+  }
+}
+
 void computeCollisionContactPoints(InteractiveRobot& robot)
 {
   // move the world geometry in the collision world
@@ -110,26 +132,15 @@ void computeCollisionContactPoints(InteractiveRobot& robot)
   c_req.max_contacts_per_pair = 5;
   c_req.verbose = false;
 
-  g_planning_scene->checkCollision(c_req, c_res, *robot.robotState());
+  g_planning_scene->checkSelfCollision(c_req, c_res, *robot.robotState());
+  ROS_INFO_STREAM("Objects in collision (printing first collision pair of "
+                                        << c_res.contacts.size() << "): " << c_res.contacts.begin()->first.first << ", "
+                                        << c_res.contacts.begin()->first.second);
 
   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 ponts 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);
-    }
+    publishCollisionMarkers(c_res);
   }
   else
   {
@@ -239,8 +250,12 @@ int main(int argc, char** argv)
   collision_detection::CollisionResult res;
   collision_detection::CollisionRequest req;
   req.contacts = true;
-  planning_scene->checkCollision(req, res);
+  planning_scene->getCollisionEnv()->checkSelfCollision(req, res, state);
   ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision."));
+  ROS_INFO_STREAM("Objects in collision (printing first collision pair of "
+                                        << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", "
+                                        << res.contacts.begin()->first.second);
+  publishCollisionMarkers(res);
   // This code is repeated for the second robot configuration.
   // END_SUB_TUTORIAL
 
@@ -283,8 +298,12 @@ int main(int argc, char** argv)
   visual_tools.publishRobotState(state);
 
   res.clear();
-  planning_scene->checkCollision(req, res);
+  planning_scene->getCollisionEnv()->checkSelfCollision(req, res, state);
   ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision."));
+  ROS_INFO_STREAM("Objects in collision (printing first collision pair of "
+                                        << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", "
+                                        << res.contacts.begin()->first.second);
+  publishCollisionMarkers(res);
 
   visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to perform a CCD check.");
 
@@ -298,6 +317,11 @@ int main(int argc, char** argv)
   res.clear();
   planning_scene->getCollisionEnv()->checkRobotCollision(req, res, state, state_before);
   ROS_INFO_STREAM_NAMED("bullet_tutorial", (res.collision ? "In collision." : "Not in collision."));
+  ROS_INFO_STREAM("Objects in collision (printing first collision pair of "
+                                        << res.contacts.size() << "): " << res.contacts.begin()->first.first << ", "
+                                        << res.contacts.begin()->first.second);
+  publishCollisionMarkers(res);
+
   // Note that the terminal output displays "In collision.".
   // END_SUB_TUTORIAL
   visualization_msgs::MarkerArray markers;