Print links in collision

Author: Abishalini

moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp

@@ -78,15 +78,28 @@ class CheckStartStateCollision : public planning_interface::PlanningRequestAdapt
     // TODO(sjahr): Would verbose make sense?
     planning_scene->checkCollision(creq, cres, start_state);
 
+    collision_detection::CollisionResult::ContactMap contacts;
+    planning_scene->getCollidingPairs(contacts);
+
     auto status = moveit::core::MoveItErrorCode();
     if (!cres.collision)
     {
       status.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
     }
     else
     {
+      collision_detection::CollisionResult::ContactMap contacts;
+      planning_scene->getCollidingPairs(contacts);
+
+      std::string contact_information = std::to_string(contacts.size()) + " contact(s) detected : ";
+
+      for (const auto& [contact_pair, contact_info] : contacts)
+      {
+        contact_information.append(contact_pair.first + " - " + contact_pair.second + ", ");
+      }
+
       status.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION;
-      status.message = std::string("Start state in collision.");
+      status.message = std::string(contact_information);
     }
     status.source = getDescription();
     return status;