Skip to content

Commit

Permalink
Add more serialization for eigen types and collision types
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Dec 6, 2024
1 parent d3867a7 commit 82820da
Show file tree
Hide file tree
Showing 5 changed files with 122 additions and 40 deletions.
31 changes: 22 additions & 9 deletions tesseract_collision/core/include/tesseract_collision/core/serialization.h
View file
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
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
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
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
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>)

0 comments on commit 82820da

Please sign in to comment.