From 40e14bcf3ec2e03204a4bae8c0984bf2bf0865df Mon Sep 17 00:00:00 2001
From: Levi Armstrong <levi.armstrong@gmail.com>
Date: Sun, 1 Dec 2024 09:41:59 -0600
Subject: [PATCH] Simplify the type erasure poly

---
 .../poly/cartesian_waypoint_poly.h                  | 13 +++++++------
 .../poly/instruction_poly.h                         | 13 +++++++------
 .../poly/joint_waypoint_poly.h                      | 13 +++++++------
 .../poly/move_instruction_poly.h                    | 13 +++++++------
 .../poly/state_waypoint_poly.h                      | 13 +++++++------
 .../tesseract_command_language/poly/waypoint_poly.h | 13 +++++++------
 6 files changed, 42 insertions(+), 36 deletions(-)

diff --git a/tesseract_command_language/include/tesseract_command_language/poly/cartesian_waypoint_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/cartesian_waypoint_poly.h
index 1536f83274..699ebb6e1f 100644
--- a/tesseract_command_language/include/tesseract_command_language/poly/cartesian_waypoint_poly.h
+++ b/tesseract_command_language/include/tesseract_command_language/poly/cartesian_waypoint_poly.h
@@ -48,20 +48,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
   using C##InstanceBase = tesseract_common::                                                                           \
       TypeErasureInstance<C, tesseract_planning::detail_cartesian_waypoint::CartesianWaypointInterface>;               \
   using C##Instance = tesseract_planning::detail_cartesian_waypoint::CartesianWaypointInstance<C>;                     \
-  using C##InstanceWrapper = tesseract_common::TypeErasureInstanceWrapper<C##Instance>;                                \
   }                                                                                                                    \
   BOOST_CLASS_EXPORT_KEY(N::C##InstanceBase)                                                                           \
   BOOST_CLASS_EXPORT_KEY(N::C##Instance)                                                                               \
-  BOOST_CLASS_EXPORT_KEY(N::C##InstanceWrapper)                                                                        \
   BOOST_CLASS_TRACKING(N::C##InstanceBase, boost::serialization::track_never)                                          \
-  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)                                              \
-  BOOST_CLASS_TRACKING(N::C##InstanceWrapper, boost::serialization::track_never)
+  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)
 
 /** @brief If shared library, this must go in the cpp after the implicit instantiation of the serialize function */
 #define TESSERACT_CARTESIAN_WAYPOINT_EXPORT_IMPLEMENT(inst)                                                            \
   BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceBase)                                                                     \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)                                                                         \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceWrapper)
+  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)
 
 /**
  * @brief This should not be used within shared libraries use the two above.
@@ -194,6 +190,11 @@ struct CartesianWaypointInstance : tesseract_common::TypeErasureInstance<T, Cart
   const std::string& getName() const final { return this->get().getName(); }
   void print(const std::string& prefix) const final { this->get().print(prefix); }
 
+  std::unique_ptr<tesseract_common::TypeErasureInterface> clone() const final
+  {
+    return std::make_unique<CartesianWaypointInstance<T>>(this->get());
+  }
+
 private:
   friend class boost::serialization::access;
   friend struct tesseract_common::Serialization;
diff --git a/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h
index 0cb3cf56d6..813ba86068 100644
--- a/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h
+++ b/tesseract_command_language/include/tesseract_command_language/poly/instruction_poly.h
@@ -51,20 +51,16 @@ struct uuid;
   using C##InstanceBase =                                                                                              \
       tesseract_common::TypeErasureInstance<C, tesseract_planning::detail_instruction::InstructionInterface>;          \
   using C##Instance = tesseract_planning::detail_instruction::InstructionInstance<C>;                                  \
-  using C##InstanceWrapper = tesseract_common::TypeErasureInstanceWrapper<C##Instance>;                                \
   }                                                                                                                    \
   BOOST_CLASS_EXPORT_KEY(N::C##InstanceBase)                                                                           \
   BOOST_CLASS_EXPORT_KEY(N::C##Instance)                                                                               \
-  BOOST_CLASS_EXPORT_KEY(N::C##InstanceWrapper)                                                                        \
   BOOST_CLASS_TRACKING(N::C##InstanceBase, boost::serialization::track_never)                                          \
-  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)                                              \
-  BOOST_CLASS_TRACKING(N::C##InstanceWrapper, boost::serialization::track_never)
+  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)
 
 /** @brief If shared library, this must go in the cpp after the implicit instantiation of the serialize function */
 #define TESSERACT_INSTRUCTION_EXPORT_IMPLEMENT(inst)                                                                   \
   BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceBase)                                                                     \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)                                                                         \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceWrapper)
+  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)
 
 /**
  * @brief This should not be used within shared libraries use the two above.
@@ -158,6 +154,11 @@ struct InstructionInstance : tesseract_common::TypeErasureInstance<T, Instructio
 
   void print(const std::string& prefix) const final { this->get().print(prefix); }
 
+  std::unique_ptr<tesseract_common::TypeErasureInterface> clone() const final
+  {
+    return std::make_unique<InstructionInstance<T>>(this->get());  // Creates a copy of this object
+  }
+
 private:
   friend class boost::serialization::access;
   friend struct tesseract_common::Serialization;
diff --git a/tesseract_command_language/include/tesseract_command_language/poly/joint_waypoint_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/joint_waypoint_poly.h
index c9aab1f93e..e77f4cc702 100644
--- a/tesseract_command_language/include/tesseract_command_language/poly/joint_waypoint_poly.h
+++ b/tesseract_command_language/include/tesseract_command_language/poly/joint_waypoint_poly.h
@@ -47,20 +47,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
   using C##InstanceBase =                                                                                              \
       tesseract_common::TypeErasureInstance<C, tesseract_planning::detail_joint_waypoint::JointWaypointInterface>;     \
   using C##Instance = tesseract_planning::detail_joint_waypoint::JointWaypointInstance<C>;                             \
-  using C##InstanceWrapper = tesseract_common::TypeErasureInstanceWrapper<C##Instance>;                                \
   }                                                                                                                    \
   BOOST_CLASS_EXPORT_KEY(N::C##InstanceBase)                                                                           \
   BOOST_CLASS_EXPORT_KEY(N::C##Instance)                                                                               \
-  BOOST_CLASS_EXPORT_KEY(N::C##InstanceWrapper)                                                                        \
   BOOST_CLASS_TRACKING(N::C##InstanceBase, boost::serialization::track_never)                                          \
-  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)                                              \
-  BOOST_CLASS_TRACKING(N::C##InstanceWrapper, boost::serialization::track_never)
+  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)
 
 /** @brief If shared library, this must go in the cpp after the implicit instantiation of the serialize function */
 #define TESSERACT_JOINT_WAYPOINT_EXPORT_IMPLEMENT(inst)                                                                \
   BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceBase)                                                                     \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)                                                                         \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceWrapper)
+  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)
 
 /**
  * @brief This should not be used within shared libraries use the two above.
@@ -204,6 +200,11 @@ struct JointWaypointInstance : tesseract_common::TypeErasureInstance<T, JointWay
   const std::string& getName() const final { return this->get().getName(); }
   void print(const std::string& prefix) const final { this->get().print(prefix); }
 
+  std::unique_ptr<tesseract_common::TypeErasureInterface> clone() const final
+  {
+    return std::make_unique<JointWaypointInstance<T>>(this->get());
+  }
+
 private:
   friend class boost::serialization::access;
   friend struct tesseract_common::Serialization;
diff --git a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h
index 8587e0550c..0058c670db 100644
--- a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h
+++ b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h
@@ -51,20 +51,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
   using C##InstanceBase =                                                                                              \
       tesseract_common::TypeErasureInstance<C, tesseract_planning::detail_move_instruction::MoveInstructionInterface>; \
   using C##Instance = tesseract_planning::detail_move_instruction::MoveInstructionInstance<C>;                         \
-  using C##InstanceWrapper = tesseract_common::TypeErasureInstanceWrapper<C##Instance>;                                \
   }                                                                                                                    \
   BOOST_CLASS_EXPORT_KEY(N::C##InstanceBase)                                                                           \
   BOOST_CLASS_EXPORT_KEY(N::C##Instance)                                                                               \
-  BOOST_CLASS_EXPORT_KEY(N::C##InstanceWrapper)                                                                        \
   BOOST_CLASS_TRACKING(N::C##InstanceBase, boost::serialization::track_never)                                          \
-  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)                                              \
-  BOOST_CLASS_TRACKING(N::C##InstanceWrapper, boost::serialization::track_never)
+  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)
 
 /** @brief If shared library, this must go in the cpp after the implicit instantiation of the serialize function */
 #define TESSERACT_MOVE_INSTRUCTION_EXPORT_IMPLEMENT(inst)                                                              \
   BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceBase)                                                                     \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)                                                                         \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceWrapper)
+  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)
 
 /**
  * @brief This should not be used within shared libraries use the two above.
@@ -283,6 +279,11 @@ struct MoveInstructionInstance : tesseract_common::TypeErasureInstance<T, MoveIn
   JointWaypointPoly createJointWaypoint() const final { return this->get().createJointWaypoint(); }
   StateWaypointPoly createStateWaypoint() const final { return this->get().createStateWaypoint(); }
 
+  std::unique_ptr<tesseract_common::TypeErasureInterface> clone() const final
+  {
+    return std::make_unique<MoveInstructionInstance<T>>(this->get());
+  }
+
 private:
   friend class boost::serialization::access;
   friend struct tesseract_common::Serialization;
diff --git a/tesseract_command_language/include/tesseract_command_language/poly/state_waypoint_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/state_waypoint_poly.h
index 4a07cc1e50..33b915d6a4 100644
--- a/tesseract_command_language/include/tesseract_command_language/poly/state_waypoint_poly.h
+++ b/tesseract_command_language/include/tesseract_command_language/poly/state_waypoint_poly.h
@@ -46,20 +46,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
   using C##InstanceBase =                                                                                              \
       tesseract_common::TypeErasureInstance<C, tesseract_planning::detail_state_waypoint::StateWaypointInterface>;     \
   using C##Instance = tesseract_planning::detail_state_waypoint::StateWaypointInstance<C>;                             \
-  using C##InstanceWrapper = tesseract_common::TypeErasureInstanceWrapper<C##Instance>;                                \
   }                                                                                                                    \
   BOOST_CLASS_EXPORT_KEY(N::C##InstanceBase)                                                                           \
   BOOST_CLASS_EXPORT_KEY(N::C##Instance)                                                                               \
-  BOOST_CLASS_EXPORT_KEY(N::C##InstanceWrapper)                                                                        \
   BOOST_CLASS_TRACKING(N::C##InstanceBase, boost::serialization::track_never)                                          \
-  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)                                              \
-  BOOST_CLASS_TRACKING(N::C##InstanceWrapper, boost::serialization::track_never)
+  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)
 
 /** @brief If shared library, this must go in the cpp after the implicit instantiation of the serialize function */
 #define TESSERACT_STATE_WAYPOINT_EXPORT_IMPLEMENT(inst)                                                                \
   BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceBase)                                                                     \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)                                                                         \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceWrapper)
+  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)
 
 /**
  * @brief This should not be used within shared libraries use the two above.
@@ -222,6 +218,11 @@ struct StateWaypointInstance : tesseract_common::TypeErasureInstance<T, StateWay
   const std::string& getName() const final { return this->get().getName(); }
   void print(const std::string& prefix) const final { this->get().print(prefix); }
 
+  std::unique_ptr<tesseract_common::TypeErasureInterface> clone() const final
+  {
+    return std::make_unique<StateWaypointInstance<T>>(this->get());
+  }
+
 private:
   friend class boost::serialization::access;
   friend struct tesseract_common::Serialization;
diff --git a/tesseract_command_language/include/tesseract_command_language/poly/waypoint_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/waypoint_poly.h
index 6a4a26442b..0d23237191 100644
--- a/tesseract_command_language/include/tesseract_command_language/poly/waypoint_poly.h
+++ b/tesseract_command_language/include/tesseract_command_language/poly/waypoint_poly.h
@@ -46,20 +46,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
   using C##InstanceBase =                                                                                              \
       tesseract_common::TypeErasureInstance<C, tesseract_planning::detail_waypoint::WaypointInterface>;                \
   using C##Instance = tesseract_planning::detail_waypoint::WaypointInstance<C>;                                        \
-  using C##InstanceWrapper = tesseract_common::TypeErasureInstanceWrapper<C##Instance>;                                \
   }                                                                                                                    \
   BOOST_CLASS_EXPORT_KEY(N::C##InstanceBase)                                                                           \
   BOOST_CLASS_EXPORT_KEY(N::C##Instance)                                                                               \
-  BOOST_CLASS_EXPORT_KEY(N::C##InstanceWrapper)                                                                        \
   BOOST_CLASS_TRACKING(N::C##InstanceBase, boost::serialization::track_never)                                          \
-  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)                                              \
-  BOOST_CLASS_TRACKING(N::C##InstanceWrapper, boost::serialization::track_never)
+  BOOST_CLASS_TRACKING(N::C##Instance, boost::serialization::track_never)
 
 /** @brief If shared library, this must go in the cpp after the implicit instantiation of the serialize function */
 #define TESSERACT_WAYPOINT_EXPORT_IMPLEMENT(inst)                                                                      \
   BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceBase)                                                                     \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)                                                                         \
-  BOOST_CLASS_EXPORT_IMPLEMENT(inst##InstanceWrapper)
+  BOOST_CLASS_EXPORT_IMPLEMENT(inst##Instance)
 
 /**
  * @brief This should not be used within shared libraries use the two above.
@@ -124,6 +120,11 @@ struct WaypointInstance : tesseract_common::TypeErasureInstance<T, WaypointInter
   const std::string& getName() const final { return this->get().getName(); }
   void print(const std::string& prefix) const final { this->get().print(prefix); }
 
+  std::unique_ptr<tesseract_common::TypeErasureInterface> clone() const final
+  {
+    return std::make_unique<WaypointInstance<T>>(this->get());
+  }
+
 private:
   friend class boost::serialization::access;
   friend struct tesseract_common::Serialization;