From aeb03f75b85ce07eae02d36b99f059b028f556ce Mon Sep 17 00:00:00 2001
From: Abishalini Sivaraman <abi.gpuram@gmail.com>
Date: Tue, 5 Mar 2024 09:56:07 -0700
Subject: [PATCH] Print links in collision (#2727)

---
 .../src/check_start_state_collision.cpp              | 12 +++++++++++-
 1 file changed, 11 insertions(+), 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
index f17c89f0b4..daf448bf8a 100644
--- 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;