Add boost serialization support to config objects

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

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() = default;
   SafetyMarginData(double default_safety_margin, double default_safety_margin_coeff);
 
   /**
@@ -69,18 +72,22 @@ struct SafetyMarginData
   /// The coeff used during optimization
   /// safety margin: contacts with distance < dist_pen are penalized
   /// Stores [dist_pen, coeff]
-  std::array<double, 2> default_safety_margin_data_;
+  std::array<double, 2> default_safety_margin_data_ {0, 0};
 
   /// This use when requesting collision data because you can only provide a
   /// single contact distance threshold.
-  double max_safety_margin_;
+  double max_safety_margin_ {0};
 
   /// A map of link pair to contact distance setting [dist_pen, coeff]
   std::unordered_map<tesseract_common::LinkNamesPair, std::array<double, 2>, tesseract_common::PairHash>
       pair_lookup_table_;
 
   /// 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

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)

trajopt_common/src/utils.cpp

@@ -1,5 +1,9 @@
 #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
 {
@@ -48,6 +52,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 +85,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)