From 5646518c1637332a169ab17c5996ce7c21d8990c Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 10 Dec 2024 11:34:54 -0600 Subject: [PATCH 1/5] 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 collision_counts; + std::map 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::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 }; From ab1a16dd9ef1df98effed985185b7ff55af4dfec Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 10 Dec 2024 13:57:37 -0600 Subject: [PATCH 2/5] Fix build warnings --- .../core/include/tesseract_collision/core/types.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index 486b553057e..4f80ff4c02a 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -296,7 +296,7 @@ class ContactResultMap std::stringstream getCollisionSummary() const { std::stringstream ss; - std::map collision_counts; + std::map collision_counts; std::map closest_distances; // Initialize distances map with max values From 69c391029a3695d49fc808dcaf5d3fa1de88d9d1 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Wed, 18 Dec 2024 10:56:01 -0600 Subject: [PATCH 3/5] switch to string output for collision summary --- .../core/include/tesseract_collision/core/types.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index 4f80ff4c02a..2e21dd12e8a 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -293,7 +293,7 @@ class ContactResultMap /** @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::string getCollisionSummary() const { std::stringstream ss; std::map collision_counts; @@ -317,8 +317,7 @@ class ContactResultMap if (collision_counts.empty()) { - ss << "No collisions detected"; - return ss; + return "No collisions detected"; } auto max_element = std::max_element(collision_counts.begin(), @@ -328,7 +327,7 @@ class ContactResultMap ss << max_element->first.first << " - " << max_element->first.second << ": " << max_element->second << " collisions, min dist: " << closest_distances[max_element->first]; - return ss; + return ss.str(); } private: From 3ca09e5fb84249faf6938681eca2deb492c23ec7 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Wed, 18 Dec 2024 11:54:52 -0600 Subject: [PATCH 4/5] Fix typo in comment specifying return type --- .../core/include/tesseract_collision/core/types.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index 2e21dd12e8a..279ebf92674 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -291,7 +291,7 @@ class ContactResultMap 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 + * @return A string containing the collision summary */ std::string getCollisionSummary() const { From f7a845c3f30f3a9e70f82827baff2b053f6c3f61 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Wed, 18 Dec 2024 14:15:20 -0600 Subject: [PATCH 5/5] Change CollisionSummary to Summary bc redundant --- .../core/include/tesseract_collision/core/types.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index 279ebf92674..86f374b7f3c 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -293,7 +293,7 @@ class ContactResultMap /** @brief Get a brief summary of the most frequently colliding link pair * @return A string containing the collision summary */ - std::string getCollisionSummary() const + std::string getSummary() const { std::stringstream ss; std::map collision_counts;