Merge branch 'master' into feature/task-composer-return-value-override

Author: Levi-Armstrong

tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h

@@ -30,6 +30,7 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Geometry>
 #include <console_bridge/console.h>
+#include <boost/core/demangle.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_command_language/constants.h>
@@ -114,20 +115,15 @@ std::shared_ptr<const ProfileType> getProfile(const std::string& ns,
     return std::static_pointer_cast<const ProfileType>(
         profile_dictionary.getProfile(ProfileType::getStaticKey(), ns, profile));
 
-  CONSOLE_BRIDGE_logDebug("Profile '%s' was not found in namespace '%s' for type '%s'. Using default if available. "
-                          "Available "
-                          "profiles:",
-                          profile.c_str(),
-                          ns.c_str(),
-                          typeid(ProfileType).name());
-
+  std::stringstream ss;
+  ss << "Profile '" << profile << "' was not found in namespace '" << ns << "' for type '"
+     << boost::core::demangle(typeid(ProfileType).name()) << "'. Using default if available. Available profiles: [";
   if (profile_dictionary.hasProfileEntry(ProfileType::getStaticKey(), ns))
-  {
     for (const auto& pair : profile_dictionary.getProfileEntry(ProfileType::getStaticKey(), ns))
-    {
-      CONSOLE_BRIDGE_logDebug("%s", pair.first.c_str());
-    }
-  }
+      ss << pair.first << ", ";
+  ss << "]";
+
+  CONSOLE_BRIDGE_logDebug(ss.str().c_str());
 
   return default_profile;
 }

tesseract_task_composer/core/src/task_composer_node.cpp

@@ -464,6 +464,7 @@ std::string TaskComposerNode::dump(std::ostream& os,
   if (conditional_)
   {
     os << "\n" << tmp << " [shape=diamond, nojustify=true label=\"" << name_ << "\\n";
+    os << "Type: " << boost::core::demangle(typeid(*this).name()) << "\\l";
     os << "UUID: " << uuid_str_ << "\\l";
     os << "Namespace: " << ns_ << "\\l";
     os << "Inputs:\\l" << input_keys_;
@@ -488,6 +489,7 @@ std::string TaskComposerNode::dump(std::ostream& os,
   else
   {
     os << "\n" << tmp << " [nojustify=true label=\"" << name_ << "\\n";
+    os << "Type: " << boost::core::demangle(typeid(*this).name()) << "\\l";
     os << "UUID: " << uuid_str_ << "\\l";
     os << "Namespace: " << ns_ << "\\l";
     os << "Inputs:\\l" << input_keys_;

tesseract_time_parameterization/core/src/instructions_trajectory.cpp

@@ -56,8 +56,6 @@ InstructionsTrajectory::InstructionsTrajectory(CompositeInstruction& program)
 
 const Eigen::VectorXd& InstructionsTrajectory::getPosition(Eigen::Index i) const
 {
-  assert(trajectory_[static_cast<std::size_t>(i)].get().isMoveInstruction());
-  assert(trajectory_[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>().getWaypoint().isStateWaypoint());
   return trajectory_[static_cast<std::size_t>(i)]
       .get()
       .as<MoveInstructionPoly>()
@@ -68,8 +66,6 @@ const Eigen::VectorXd& InstructionsTrajectory::getPosition(Eigen::Index i) const
 
 Eigen::VectorXd& InstructionsTrajectory::getPosition(Eigen::Index i)
 {
-  assert(trajectory_[static_cast<std::size_t>(i)].get().isMoveInstruction());
-  assert(trajectory_[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>().getWaypoint().isStateWaypoint());
   return trajectory_[static_cast<std::size_t>(i)]
       .get()
       .as<MoveInstructionPoly>()
@@ -80,8 +76,6 @@ Eigen::VectorXd& InstructionsTrajectory::getPosition(Eigen::Index i)
 
 const Eigen::VectorXd& InstructionsTrajectory::getVelocity(Eigen::Index i) const
 {
-  assert(trajectory_[static_cast<std::size_t>(i)].get().isMoveInstruction());
-  assert(trajectory_[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>().getWaypoint().isStateWaypoint());
   return trajectory_[static_cast<std::size_t>(i)]
       .get()
       .as<MoveInstructionPoly>()
@@ -92,8 +86,6 @@ const Eigen::VectorXd& InstructionsTrajectory::getVelocity(Eigen::Index i) const
 
 Eigen::VectorXd& InstructionsTrajectory::getVelocity(Eigen::Index i)
 {
-  assert(trajectory_[static_cast<std::size_t>(i)].get().isMoveInstruction());
-  assert(trajectory_[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>().getWaypoint().isStateWaypoint());
   return trajectory_[static_cast<std::size_t>(i)]
       .get()
       .as<MoveInstructionPoly>()
@@ -104,8 +96,6 @@ Eigen::VectorXd& InstructionsTrajectory::getVelocity(Eigen::Index i)
 
 const Eigen::VectorXd& InstructionsTrajectory::getAcceleration(Eigen::Index i) const
 {
-  assert(trajectory_[static_cast<std::size_t>(i)].get().isMoveInstruction());
-  assert(trajectory_[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>().getWaypoint().isStateWaypoint());
   return trajectory_[static_cast<std::size_t>(i)]
       .get()
       .as<MoveInstructionPoly>()
@@ -116,8 +106,6 @@ const Eigen::VectorXd& InstructionsTrajectory::getAcceleration(Eigen::Index i) c
 
 Eigen::VectorXd& InstructionsTrajectory::getAcceleration(Eigen::Index i)
 {
-  assert(trajectory_[static_cast<std::size_t>(i)].get().isMoveInstruction());
-  assert(trajectory_[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>().getWaypoint().isStateWaypoint());
   return trajectory_[static_cast<std::size_t>(i)]
       .get()
       .as<MoveInstructionPoly>()
@@ -128,8 +116,6 @@ Eigen::VectorXd& InstructionsTrajectory::getAcceleration(Eigen::Index i)
 
 double InstructionsTrajectory::getTimeFromStart(Eigen::Index i) const
 {
-  assert(trajectory_[static_cast<std::size_t>(i)].get().isMoveInstruction());
-  assert(trajectory_[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>().getWaypoint().isStateWaypoint());
   return trajectory_[static_cast<std::size_t>(i)]
       .get()
       .as<MoveInstructionPoly>()
@@ -143,8 +129,6 @@ void InstructionsTrajectory::setData(Eigen::Index i,
                                      const Eigen::VectorXd& acceleration,
                                      double time)
 {
-  assert(trajectory_[static_cast<std::size_t>(i)].get().isMoveInstruction());
-  assert(trajectory_[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>().getWaypoint().isStateWaypoint());
   auto& swp =
       trajectory_[static_cast<std::size_t>(i)].get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
   swp.setVelocity(velocity);