Skip to content

Commit

Permalink
Get a brief string summary of a contact map (#1079)
Browse files Browse the repository at this point in the history
  • Loading branch information
marrts authored Dec 19, 2024
1 parent 6757445 commit c22037f
Showing 1 changed file with 40 additions and 0 deletions.
40 changes: 40 additions & 0 deletions tesseract_collision/core/include/tesseract_collision/core/types.h
Original file line number Diff line number Diff line change
Expand Up @@ -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 };
Expand Down

0 comments on commit c22037f

Please sign in to comment.