Add boost serialization support to config objects

tesseract-robotics · Dec 6, 2024 · d5a2101 · d5a2101
1 parent 29ce7ea
commit d5a2101
Show file tree

Hide file tree

Showing 4 changed files with 69 additions and 0 deletions.
diff --git a/trajopt_common/include/trajopt_common/collision_types.h b/trajopt_common/include/trajopt_common/collision_types.h
@@ -87,6 +87,10 @@ struct CollisionCoeffData
 
   /// Pairs containing zero coeff
   std::set<tesseract_common::LinkNamesPair> zero_coeff_;
+
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
 };
 
 /**
@@ -113,6 +117,11 @@ struct TrajOptCollisionConfig : public tesseract_collision::CollisionCheckConfig
    * It still finds all contacts but sorts based on the worst uses those up to the max_num_cnt.
    */
   int max_num_cnt{ 3 };
+
+protected:
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
 };
 
 /** @brief A data structure to contain a links gradient results */
@@ -264,4 +273,8 @@ struct CollisionCacheData
 };
 
 }  // namespace trajopt_common
+
+BOOST_CLASS_EXPORT_KEY(trajopt_common::CollisionCoeffData)
+BOOST_CLASS_EXPORT_KEY(trajopt_common::TrajOptCollisionConfig)
+
 #endif  // TRAJOPT_COMMON_COLLISION_TYPES_H
diff --git a/trajopt_common/include/trajopt_common/utils.hpp b/trajopt_common/include/trajopt_common/utils.hpp
@@ -10,6 +10,8 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <array>
 #include <set>
 #include <tesseract_common/types.h>
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/export.hpp>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 namespace trajopt_common
@@ -20,6 +22,7 @@ struct SafetyMarginData
   using Ptr = std::shared_ptr<SafetyMarginData>;
   using ConstPtr = std::shared_ptr<const SafetyMarginData>;
 
+  SafetyMarginData();
   SafetyMarginData(double default_safety_margin, double default_safety_margin_coeff);
 
   /**
@@ -81,6 +84,10 @@ struct SafetyMarginData
 
   /// Pairs containing zero coeff
   std::set<tesseract_common::LinkNamesPair> zero_coeff_;
+
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
 };
 
 /**
@@ -116,4 +123,7 @@ Eigen::Isometry3d addTwist(const Eigen::Isometry3d& t1,
                            double dt);
 }  // namespace trajopt_common
 
+BOOST_CLASS_EXPORT_KEY(trajopt_common::SafetyMarginData)
+
+
 #endif  // TRAJOPT_COMMON_UTILS_HPP
diff --git a/trajopt_common/src/collision_types.cpp b/trajopt_common/src/collision_types.cpp
@@ -23,6 +23,9 @@
  */
 
 #include <trajopt_common/collision_types.h>
+#include <tesseract_collision/core/serialization.h>
+#include <boost/serialization/unordered_map.hpp>
+#include <boost/serialization/set.hpp>
 
 namespace trajopt_common
 {
@@ -58,11 +61,28 @@ const std::set<tesseract_common::LinkNamesPair>& CollisionCoeffData::getPairsWit
   return zero_coeff_;
 }
 
+template <class Archive>
+void CollisionCoeffData::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_NVP(default_collision_coeff_);
+  ar& BOOST_SERIALIZATION_NVP(lookup_table_);
+  ar& BOOST_SERIALIZATION_NVP(zero_coeff_);
+}
+
 TrajOptCollisionConfig::TrajOptCollisionConfig(double margin, double coeff)
   : CollisionCheckConfig(margin), collision_coeff_data(coeff)
 {
 }
 
+template <class Archive>
+void TrajOptCollisionConfig::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(CollisionCheckConfig);
+  ar& BOOST_SERIALIZATION_NVP(collision_coeff_data);
+  ar& BOOST_SERIALIZATION_NVP(collision_margin_buffer);
+  ar& BOOST_SERIALIZATION_NVP(max_num_cnt);
+}
+
 double LinkMaxError::getMaxError() const
 {
   if (has_error[0] && has_error[1])
@@ -261,3 +281,10 @@ double GradientResultsSet::getMaxErrorWithBufferT1() const
 }
 
 }  // namespace trajopt_common
+
+
+#include <tesseract_common/serialization.h>
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(trajopt_common::CollisionCoeffData)
+BOOST_CLASS_EXPORT_IMPLEMENT(trajopt_common::CollisionCoeffData)
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(trajopt_common::TrajOptCollisionConfig)
+BOOST_CLASS_EXPORT_IMPLEMENT(trajopt_common::TrajOptCollisionConfig)
diff --git a/trajopt_common/src/utils.cpp b/trajopt_common/src/utils.cpp
@@ -1,8 +1,14 @@
 #include <trajopt_common/utils.hpp>
 #include <tesseract_common/utils.h>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/array.hpp>
+#include <boost/serialization/set.hpp>
+#include <boost/serialization/unordered_map.hpp>
 
 namespace trajopt_common
 {
+
+SafetyMarginData::SafetyMarginData() : SafetyMarginData(0, 10) {}
 SafetyMarginData::SafetyMarginData(double default_safety_margin, double default_safety_margin_coeff)
   : default_safety_margin_data_({ default_safety_margin, default_safety_margin_coeff })
   , max_safety_margin_(default_safety_margin)
@@ -48,6 +54,15 @@ const std::set<std::pair<std::string, std::string>>& SafetyMarginData::getPairsW
   return zero_coeff_;
 }
 
+template <class Archive>
+void SafetyMarginData::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_NVP(default_safety_margin_data_);
+  ar& BOOST_SERIALIZATION_NVP(max_safety_margin_);
+  ar& BOOST_SERIALIZATION_NVP(pair_lookup_table_);
+  ar& BOOST_SERIALIZATION_NVP(zero_coeff_);
+}
+
 std::vector<SafetyMarginData::Ptr> createSafetyMarginDataVector(int num_elements,
                                                                 double default_safety_margin,
                                                                 double default_safety_margin_coeff)
@@ -72,3 +87,7 @@ Eigen::Isometry3d addTwist(const Eigen::Isometry3d& t1,
   return t2;
 }
 }  // namespace trajopt_common
+
+#include <tesseract_common/serialization.h>
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(trajopt_common::SafetyMarginData)
+BOOST_CLASS_EXPORT_IMPLEMENT(trajopt_common::SafetyMarginData)