diff --git a/trajopt_common/include/trajopt_common/collision_types.h b/trajopt_common/include/trajopt_common/collision_types.h index b1246052..f87823fb 100644 --- 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 zero_coeff_; + + friend class boost::serialization::access; + template + 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 + 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 index 3f2bacec..f60e518a 100644 --- 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 #include #include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_common @@ -20,6 +22,7 @@ struct SafetyMarginData using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; + SafetyMarginData(); SafetyMarginData(double default_safety_margin, double default_safety_margin_coeff); /** @@ -81,6 +84,10 @@ struct SafetyMarginData /// Pairs containing zero coeff std::set zero_coeff_; + + friend class boost::serialization::access; + template + 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 index 00b6a3cd..21dc8668 100644 --- a/trajopt_common/src/collision_types.cpp +++ b/trajopt_common/src/collision_types.cpp @@ -23,6 +23,9 @@ */ #include +#include +#include +#include namespace trajopt_common { @@ -58,11 +61,28 @@ const std::set& CollisionCoeffData::getPairsWit return zero_coeff_; } +template +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 +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_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 index cbfbaff8..20f84dce 100644 --- a/trajopt_common/src/utils.cpp +++ b/trajopt_common/src/utils.cpp @@ -1,8 +1,14 @@ #include #include +#include +#include +#include +#include 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>& SafetyMarginData::getPairsW return zero_coeff_; } +template +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 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_SERIALIZE_ARCHIVES_INSTANTIATE(trajopt_common::SafetyMarginData) +BOOST_CLASS_EXPORT_IMPLEMENT(trajopt_common::SafetyMarginData)