From 82820da663f4ccdc9dd9224bbd15df70b3c9133b Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 6 Dec 2024 09:16:07 -0600 Subject: [PATCH] Add more serialization for eigen types and collision types --- .../tesseract_collision/core/serialization.h | 31 +++++-- .../core/src/serialization.cpp | 91 ++++++++++++++----- .../tesseract_common/eigen_serialization.h | 19 ++-- .../include/tesseract_common/serialization.h | 10 ++ tesseract_common/src/eigen_serialization.cpp | 11 +++ 5 files changed, 122 insertions(+), 40 deletions(-) diff --git a/tesseract_collision/core/include/tesseract_collision/core/serialization.h b/tesseract_collision/core/include/tesseract_collision/core/serialization.h index 585bca058b7..835b74c1d6a 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/serialization.h +++ b/tesseract_collision/core/include/tesseract_collision/core/serialization.h @@ -32,27 +32,40 @@ namespace boost::serialization { -/************************************************/ -/****** tesseract_collision::ContactResult ******/ -/************************************************/ +template +void serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version); // NOLINT template -void save(Archive& ar, const tesseract_collision::ContactResult& g, const unsigned int version); // NOLINT +void save(Archive& ar, const tesseract_collision::ContactResultMap& g, const unsigned int version); // NOLINT template -void load(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version); // NOLINT +void load(Archive& ar, tesseract_collision::ContactResultMap& g, const unsigned int version); // NOLINT template -void serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version); // NOLINT +void serialize(Archive& ar, tesseract_collision::ContactResultMap& g, const unsigned int version); // NOLINT template -void save(Archive& ar, const tesseract_collision::ContactResultMap& g, const unsigned int version); // NOLINT +void serialize(Archive& ar, tesseract_collision::ContactRequest& g, const unsigned int version); // NOLINT template -void load(Archive& ar, tesseract_collision::ContactResultMap& g, const unsigned int version); // NOLINT +void serialize(Archive& ar, tesseract_collision::ContactManagerConfig& g, const unsigned int version); // NOLINT template -void serialize(Archive& ar, tesseract_collision::ContactResultMap& g, const unsigned int version); // NOLINT +void serialize(Archive& ar, tesseract_collision::CollisionCheckConfig& g, const unsigned int version); // NOLINT + +template +void serialize(Archive& ar, + tesseract_collision::ContactTrajectorySubstepResults& g, + const unsigned int version); // NOLINT + +template +void serialize(Archive& ar, + tesseract_collision::ContactTrajectoryStepResults& g, + const unsigned int version); // NOLINT + +template +void serialize(Archive& ar, tesseract_collision::ContactTrajectoryResults& g, const unsigned int version); // NOLINT + } // namespace boost::serialization TESSERACT_ANY_EXPORT_KEY(tesseract_collision::ContactResult, TesseractCollisionContactResult) diff --git a/tesseract_collision/core/src/serialization.cpp b/tesseract_collision/core/src/serialization.cpp index 77f7243b42e..a4366cadbb4 100644 --- a/tesseract_collision/core/src/serialization.cpp +++ b/tesseract_collision/core/src/serialization.cpp @@ -39,6 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -50,26 +51,9 @@ namespace boost::serialization /************************************************/ /****** tesseract_collision::ContactResult ******/ /************************************************/ -template -void save(Archive& ar, const tesseract_collision::ContactResult& g, const unsigned int /*version*/) -{ - ar& boost::serialization::make_nvp("distance", g.distance); - ar& boost::serialization::make_nvp("type_id", g.type_id); - ar& boost::serialization::make_nvp("link_names", g.link_names); - ar& boost::serialization::make_nvp("shape_id", g.shape_id); - ar& boost::serialization::make_nvp("subshape_id", g.subshape_id); - ar& boost::serialization::make_nvp("nearest_points", g.nearest_points); - ar& boost::serialization::make_nvp("nearest_points_local", g.nearest_points_local); - ar& boost::serialization::make_nvp("transform", g.transform); - ar& boost::serialization::make_nvp("normal", g.normal); - ar& boost::serialization::make_nvp("cc_time", g.cc_time); - ar& boost::serialization::make_nvp("cc_type", g.cc_type); - ar& boost::serialization::make_nvp("cc_transform", g.cc_transform); - ar& boost::serialization::make_nvp("single_contact_point", g.single_contact_point); -} template -void load(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int /*version*/) +void serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int /*version*/) { ar& boost::serialization::make_nvp("distance", g.distance); ar& boost::serialization::make_nvp("type_id", g.type_id); @@ -86,12 +70,6 @@ void load(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int ar& boost::serialization::make_nvp("single_contact_point", g.single_contact_point); } -template -void serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version) -{ - split_free(ar, g, version); -} - /***************************************************/ /****** tesseract_collision::ContactResultMap ******/ /***************************************************/ @@ -116,11 +94,74 @@ void serialize(Archive& ar, tesseract_collision::ContactResultMap& g, const unsi { split_free(ar, g, version); } + +template +void serialize(Archive& ar, tesseract_collision::ContactRequest& g, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("type", g.type); + ar& boost::serialization::make_nvp("calculate_penetration", g.calculate_penetration); + ar& boost::serialization::make_nvp("calculate_distance", g.calculate_distance); + ar& boost::serialization::make_nvp("contact_limit", g.contact_limit); + // ar& boost::serialization::make_nvp("is_valid", g.is_valid); /** @todo FIX */ +} + +template +void serialize(Archive& ar, tesseract_collision::ContactManagerConfig& g, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("margin_data_override_type", g.margin_data_override_type); + ar& boost::serialization::make_nvp("margin_data", g.margin_data); + ar& boost::serialization::make_nvp("acm", g.acm); + ar& boost::serialization::make_nvp("acm_override_type", g.acm_override_type); + ar& boost::serialization::make_nvp("modify_object_enabled", g.modify_object_enabled); +} + +template +void serialize(Archive& ar, tesseract_collision::CollisionCheckConfig& g, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("contact_manager_config", g.contact_manager_config); + ar& boost::serialization::make_nvp("contact_request", g.contact_request); + ar& boost::serialization::make_nvp("type", g.type); + ar& boost::serialization::make_nvp("longest_valid_segment_length", g.longest_valid_segment_length); + ar& boost::serialization::make_nvp("check_program_mode", g.check_program_mode); +} + +template +void serialize(Archive& ar, tesseract_collision::ContactTrajectorySubstepResults& g, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("contacts", g.contacts); + ar& boost::serialization::make_nvp("substep", g.substep); + ar& boost::serialization::make_nvp("state0", g.state0); + ar& boost::serialization::make_nvp("state1", g.state1); +} + +template +void serialize(Archive& ar, tesseract_collision::ContactTrajectoryStepResults& g, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("substeps", g.substeps); + ar& boost::serialization::make_nvp("step", g.step); + ar& boost::serialization::make_nvp("state0", g.state0); + ar& boost::serialization::make_nvp("state1", g.state1); + ar& boost::serialization::make_nvp("total_substeps", g.total_substeps); +} + +template +void serialize(Archive& ar, tesseract_collision::ContactTrajectoryResults& g, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("steps", g.steps); + ar& boost::serialization::make_nvp("joint_names", g.joint_names); + ar& boost::serialization::make_nvp("total_steps", g.total_steps); +} } // namespace boost::serialization #include -TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactResult) +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactResult) TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactResultMap) +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactRequest) +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactManagerConfig) +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::CollisionCheckConfig) +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactTrajectorySubstepResults) +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactTrajectoryStepResults) +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactTrajectoryResults) TESSERACT_ANY_EXPORT_IMPLEMENT(TesseractCollisionContactResult) TESSERACT_ANY_EXPORT_IMPLEMENT(TesseractCollisionContactResultMap) diff --git a/tesseract_common/include/tesseract_common/eigen_serialization.h b/tesseract_common/include/tesseract_common/eigen_serialization.h index 2224a433f39..82cad40f591 100644 --- a/tesseract_common/include/tesseract_common/eigen_serialization.h +++ b/tesseract_common/include/tesseract_common/eigen_serialization.h @@ -129,14 +129,21 @@ void load(Archive& ar, std::variant& g, const un template void serialize(Archive& ar, std::variant& g, const unsigned int version); // NOLINT +/****************************************/ +/****** Eigen::Matrix *****/ +/****************************************/ + +template +void serialize(Archive& ar, Eigen::Matrix& g, const unsigned int version); // NOLINT + } // namespace boost::serialization // Set the tracking to track_never for all Eigen types. -BOOST_CLASS_TRACKING(Eigen::VectorXd, boost::serialization::track_never); -BOOST_CLASS_TRACKING(Eigen::Vector3d, boost::serialization::track_never); -BOOST_CLASS_TRACKING(Eigen::Vector4d, boost::serialization::track_never); -BOOST_CLASS_TRACKING(Eigen::VectorXi, boost::serialization::track_never); -BOOST_CLASS_TRACKING(Eigen::Isometry3d, boost::serialization::track_never); -BOOST_CLASS_TRACKING(Eigen::MatrixX2d, boost::serialization::track_never); +BOOST_CLASS_TRACKING(Eigen::VectorXd, boost::serialization::track_never) +BOOST_CLASS_TRACKING(Eigen::Vector3d, boost::serialization::track_never) +BOOST_CLASS_TRACKING(Eigen::Vector4d, boost::serialization::track_never) +BOOST_CLASS_TRACKING(Eigen::VectorXi, boost::serialization::track_never) +BOOST_CLASS_TRACKING(Eigen::Isometry3d, boost::serialization::track_never) +BOOST_CLASS_TRACKING(Eigen::MatrixX2d, boost::serialization::track_never) #endif // TESSERACT_COMMON_SERIALIZATION_H diff --git a/tesseract_common/include/tesseract_common/serialization.h b/tesseract_common/include/tesseract_common/serialization.h index f62ea45c9c7..242b61d9e96 100644 --- a/tesseract_common/include/tesseract_common/serialization.h +++ b/tesseract_common/include/tesseract_common/serialization.h @@ -52,6 +52,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP template void Type::serialize(boost::archive::binary_oarchive& ar, const unsigned int version); \ template void Type::serialize(boost::archive::binary_iarchive& ar, const unsigned int version); +#define TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(Type) \ + template void boost::serialization::serialize( \ + boost::archive::xml_oarchive& ar, Type& g, const unsigned int version); /* NOLINT */ \ + template void boost::serialization::serialize( \ + boost::archive::xml_iarchive& ar, Type& g, const unsigned int version); /* NOLINT */ \ + template void boost::serialization::serialize( \ + boost::archive::binary_oarchive& ar, Type& g, const unsigned int version); /* NOLINT */ \ + template void boost::serialization::serialize( \ + boost::archive::binary_iarchive& ar, Type& g, const unsigned int version); /* NOLINT */ + // Use this macro for serialization defined using the invasive method inside the class with custom load/save functions #define TESSERACT_SERIALIZE_SAVE_LOAD_ARCHIVES_INSTANTIATE(Type) \ template void Type::serialize(boost::archive::xml_oarchive& ar, const unsigned int version); \ diff --git a/tesseract_common/src/eigen_serialization.cpp b/tesseract_common/src/eigen_serialization.cpp index 8ba642a4f8e..4719f9e87f7 100644 --- a/tesseract_common/src/eigen_serialization.cpp +++ b/tesseract_common/src/eigen_serialization.cpp @@ -241,6 +241,16 @@ void serialize(Archive& ar, std::variant& g, con split_free(ar, g, version); } +/****************************************/ +/****** Eigen::Matrix *****/ +/****************************************/ + +template +void serialize(Archive& ar, Eigen::Matrix& g, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("data", boost::serialization::make_array(g.data(), 6)); +} + } // namespace boost::serialization #include @@ -251,3 +261,4 @@ TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(Eigen::VectorXi) TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(Eigen::Isometry3d) TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(Eigen::MatrixX2d) TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(std::variant) +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(Eigen::Matrix)