diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index 86f374b7f3c..e70b026a838 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -290,45 +290,11 @@ 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 + /** + * @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 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()) - { - 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(); - } + std::string getSummary() const; private: ContainerType data_; diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp index 6ebb651eced..21c396c02cb 100644 --- a/tesseract_collision/core/src/types.cpp +++ b/tesseract_collision/core/src/types.cpp @@ -308,6 +308,43 @@ bool ContactResultMap::operator==(const ContactResultMap& rhs) const } bool ContactResultMap::operator!=(const ContactResultMap& rhs) const { return !operator==(rhs); } +std::string ContactResultMap::getSummary() 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()) + { + 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(); +} + ContactTestData::ContactTestData(const std::vector& active, CollisionMarginData collision_margin_data, IsContactAllowedFn fn,