From dd348a9c6f5e99fafd0556a47706b39f9d15b156 Mon Sep 17 00:00:00 2001 From: Levi Armstrong <levi.armstrong@gmail.com> Date: Mon, 20 Mar 2023 18:35:39 -0500 Subject: [PATCH] Add ContactResult boost serialization --- tesseract_collision/CMakeLists.txt | 2 +- .../cmake/tesseract_collision-config.cmake.in | 4 +- tesseract_collision/core/CMakeLists.txt | 4 +- .../tesseract_collision/core/serialization.h | 61 ++++++++++++ .../core/src/serialization.cpp | 92 +++++++++++++++++++ 5 files changed, 159 insertions(+), 4 deletions(-) create mode 100644 tesseract_collision/core/include/tesseract_collision/core/serialization.h create mode 100644 tesseract_collision/core/src/serialization.cpp diff --git a/tesseract_collision/CMakeLists.txt b/tesseract_collision/CMakeLists.txt index 3286c4e9d85..8d5bb0f468a 100644 --- a/tesseract_collision/CMakeLists.txt +++ b/tesseract_collision/CMakeLists.txt @@ -10,7 +10,7 @@ if(WIN32) endif() find_package(Eigen3 REQUIRED) -find_package(Boost REQUIRED COMPONENTS system program_options) +find_package(Boost REQUIRED COMPONENTS system program_options serialization) find_package(octomap REQUIRED) find_package(console_bridge REQUIRED) find_package(tesseract_geometry REQUIRED) diff --git a/tesseract_collision/cmake/tesseract_collision-config.cmake.in b/tesseract_collision/cmake/tesseract_collision-config.cmake.in index c794b0d3f8a..215f061e7d7 100644 --- a/tesseract_collision/cmake/tesseract_collision-config.cmake.in +++ b/tesseract_collision/cmake/tesseract_collision-config.cmake.in @@ -6,9 +6,9 @@ set_and_check(@PROJECT_NAME@_LIBRARY_DIRS "${PACKAGE_PREFIX_DIR}/lib") include(CMakeFindDependencyMacro) if(${CMAKE_VERSION} VERSION_LESS "3.15.0") - find_package(Boost REQUIRED COMPONENTS system) + find_package(Boost REQUIRED COMPONENTS system serialization) else() - find_dependency(Boost COMPONENTS system) + find_dependency(Boost COMPONENTS system serialization) endif() find_dependency(Eigen3) find_dependency(octomap) diff --git a/tesseract_collision/core/CMakeLists.txt b/tesseract_collision/core/CMakeLists.txt index 2c37b121d12..dd8c251c9b2 100644 --- a/tesseract_collision/core/CMakeLists.txt +++ b/tesseract_collision/core/CMakeLists.txt @@ -2,10 +2,11 @@ add_library( ${PROJECT_NAME}_core src/common.cpp - src/types.cpp src/contact_managers_plugin_factory.cpp src/continuous_contact_manager.cpp src/discrete_contact_manager.cpp + src/serialization.cpp + src/types.cpp src/utils.cpp) target_link_libraries( ${PROJECT_NAME}_core @@ -14,6 +15,7 @@ target_link_libraries( tesseract::tesseract_geometry Boost::boost Boost::system + Boost::serialization yaml-cpp) target_compile_options(${PROJECT_NAME}_core PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) target_compile_options(${PROJECT_NAME}_core PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) diff --git a/tesseract_collision/core/include/tesseract_collision/core/serialization.h b/tesseract_collision/core/include/tesseract_collision/core/serialization.h new file mode 100644 index 00000000000..7284575f609 --- /dev/null +++ b/tesseract_collision/core/include/tesseract_collision/core/serialization.h @@ -0,0 +1,61 @@ +/** + * @file serialization.h + * @brief Tesseracts Collision Serialization + * + * @author Levi Armstrong + * @date March 20, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COLLISION_SERIALIZATION_H +#define TESSERACT_COLLISION_SERIALIZATION_H + +#include <tesseract_common/macros.h> +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include <variant> +#include <fstream> +#include <sstream> +#include <boost/archive/xml_oarchive.hpp> +#include <boost/archive/xml_iarchive.hpp> +#include <boost/archive/binary_oarchive.hpp> +#include <boost/archive/binary_iarchive.hpp> +#include <boost/serialization/tracking.hpp> +#include <boost/serialization/tracking_enum.hpp> +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include <tesseract_collision/core/types.h> + +namespace boost::serialization +{ +/************************************************/ +/****** tesseract_collision::ContactResult ******/ +/************************************************/ + +template <class Archive> +void save(Archive& ar, const tesseract_collision::ContactResult& g, const unsigned int version); // NOLINT + +template <class Archive> +void load(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version); // NOLINT + +template <class Archive> +void serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version); // NOLINT + +} // namespace boost::serialization + +#endif // TESSERACT_COLLISION_SERIALIZATION_H diff --git a/tesseract_collision/core/src/serialization.cpp b/tesseract_collision/core/src/serialization.cpp new file mode 100644 index 00000000000..8b657696f99 --- /dev/null +++ b/tesseract_collision/core/src/serialization.cpp @@ -0,0 +1,92 @@ +/** + * @file serialization.cpp + * @brief Contact results serialization wrappers + * @details Supports the following + * - tesseract_collision::ContactResult + * - tesseract_collision::ContactResultVector + * - tesseract_collision::ContactResultMap + * + * @author Levi Armstrong + * @date March 20, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include <tesseract_common/macros.h> +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include <boost/serialization/base_object.hpp> +#include <boost/serialization/nvp.hpp> +#include <boost/serialization/array.hpp> +#include <boost/serialization/vector.hpp> +#include <tesseract_common/eigen_serialization.h> +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include <tesseract_collision/core/serialization.h> + +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*/) +{ + 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 serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version) +{ + split_free(ar, g, version); +} + +} // namespace boost::serialization + +#include <tesseract_common/serialization.h> +TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactResult)