From 5646518c1637332a169ab17c5996ce7c21d8990c Mon Sep 17 00:00:00 2001
From: Tyler Marr <tmarr@confinityrobotics.com>
Date: Tue, 10 Dec 2024 11:34:54 -0600
Subject: [PATCH] Get a brief string summary of a contact map

---
 .../include/tesseract_collision/core/types.h  | 41 +++++++++++++++++++
 1 file changed, 41 insertions(+)

diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h
index b1b14aa3eac..486b553057e 100644
--- a/tesseract_collision/core/include/tesseract_collision/core/types.h
+++ b/tesseract_collision/core/include/tesseract_collision/core/types.h
@@ -290,6 +290,47 @@ class ContactResultMap
   bool operator==(const ContactResultMap& rhs) const;
   bool operator!=(const ContactResultMap& rhs) const;
 
+  /** @brief Get a brief summary of the most frequently colliding link pair
+   * @return A string stream containing the collision summary
+   */
+  std::stringstream getCollisionSummary() const
+  {
+    std::stringstream ss;
+    std::map<KeyType, int> collision_counts;
+    std::map<KeyType, double> closest_distances;
+
+    // Initialize distances map with max values
+    for (const auto& pair : data_)
+    {
+      if (!pair.second.empty())
+      {
+        collision_counts[pair.first] = pair.second.size();
+        closest_distances[pair.first] = std::numeric_limits<double>::max();
+
+        // Find closest distance for this pair
+        for (const auto& result : pair.second)
+        {
+          closest_distances[pair.first] = std::min(closest_distances[pair.first], result.distance);
+        }
+      }
+    }
+
+    if (collision_counts.empty())
+    {
+      ss << "No collisions detected";
+      return ss;
+    }
+
+    auto max_element = std::max_element(collision_counts.begin(),
+                                        collision_counts.end(),
+                                        [](const auto& p1, const auto& p2) { return p1.second < p2.second; });
+
+    ss << max_element->first.first << " - " << max_element->first.second << ": " << max_element->second
+       << " collisions, min dist: " << closest_distances[max_element->first];
+
+    return ss;
+  }
+
 private:
   ContainerType data_;
   long count_{ 0 };