Updated Cartesian waypoint to hold a link relative to which its trans…

…formation is relative
tesseract-robotics · Sep 17, 2019 · 0fa39a2 · 0fa39a2
1 parent be93a57
commit 0fa39a2
Showing 1 changed file with 12 additions and 5 deletions.
diff --git a/...ract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/waypoint.h b/...ract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/waypoint.h
@@ -170,14 +170,17 @@ class CartesianWaypoint : public Waypoint
   using Ptr = std::shared_ptr<CartesianWaypoint>;
   using ConstPtr = std::shared_ptr<const CartesianWaypoint>;
 
-  CartesianWaypoint(Eigen::Isometry3d cartesian_position)
-    : Waypoint(WaypointType::CARTESIAN_WAYPOINT), cartesian_position_(std::move(cartesian_position))
+  CartesianWaypoint(Eigen::Isometry3d cartesian_position, std::string parent_link = "")
+    : Waypoint(WaypointType::CARTESIAN_WAYPOINT)
+    , cartesian_position_(std::move(cartesian_position))
+    , parent_link_(std::move(parent_link))
   {
     setCoefficients(Eigen::VectorXd::Ones(6));
   }
 
-  CartesianWaypoint(Eigen::Vector3d position, Eigen::Quaterniond orientation)
+  CartesianWaypoint(Eigen::Vector3d position, Eigen::Quaterniond orientation, std::string link = "")
     : Waypoint(WaypointType::CARTESIAN_WAYPOINT)
+    , parent_link_(std::move(link))
   {
     cartesian_position_.translation() = position;
     cartesian_position_.linear() = orientation.toRotationMatrix();
@@ -191,19 +194,23 @@ class CartesianWaypoint : public Waypoint
   const Eigen::Isometry3d& getTransform() const { return cartesian_position_; }
 
   /** @brief Convenience function that returns the xyz cartesian position contained in cartesian_position_ */
-  Eigen::Vector3d getPosition() { return cartesian_position_.translation(); }
+  Eigen::Vector3d getPosition() const { return cartesian_position_.translation(); }
   /**
    * @brief Convenience function that returns the wxyz rotation quarternion contained in cartesian_position
    * @return Quaternion(w, x, y, z)
    */
-  Eigen::Vector4d getOrientation()
+  Eigen::Vector4d getOrientation() const
   {
     Eigen::Quaterniond q(cartesian_position_.rotation());
     return Eigen::Vector4d(q.w(), q.x(), q.y(), q.z());
   }
 
+  /** @brief Gets the name of the link to which this position is relative */
+  std::string getParentLinkName() const { return parent_link_; }
+
 protected:
   Eigen::Isometry3d cartesian_position_; /**< @brief Pose of this waypoint */
+  std::string parent_link_; /** @brief The link to which this position is defined */
 };
 
 /** @brief Defines a joint toleranced position waypoint for use with Tesseract Planners*/