Implement a condensed summary for ContactTrajectoryResults (#1205)

Author: marrts

tesseract_collision/core/include/tesseract_collision/core/types.h

@@ -632,6 +632,8 @@ struct ContactTrajectoryResults
 
   std::stringstream collisionFrequencyPerLink() const;
 
+  std::stringstream condensedSummary() const;
+
   std::vector<ContactTrajectoryStepResults> steps;
   std::vector<std::string> joint_names;
   int total_steps = 0;

tesseract_collision/core/src/types.cpp

@@ -1023,4 +1023,62 @@ std::stringstream ContactTrajectoryResults::collisionFrequencyPerLink() const
   return ss;
 }
 
+std::stringstream ContactTrajectoryResults::condensedSummary() const
+{
+  std::stringstream ss;
+
+  if (numContacts() == 0)
+  {
+    ss << "No contacts detected\n";
+    return ss;
+  }
+
+  ss << "Contact(s) found: \n";
+  ss << "Format: Step: [link_a, link_b] @ distance\n";
+
+  // Iterate through each step to find the first collision per step
+  for (const auto& step : steps)
+  {
+    if (step.numContacts() == 0)
+      continue;
+
+    // Find the first collision in this step
+    bool found_first_collision = false;
+    for (const auto& substep : step.substeps)
+    {
+      if (substep.contacts.empty() || substep.numContacts() == 0)
+        continue;
+
+      // Get the first collision pair from this substep
+      for (const auto& collision_pair : substep.contacts.getContainer())
+      {
+        if (collision_pair.second.empty())
+          continue;
+
+        // Calculate the step with substep decimal
+        auto step_with_substep = static_cast<double>(step.step);
+        if (step.total_substeps > 0)
+        {
+          step_with_substep += static_cast<double>(substep.substep) / static_cast<double>(step.total_substeps);
+        }
+
+        // Get the first contact result from this collision pair
+        const auto& first_contact = collision_pair.second.front();
+
+        // Format: "14.4: [link_a, link_b]->0.001"
+        ss << "Step " << std::fixed << std::setprecision(1) << step_with_substep << ": [" << first_contact.link_names[0]
+           << ", " << first_contact.link_names[1] << "] @ " << std::setprecision(4) << first_contact.distance << "\n";
+
+        found_first_collision = true;
+        break;
+      }
+
+      if (found_first_collision)
+        break;
+    }
+  }
+
+  return ss;
+}
+
 }  // namespace tesseract_collision

tesseract_collision/test/collision_core_unit.cpp

@@ -1533,6 +1533,23 @@ TEST(TesseractCoreUnit, ContactTrajectoryResultsUnit)  // NOLINT
 
     EXPECT_TRUE(static_cast<bool>(results));
     EXPECT_FALSE(freq_ss.str().find("No contacts detected") != std::string::npos);
+
+    // Test condensed summary method
+    EXPECT_NO_THROW(results.condensedSummary());
+
+    std::stringstream condensed_ss = results.condensedSummary();
+    EXPECT_FALSE(condensed_ss.str().empty());
+    EXPECT_FALSE(condensed_ss.str().find("No contacts detected") != std::string::npos);
+
+    // Check that the condensed summary contains expected format: "step.substep: [link1, link2]->distance"
+    std::string condensed_output = condensed_ss.str();
+    std::cout << "Condensed Summary Output:\n" << condensed_output;
+
+    // Should contain "0.0: [link1, link2]" for first collision at step 0, substep 0
+    EXPECT_TRUE(condensed_output.find("0.0: [link1, link2]") != std::string::npos);
+
+    // Should contain "1.3: [link3, link4]" for first collision at step 1, substep 1 (1/3 = 0.3, so 1.3)
+    EXPECT_TRUE(condensed_output.find("1.3: [link3, link4]") != std::string::npos);
   }
 
   // Test with no contacts
@@ -1564,6 +1581,13 @@ TEST(TesseractCoreUnit, ContactTrajectoryResultsUnit)  // NOLINT
     EXPECT_FALSE(freq_ss.str().empty());
     EXPECT_TRUE(freq_ss.str().find("No contacts detected") != std::string::npos);
 
+    // Test condensed summary method with no contacts
+    EXPECT_NO_THROW(results.condensedSummary());
+
+    std::stringstream condensed_ss = results.condensedSummary();
+    EXPECT_FALSE(condensed_ss.str().empty());
+    EXPECT_TRUE(condensed_ss.str().find("No contacts detected") != std::string::npos);
+
     EXPECT_FALSE(static_cast<bool>(results));
   }