Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add more serialization for eigen types and collision types #1076

Merged
merged 1 commit into from
Dec 6, 2024
Merged

Add more serialization for eigen types and collision types #1076

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 22 additions & 9 deletions tesseract_collision/core/include/tesseract_collision/core/serialization.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,27 +32,40 @@

namespace boost::serialization
{
/************************************************/
/****** tesseract_collision::ContactResult ******/
/************************************************/
template <class Archive>
void serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version); // NOLINT

template <class Archive>
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 <class Archive>
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 <class Archive>
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 <class Archive>
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 <class Archive>
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 <class Archive>
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 <class Archive>
void serialize(Archive& ar,
tesseract_collision::ContactTrajectorySubstepResults& g,
const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar,
tesseract_collision::ContactTrajectoryStepResults& g,
const unsigned int version); // NOLINT

template <class Archive>
void serialize(Archive& ar, tesseract_collision::ContactTrajectoryResults& g, const unsigned int version); // NOLINT

} // namespace boost::serialization

TESSERACT_ANY_EXPORT_KEY(tesseract_collision::ContactResult, TesseractCollisionContactResult)
Expand Down
91 changes: 66 additions & 25 deletions tesseract_collision/core/src/serialization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/serialization/array.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/unordered_map.hpp>
#include <tesseract_common/eigen_serialization.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

Expand All @@ -50,26 +51,9 @@ namespace boost::serialization
/************************************************/
/****** tesseract_collision::ContactResult ******/
/************************************************/
template <class Archive>
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 <class Archive>
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);
Expand All @@ -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 <class Archive>
void serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version)
{
split_free(ar, g, version);
}

/***************************************************/
/****** tesseract_collision::ContactResultMap ******/
/***************************************************/
Expand All @@ -116,11 +94,74 @@ void serialize(Archive& ar, tesseract_collision::ContactResultMap& g, const unsi
{
split_free(ar, g, version);
}

template <class Archive>
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 <class Archive>
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 <class Archive>
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 <class Archive>
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 <class Archive>
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 <class Archive>
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_common/serialization.h>
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)
Expand Down
19 changes: 13 additions & 6 deletions tesseract_common/include/tesseract_common/eigen_serialization.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,14 +129,21 @@ void load(Archive& ar, std::variant<std::string, Eigen::Isometry3d>& g, const un
template <class Archive>
void serialize(Archive& ar, std::variant<std::string, Eigen::Isometry3d>& g, const unsigned int version); // NOLINT

/****************************************/
/****** Eigen::Matrix<double, 6, 1> *****/
/****************************************/

template <class Archive>
void serialize(Archive& ar, Eigen::Matrix<double, 6, 1>& 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
10 changes: 10 additions & 0 deletions tesseract_common/include/tesseract_common/serialization.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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); \
Expand Down
11 changes: 11 additions & 0 deletions tesseract_common/src/eigen_serialization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -241,6 +241,16 @@ void serialize(Archive& ar, std::variant<std::string, Eigen::Isometry3d>& g, con
split_free(ar, g, version);
}

/****************************************/
/****** Eigen::Matrix<double, 6, 1> *****/
/****************************************/

template <class Archive>
void serialize(Archive& ar, Eigen::Matrix<double, 6, 1>& g, const unsigned int /*version*/)
{
ar& boost::serialization::make_nvp("data", boost::serialization::make_array(g.data(), 6));
}

} // namespace boost::serialization

#include <tesseract_common/serialization.h>
Expand All @@ -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<std::string COMMA Eigen::Isometry3d>)
TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(Eigen::Matrix<double COMMA 6 COMMA 1>)
Loading