Add print to waypoint

tesseract-robotics · Oct 6, 2020 · feb3687 · feb3687
1 parent 93004b1
commit feb3687
Show file tree

Hide file tree

Showing 9 changed files with 40 additions and 2 deletions.
diff --git a/...anning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h b/...anning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h
@@ -131,6 +131,13 @@ class CartesianWaypoint
 
   int getType() const { return static_cast<int>(WaypointType::CARTESIAN_WAYPOINT); }
 
+  void print(const std::string& prefix = "") const
+  {
+    std::cout << prefix << "Cart WP: xyz=" << this->translation().x() << ", " << this->translation().y() << ", "
+              << this->translation().z() << std::endl;
+    // TODO: Add rotation
+  };
+
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const
   {
     Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " ");

diff --git a/...ct_planning/tesseract_command_language/include/tesseract_command_language/core/waypoint.h b/...ct_planning/tesseract_command_language/include/tesseract_command_language/core/waypoint.h
@@ -47,6 +47,8 @@ struct WaypointInnerBase
 
   virtual int getType() const = 0;
 
+  virtual void print(const std::string& prefix) const = 0;
+
   virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
 
   // This is not required for user defined implementation
@@ -74,6 +76,8 @@ struct WaypointInner final : WaypointInnerBase
 
   int getType() const final { return waypoint_.getType(); }
 
+  void print(const std::string& prefix) const final { waypoint_.print(prefix); }
+
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const final { return waypoint_.toXML(doc); }
 
   void* recover() final { return &waypoint_; }
@@ -134,6 +138,8 @@ class Waypoint
 
   int getType() const { return waypoint_->getType(); }
 
+  void print(const std::string& prefix = "") const { waypoint_->print(prefix); }
+
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const { return waypoint_->toXML(doc); }
 
   template <typename T>

diff --git a/...t_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h b/...t_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h
@@ -89,6 +89,11 @@ class JointWaypoint
 
   int getType() const { return static_cast<int>(WaypointType::JOINT_WAYPOINT); }
 
+  void print(const std::string& prefix = "") const
+  {
+    std::cout << prefix << "Joint WP: " << this->transpose() << std::endl;
+  };
+
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const
   {
     Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " ");

diff --git a/...ct_planning/tesseract_command_language/include/tesseract_command_language/null_waypoint.h b/...ct_planning/tesseract_command_language/include/tesseract_command_language/null_waypoint.h
@@ -42,6 +42,8 @@ class NullWaypoint
 
   int getType() const;
 
+  void print(const std::string& prefix = "") const;
+
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
 };
 }  // namespace tesseract_planning

diff --git a/...t_planning/tesseract_command_language/include/tesseract_command_language/state_waypoint.h b/...t_planning/tesseract_command_language/include/tesseract_command_language/state_waypoint.h
@@ -49,6 +49,8 @@ class StateWaypoint
 
   int getType() const;
 
+  void print(const std::string& prefix = "") const;
+
   tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
 
   /** @brief The joint corresponding to the position vector. */

diff --git a/tesseract/tesseract_planning/tesseract_command_language/src/move_instruction.cpp b/tesseract/tesseract_planning/tesseract_command_language/src/move_instruction.cpp
@@ -70,7 +70,9 @@ void MoveInstruction::setDescription(const std::string& description) { descripti
 void MoveInstruction::print(std::string prefix) const
 {
   std::cout << prefix + "Move Instruction, Type: " << getType() << ", Plan Type: " << static_cast<int>(move_type_)
-            << ",  Waypoint Type:" << getWaypoint().getType() << ",  Description: " << getDescription() << std::endl;
+            << ", ";
+  getWaypoint().print();
+  std::cout << ", Description: " << getDescription() << std::endl;
 }
 
 void MoveInstruction::setMoveType(MoveInstructionType move_type) { move_type_ = move_type; }

diff --git a/tesseract/tesseract_planning/tesseract_command_language/src/null_waypoint.cpp b/tesseract/tesseract_planning/tesseract_command_language/src/null_waypoint.cpp
@@ -24,6 +24,11 @@
  * limitations under the License.
  */
 
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <iostream>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
 #include <tesseract_command_language/null_waypoint.h>
 #include <tesseract_command_language/waypoint_type.h>
 
@@ -33,6 +38,8 @@ NullWaypoint::NullWaypoint(const tinyxml2::XMLElement& /*xml_element*/) {}
 
 int NullWaypoint::getType() const { return static_cast<int>(WaypointType::NULL_WAYPOINT); }
 
+void NullWaypoint::print(const std::string& prefix) const { std::cout << prefix << "Null WP"; };
+
 tinyxml2::XMLElement* NullWaypoint::toXML(tinyxml2::XMLDocument& doc) const
 {
   tinyxml2::XMLElement* xml_waypoint = doc.NewElement("Waypoint");

diff --git a/tesseract/tesseract_planning/tesseract_command_language/src/plan_instruction.cpp b/tesseract/tesseract_planning/tesseract_command_language/src/plan_instruction.cpp
@@ -61,7 +61,9 @@ void PlanInstruction::setDescription(const std::string& description) { descripti
 void PlanInstruction::print(std::string prefix) const
 {
   std::cout << prefix + "Plan Instruction, Type: " << getType() << ", Plan Type: " << static_cast<int>(plan_type_)
-            << ",  Waypoint Type:" << getWaypoint().getType() << ",  Description: " << getDescription() << std::endl;
+            << ", ";
+  getWaypoint().print();
+  std::cout << ", Description: " << getDescription() << std::endl;
 }
 
 PlanInstructionType PlanInstruction::getPlanType() const { return plan_type_; }

diff --git a/tesseract/tesseract_planning/tesseract_command_language/src/state_waypoint.cpp b/tesseract/tesseract_planning/tesseract_command_language/src/state_waypoint.cpp
@@ -138,6 +138,11 @@ StateWaypoint::StateWaypoint(const tinyxml2::XMLElement& xml_element)
 
 int StateWaypoint::getType() const { return static_cast<int>(WaypointType::STATE_WAYPOINT); }
 
+void StateWaypoint::print(const std::string& prefix) const
+{
+  std::cout << prefix << "State WP: Pos=" << position.transpose() << std::endl;
+}
+
 tinyxml2::XMLElement* StateWaypoint::toXML(tinyxml2::XMLDocument& doc) const
 {
   Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " ");