Add constructors to WaypointPoly to support State, Joint and Cartesian Interface class

Author: Levi-Armstrong

tesseract_command_language/include/tesseract_command_language/fwd.h

@@ -6,6 +6,10 @@
 namespace tesseract_planning
 {
 // Waypoint Poly
+class WaypointInterface;
+class CartesianWaypointInterface;
+class JointWaypointInterface;
+class StateWaypointInterface;
 class WaypointPoly;
 class CartesianWaypointPoly;
 class JointWaypointPoly;
@@ -17,8 +21,10 @@ class JointWaypoint;
 class StateWaypoint;
 
 // Instruction Poly
+class InstructionInterface;
 class InstructionPoly;
 enum class MoveInstructionType : std::uint8_t;
+class MoveInstructionInterface;
 class MoveInstructionPoly;
 
 // Instructions

tesseract_command_language/include/tesseract_command_language/move_instruction.h

@@ -40,7 +40,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-class ProfileDictionary;
 /**
  * @brief The move instruction is used when defining the results of a motion planning request
  * @details

tesseract_command_language/include/tesseract_command_language/poly/waypoint_poly.h

@@ -41,6 +41,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
+class CartesianWaypointInterface;
+class JointWaypointInterface;
+class StateWaypointInterface;
+
 /**
  * @brief The WaypointInterface class
  */
@@ -104,6 +108,9 @@ class WaypointPoly
   WaypointPoly& operator=(WaypointPoly&& other) noexcept = default;
 
   WaypointPoly(const WaypointInterface& impl);
+  WaypointPoly(const CartesianWaypointInterface& impl);
+  WaypointPoly(const JointWaypointInterface& impl);
+  WaypointPoly(const StateWaypointInterface& impl);
 
   /**
    * @brief Set the name of the waypoint

tesseract_command_language/src/poly/waypoint_poly.cpp

@@ -8,6 +8,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/poly/cartesian_waypoint_poly.h>
 #include <tesseract_command_language/poly/joint_waypoint_poly.h>
 #include <tesseract_command_language/poly/state_waypoint_poly.h>
+#include <tesseract_command_language/cartesian_waypoint.h>
+#include <tesseract_command_language/joint_waypoint.h>
+#include <tesseract_command_language/state_waypoint.h>
 #include <tesseract_common/serialization.h>
 
 namespace tesseract_planning
@@ -39,6 +42,12 @@ WaypointPoly& WaypointPoly::operator=(const WaypointPoly& other)
 }
 
 WaypointPoly::WaypointPoly(const WaypointInterface& impl) : impl_(impl.clone()) {}
+WaypointPoly::WaypointPoly(const CartesianWaypointInterface& impl)
+  : impl_(std::make_unique<CartesianWaypointPoly>(impl))
+{
+}
+WaypointPoly::WaypointPoly(const JointWaypointInterface& impl) : impl_(std::make_unique<JointWaypointPoly>(impl)) {}
+WaypointPoly::WaypointPoly(const StateWaypointInterface& impl) : impl_(std::make_unique<StateWaypointPoly>(impl)) {}
 
 void WaypointPoly::setName(const std::string& name) { impl_->setName(name); }
 const std::string& WaypointPoly::getName() const { return impl_->getName(); }

tesseract_command_language/test/command_language_test_program.hpp

@@ -21,8 +21,8 @@ inline CompositeInstruction getTestProgram(std::string profile,
 
   // Start Joint Position for the program
   std::vector<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
-  StateWaypointPoly wp0{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) };
-  JointWaypointPoly wp00{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(6)) };
+  StateWaypoint wp0{ joint_names, Eigen::VectorXd::Zero(6) };
+  JointWaypoint wp00{ joint_names, Eigen::VectorXd::Zero(6) };
   MoveInstruction start_instruction(wp0, MoveInstructionType::FREESPACE, "freespace_profile");
   MoveInstruction end_instruction(wp00, MoveInstructionType::FREESPACE, "freespace_profile");
   start_instruction.setDescription("Start Instruction");
@@ -33,22 +33,22 @@ inline CompositeInstruction getTestProgram(std::string profile,
   seed_state.position = Eigen::VectorXd::Zero(6);
 
   // Define raster poses
-  CartesianWaypointPoly wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) *
-                                                Eigen::Quaterniond(0, 0, -1.0, 0));
+  CartesianWaypoint wp1(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) *
+                        Eigen::Quaterniond(0, 0, -1.0, 0));
   wp1.setSeed(seed_state);
-  CartesianWaypointPoly wp2 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.2, 0.8) *
-                                                Eigen::Quaterniond(0, 0, -1.0, 0));
-  CartesianWaypointPoly wp3 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.1, 0.8) *
-                                                Eigen::Quaterniond(0, 0, -1.0, 0));
+  CartesianWaypoint wp2(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.2, 0.8) *
+                        Eigen::Quaterniond(0, 0, -1.0, 0));
+  CartesianWaypoint wp3(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.1, 0.8) *
+                        Eigen::Quaterniond(0, 0, -1.0, 0));
   wp3.setSeed(seed_state);
-  CartesianWaypointPoly wp4 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, 0.0, 0.8) *
-                                                Eigen::Quaterniond(0, 0, -1.0, 0));
-  CartesianWaypointPoly wp5 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, 0.1, 0.8) *
-                                                Eigen::Quaterniond(0, 0, -1.0, 0));
+  CartesianWaypoint wp4(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, 0.0, 0.8) *
+                        Eigen::Quaterniond(0, 0, -1.0, 0));
+  CartesianWaypoint wp5(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, 0.1, 0.8) *
+                        Eigen::Quaterniond(0, 0, -1.0, 0));
   wp5.setSeed(seed_state);
-  CartesianWaypointPoly wp6 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, 0.2, 0.8) *
-                                                Eigen::Quaterniond(0, 0, -1.0, 0));
-  JointWaypointPoly wp7 = JointWaypoint(joint_names, Eigen::VectorXd::Ones(6));
+  CartesianWaypoint wp6(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, 0.2, 0.8) *
+                        Eigen::Quaterniond(0, 0, -1.0, 0));
+  JointWaypoint wp7(joint_names, Eigen::VectorXd::Ones(6));
 
   // Define raster move instruction
   MoveInstruction plan_c0(wp2, MoveInstructionType::LINEAR, "RASTER");