Add boost serialization support to config objects

.clang-format

@@ -52,17 +52,17 @@ UseTab: Never
 BreakBeforeBraces: Custom
 
 # Control of individual brace wrapping cases
-BraceWrapping: {
-    AfterCaseLabel: 'true',
-    AfterClass: 'true',
-    AfterControlStatement: 'true',
-    AfterEnum : 'true',
-    AfterFunction : 'true',
-    AfterNamespace : 'true',
-    AfterStruct : 'true',
-    AfterUnion : 'true',
-    BeforeCatch : 'true',
-    BeforeElse : 'true',
-    IndentBraces : 'false',
-}
+BraceWrapping:
+    AfterCaseLabel : 'true'
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+
 ...

.github/workflows/clang_format.yml

@@ -11,16 +11,17 @@ on:
 
 jobs:
   clang_format:
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v1
 
       - name: Run clang format
         run: |
           sudo apt update
-          sudo apt install -y git clang-format-8
+          sudo apt install -y git clang-format-12
           if [ $? -ge 1 ]; then return 1; fi
           ./.run-clang-format
           if [ $? -ge 1 ]; then return 1; fi
           output=$(git diff)
+          echo $output
           if [ -n "$output" ]; then exit 1; else exit 0; fi

.run-clang-format

@@ -1,2 +1,2 @@
 #!/bin/bash
-find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-8 -style=file -i {} \;
+find . -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-12 -style=file -i {} \;

trajopt/src/collision_terms.cpp

@@ -25,10 +25,8 @@ TRAJOPT_IGNORE_WARNINGS_POP
 
 namespace trajopt
 {
-
 namespace
 {
-
 void CollisionsToDistances(const ContactResultVectorWrapper& dist_results, DblVec& dists)
 {
   dists.clear();

trajopt_common/CMakeLists.txt

@@ -12,7 +12,7 @@ if(WIN32)
 endif()
 
 find_package(Eigen3 REQUIRED)
-find_package(Boost COMPONENTS program_options REQUIRED)
+find_package(Boost COMPONENTS program_options serialization REQUIRED)
 find_package(ros_industrial_cmake_boilerplate REQUIRED)
 find_package(tesseract_common REQUIRED)
 find_package(tesseract_collision COMPONENTS core REQUIRED)
@@ -34,6 +34,7 @@ target_link_libraries(
   ${PROJECT_NAME}
   PUBLIC Eigen3::Eigen
          Boost::program_options
+         Boost::serialization
          tesseract::tesseract_common
          tesseract::tesseract_collision_core
          tesseract::tesseract_kinematics_core)
@@ -82,7 +83,9 @@ if(TRAJOPT_PACKAGE)
     LINUX_DEPENDS
     "libeigen3-dev"
     "libboost-program-options-dev"
+    "libboost-serialization-dev"
     WINDOWS_DEPENDS
     "Eigen3"
-    "boost-program-options")
+    "boost-program-options"
+    "boost_serialization")
 endif()

trajopt_common/cmake/trajopt_common-config.cmake.in

@@ -8,11 +8,11 @@ set(@PROJECT_NAME@_LIBRARIES trajopt::@PROJECT_NAME@)
 include(CMakeFindDependencyMacro)
 find_dependency(Eigen3)
 if(${CMAKE_VERSION} VERSION_LESS "3.15.0")
-    find_package(Boost COMPONENTS program_options REQUIRED)
+    find_package(Boost COMPONENTS program_options serialization REQUIRED)
     find_package(tesseract_collision COMPONENTS core REQUIRED)
     find_package(tesseract_kinematics COMPONENTS core REQUIRED)
 else()
-    find_dependency(Boost COMPONENTS program_options)
+    find_dependency(Boost COMPONENTS program_options serialization)
     find_dependency(tesseract_collision COMPONENTS core)
     find_dependency(tesseract_kinematics COMPONENTS core)
 endif()

trajopt_common/include/trajopt_common/collision_types.h

@@ -87,6 +87,10 @@ struct CollisionCoeffData
 
   /// Pairs containing zero coeff
   std::set<tesseract_common::LinkNamesPair> zero_coeff_;
+
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
 };
 
 /**
@@ -113,6 +117,11 @@ struct TrajOptCollisionConfig : public tesseract_collision::CollisionCheckConfig
    * It still finds all contacts but sorts based on the worst uses those up to the max_num_cnt.
    */
   int max_num_cnt{ 3 };
+
+protected:
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
 };
 
 /** @brief A data structure to contain a links gradient results */
@@ -264,4 +273,8 @@ struct CollisionCacheData
 };
 
 }  // namespace trajopt_common
+
+BOOST_CLASS_EXPORT_KEY(trajopt_common::CollisionCoeffData)
+BOOST_CLASS_EXPORT_KEY(trajopt_common::TrajOptCollisionConfig)
+
 #endif  // TRAJOPT_COMMON_COLLISION_TYPES_H

trajopt_common/include/trajopt_common/utils.hpp

@@ -10,6 +10,8 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <array>
 #include <set>
 #include <tesseract_common/types.h>
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/export.hpp>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 namespace trajopt_common
@@ -20,6 +22,7 @@ struct SafetyMarginData
   using Ptr = std::shared_ptr<SafetyMarginData>;
   using ConstPtr = std::shared_ptr<const SafetyMarginData>;
 
+  SafetyMarginData();
   SafetyMarginData(double default_safety_margin, double default_safety_margin_coeff);
 
   /**
@@ -81,6 +84,10 @@ struct SafetyMarginData
 
   /// Pairs containing zero coeff
   std::set<tesseract_common::LinkNamesPair> zero_coeff_;
+
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive&, const unsigned int);  // NOLINT
 };
 
 /**
@@ -116,4 +123,6 @@ Eigen::Isometry3d addTwist(const Eigen::Isometry3d& t1,
                            double dt);
 }  // namespace trajopt_common
 
+BOOST_CLASS_EXPORT_KEY(trajopt_common::SafetyMarginData)
+
 #endif  // TRAJOPT_COMMON_UTILS_HPP

trajopt_common/src/collision_types.cpp

@@ -23,6 +23,12 @@
  */
 
 #include <trajopt_common/collision_types.h>
+#include <tesseract_collision/core/serialization.h>
+#if (BOOST_VERSION >= 107400) && (BOOST_VERSION < 107500)
+#include <boost/serialization/library_version_type.hpp>
+#endif
+#include <boost/serialization/unordered_map.hpp>
+#include <boost/serialization/set.hpp>
 
 namespace trajopt_common
 {
@@ -58,11 +64,28 @@ const std::set<tesseract_common::LinkNamesPair>& CollisionCoeffData::getPairsWit
   return zero_coeff_;
 }
 
+template <class Archive>
+void CollisionCoeffData::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_NVP(default_collision_coeff_);
+  ar& BOOST_SERIALIZATION_NVP(lookup_table_);
+  ar& BOOST_SERIALIZATION_NVP(zero_coeff_);
+}
+
 TrajOptCollisionConfig::TrajOptCollisionConfig(double margin, double coeff)
   : CollisionCheckConfig(margin), collision_coeff_data(coeff)
 {
 }
 
+template <class Archive>
+void TrajOptCollisionConfig::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(CollisionCheckConfig);
+  ar& BOOST_SERIALIZATION_NVP(collision_coeff_data);
+  ar& BOOST_SERIALIZATION_NVP(collision_margin_buffer);
+  ar& BOOST_SERIALIZATION_NVP(max_num_cnt);
+}
+
 double LinkMaxError::getMaxError() const
 {
   if (has_error[0] && has_error[1])
@@ -261,3 +284,9 @@ double GradientResultsSet::getMaxErrorWithBufferT1() const
 }
 
 }  // namespace trajopt_common
+
+#include <tesseract_common/serialization.h>
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(trajopt_common::CollisionCoeffData)
+BOOST_CLASS_EXPORT_IMPLEMENT(trajopt_common::CollisionCoeffData)
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(trajopt_common::TrajOptCollisionConfig)
+BOOST_CLASS_EXPORT_IMPLEMENT(trajopt_common::TrajOptCollisionConfig)

trajopt_common/src/utils.cpp

@@ -1,8 +1,16 @@
 #include <trajopt_common/utils.hpp>
 #include <tesseract_common/utils.h>
+#if (BOOST_VERSION >= 107400) && (BOOST_VERSION < 107500)
+#include <boost/serialization/library_version_type.hpp>
+#endif
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/array.hpp>
+#include <boost/serialization/set.hpp>
+#include <boost/serialization/unordered_map.hpp>
 
 namespace trajopt_common
 {
+SafetyMarginData::SafetyMarginData() : SafetyMarginData(0, 10) {}
 SafetyMarginData::SafetyMarginData(double default_safety_margin, double default_safety_margin_coeff)
   : default_safety_margin_data_({ default_safety_margin, default_safety_margin_coeff })
   , max_safety_margin_(default_safety_margin)
@@ -48,6 +56,15 @@ const std::set<std::pair<std::string, std::string>>& SafetyMarginData::getPairsW
   return zero_coeff_;
 }
 
+template <class Archive>
+void SafetyMarginData::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_NVP(default_safety_margin_data_);
+  ar& BOOST_SERIALIZATION_NVP(max_safety_margin_);
+  ar& BOOST_SERIALIZATION_NVP(pair_lookup_table_);
+  ar& BOOST_SERIALIZATION_NVP(zero_coeff_);
+}
+
 std::vector<SafetyMarginData::Ptr> createSafetyMarginDataVector(int num_elements,
                                                                 double default_safety_margin,
                                                                 double default_safety_margin_coeff)
@@ -72,3 +89,7 @@ Eigen::Isometry3d addTwist(const Eigen::Isometry3d& t1,
   return t2;
 }
 }  // namespace trajopt_common
+
+#include <tesseract_common/serialization.h>
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(trajopt_common::SafetyMarginData)
+BOOST_CLASS_EXPORT_IMPLEMENT(trajopt_common::SafetyMarginData)

trajopt_ext/vhacd/src/btConvexHullComputer.cpp

@@ -115,8 +115,8 @@ class btConvexHullInternal
       Int128 result;
       __asm__("addq %[bl], %[rl]\n\t"
               "adcq %[bh], %[rh]\n\t"
-              : [ rl ] "=r"(result.low), [ rh ] "=r"(result.high)
-              : "0"(low), "1"(high), [ bl ] "g"(b.low), [ bh ] "g"(b.high)
+              : [rl] "=r"(result.low), [rh] "=r"(result.high)
+              : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
               : "cc");
       return result;
 #else
@@ -131,8 +131,8 @@ class btConvexHullInternal
       Int128 result;
       __asm__("subq %[bl], %[rl]\n\t"
               "sbbq %[bh], %[rh]\n\t"
-              : [ rl ] "=r"(result.low), [ rh ] "=r"(result.high)
-              : "0"(low), "1"(high), [ bl ] "g"(b.low), [ bh ] "g"(b.high)
+              : [rl] "=r"(result.low), [rh] "=r"(result.high)
+              : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
               : "cc");
       return result;
 #else
@@ -145,8 +145,8 @@ class btConvexHullInternal
 #ifdef USE_X86_64_ASM
       __asm__("addq %[bl], %[rl]\n\t"
               "adcq %[bh], %[rh]\n\t"
-              : [ rl ] "=r"(low), [ rh ] "=r"(high)
-              : "0"(low), "1"(high), [ bl ] "g"(b.low), [ bh ] "g"(b.high)
+              : [rl] "=r"(low), [rh] "=r"(high)
+              : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
               : "cc");
 #else
       uint64_t lo = low + b.low;
@@ -705,7 +705,7 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_
   Int128 result;
 
 #ifdef USE_X86_64_ASM
-  __asm__("imulq %[b]" : "=a"(result.low), "=d"(result.high) : "0"(a), [ b ] "r"(b) : "cc");
+  __asm__("imulq %[b]" : "=a"(result.low), "=d"(result.high) : "0"(a), [b] "r"(b) : "cc");
   return result;
 
 #else
@@ -729,7 +729,7 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint6
   Int128 result;
 
 #ifdef USE_X86_64_ASM
-  __asm__("mulq %[b]" : "=a"(result.low), "=d"(result.high) : "0"(a), [ b ] "r"(b) : "cc");
+  __asm__("mulq %[b]" : "=a"(result.low), "=d"(result.high) : "0"(a), [b] "r"(b) : "cc");
 
 #else
   DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high);
@@ -771,8 +771,8 @@ int32_t btConvexHullInternal::Rational64::compare(const Rational64& b) const
           "decb %%bh\n\t"    // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive
                              // (i.e., same sign as difference)
           "shll $16, %%ebx\n\t"  // ebx has same sign as difference
-          : "=&b"(result), [ tmp ] "=&r"(tmp), "=a"(dummy)
-          : "a"(denominator), [ bn ] "g"(b.numerator), [ tn ] "g"(numerator), [ bd ] "g"(b.denominator)
+          : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
+          : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
           : "%rdx", "cc");
   return result ? result ^ sign  // if sign is +1, only bit 0 of result is inverted, which does not change the sign of
                                  // result (and cannot result in zero)

trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp

@@ -188,7 +188,6 @@ Eigen::VectorXd OSQPEigenSolver::getSolution()
 TRAJOPT_IGNORE_WARNINGS_PUSH
 namespace
 {
-
 void* csc_malloc(c_int n, c_int size)
 {
   return c_malloc(n * size);  // NOLINT

trajopt_sco/src/solver_interface.cpp

@@ -217,7 +217,8 @@ std::ostream& operator<<(std::ostream& os, const ModelType& cs)
   if (cs_ivalue_ > ModelType::MODEL_NAMES_.size())
   {
     std::stringstream conversion_error;
-    conversion_error << "Error converting ModelType to string - " << "enum value is " << cs_ivalue_ << '\n';
+    conversion_error << "Error converting ModelType to string - "
+                     << "enum value is " << cs_ivalue_ << '\n';
     throw std::runtime_error(conversion_error.str());
   }
   os << ModelType::MODEL_NAMES_[cs_ivalue_];