Print links in collision (#2727)

moveit · Mar 5, 2024 · aeb03f7 · aeb03f7
1 parent 680b783
commit aeb03f7
Showing 1 changed file with 11 additions and 1 deletion.
diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp
@@ -85,8 +85,18 @@ class CheckStartStateCollision : public planning_interface::PlanningRequestAdapt
     }
     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;