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)