Add any poly support for collision types

tesseract-robotics · Aug 9, 2024 · 996931f · 996931f
1 parent 8502a4d
commit 996931f
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 1 deletion.
diff --git a/tesseract_collision/core/include/tesseract_collision/core/serialization.h b/tesseract_collision/core/include/tesseract_collision/core/serialization.h
@@ -26,7 +26,9 @@
 #ifndef TESSERACT_COLLISION_SERIALIZATION_H
 #define TESSERACT_COLLISION_SERIALIZATION_H
 
-#include <tesseract_collision/core/fwd.h>
+#include <tesseract_collision/core/types.h>
+#include <tesseract_common/any_poly.h>
+#include <vector>
 
 namespace boost::serialization
 {
@@ -53,4 +55,8 @@ template <class Archive>
 void serialize(Archive& ar, tesseract_collision::ContactResultMap& g, const unsigned int version);  // NOLINT
 }  // namespace boost::serialization
 
+TESSERACT_ANY_EXPORT_KEY(tesseract_collision::ContactResult, TesseractCollisionContactResult)
+TESSERACT_ANY_EXPORT_KEY(tesseract_collision::ContactResultMap, TesseractCollisionContactResultMap)
+TESSERACT_ANY_EXPORT_KEY(std::vector<tesseract_collision::ContactResultMap>, TesseractCollisionContactResultMapVector)
+
 #endif  // TESSERACT_COLLISION_SERIALIZATION_H
diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h
@@ -133,6 +133,9 @@ struct ContactResult
 
   /** @brief reset to default values */
   void clear();
+
+  bool operator==(const ContactResult& rhs) const;
+  bool operator!=(const ContactResult& rhs) const;
 };
 
 using ContactResultVector = tesseract_common::AlignedVector<ContactResult>;
@@ -284,6 +287,9 @@ class ContactResultMap
   const ContactResultVector& at(const KeyType& key) const;
   ConstIteratorType find(const KeyType& key) const;
 
+  bool operator==(const ContactResultMap& rhs) const;
+  bool operator!=(const ContactResultMap& rhs) const;
+
 private:
   ContainerType data_;
   long count_{ 0 };

diff --git a/tesseract_collision/core/src/serialization.cpp b/tesseract_collision/core/src/serialization.cpp
@@ -121,3 +121,7 @@ void serialize(Archive& ar, tesseract_collision::ContactResultMap& g, const unsi
 #include <tesseract_common/serialization.h>
 TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactResult)
 TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactResultMap)
+
+TESSERACT_ANY_EXPORT_IMPLEMENT(TesseractCollisionContactResult)
+TESSERACT_ANY_EXPORT_IMPLEMENT(TesseractCollisionContactResultMap)
+TESSERACT_ANY_EXPORT_IMPLEMENT(TesseractCollisionContactResultMapVector)
diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp
@@ -58,6 +58,32 @@ void ContactResult::clear()
   single_contact_point = false;
 }
 
+bool ContactResult::operator==(const ContactResult& rhs) const
+{
+  bool ret_val = true;
+  ret_val &= tesseract_common::almostEqualRelativeAndAbs(distance, rhs.distance);
+  ret_val &= tesseract_common::almostEqualRelativeAndAbs(nearest_points[0], rhs.nearest_points[0]);
+  ret_val &= tesseract_common::almostEqualRelativeAndAbs(nearest_points[1], rhs.nearest_points[1]);
+  ret_val &= tesseract_common::almostEqualRelativeAndAbs(nearest_points_local[0], rhs.nearest_points_local[0]);
+  ret_val &= tesseract_common::almostEqualRelativeAndAbs(nearest_points_local[1], rhs.nearest_points_local[1]);
+  ret_val &= transform[0].isApprox(rhs.transform[0]);
+  ret_val &= transform[1].isApprox(rhs.transform[1]);
+  ret_val &= (link_names == rhs.link_names);
+  ret_val &= (shape_id == rhs.shape_id);
+  ret_val &= (subshape_id == rhs.subshape_id);
+  ret_val &= (type_id == rhs.type_id);
+  ret_val &= tesseract_common::almostEqualRelativeAndAbs(normal, rhs.normal);
+  ret_val &= tesseract_common::almostEqualRelativeAndAbs(cc_time[0], rhs.cc_time[0]);
+  ret_val &= tesseract_common::almostEqualRelativeAndAbs(cc_time[1], rhs.cc_time[1]);
+  ret_val &= (cc_type == rhs.cc_type);
+  ret_val &= cc_transform[0].isApprox(rhs.cc_transform[0]);
+  ret_val &= cc_transform[1].isApprox(rhs.cc_transform[1]);
+  ret_val &= (single_contact_point == rhs.single_contact_point);
+
+  return ret_val;
+}
+bool ContactResult::operator!=(const ContactResult& rhs) const { return !operator==(rhs); }
+
 ContactRequest::ContactRequest(ContactTestType type) : type(type) {}
 
 ContactResult& ContactResultMap::addContactResult(const KeyType& key, ContactResult result)
@@ -276,6 +302,12 @@ void ContactResultMap::filter(const FilterFn& filter)
   assert(count_ >= 0);
 }
 
+bool ContactResultMap::operator==(const ContactResultMap& rhs) const
+{
+  return ((data_ == rhs.data_) && (count_ == rhs.count_));
+}
+bool ContactResultMap::operator!=(const ContactResultMap& rhs) const { return !operator==(rhs); }
+
 ContactTestData::ContactTestData(const std::vector<std::string>& active,
                                  CollisionMarginData collision_margin_data,
                                  IsContactAllowedFn fn,