From c22037f83b13429ae4cfc26352c705017d534cf6 Mon Sep 17 00:00:00 2001
From: Tyler Marr <41449746+marrts@users.noreply.github.com>
Date: Thu, 19 Dec 2024 08:13:30 -0700
Subject: [PATCH] Get a brief string summary of a contact map (#1079)

---
 .../include/tesseract_collision/core/types.h  | 40 +++++++++++++++++++
 1 file changed, 40 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..86f374b7f3c 100644
--- a/tesseract_collision/core/include/tesseract_collision/core/types.h
+++ b/tesseract_collision/core/include/tesseract_collision/core/types.h
@@ -290,6 +290,46 @@ 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 containing the collision summary
+   */
+  std::string getSummary() const
+  {
+    std::stringstream ss;
+    std::map<KeyType, std::size_t> 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())
+    {
+      return "No collisions detected";
+    }
+
+    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.str();
+  }
+
 private:
   ContainerType data_;
   long count_{ 0 };