Feat/more verbose planning failures (#440)

tesseract_motion_planners/core/include/tesseract_motion_planners/core/planner.h

@@ -72,6 +72,9 @@ class MotionPlanner
   /** @brief Check planning request */
   static bool checkRequest(const PlannerRequest& request);
 
+  /** @brief Check planning request and give reason for failure */
+  static bool checkRequest(const PlannerRequest& request, std::string& reason);
+
   /** @brief Assign a solution to the move instruction */
   static void assignSolution(MoveInstructionPoly& mi,
                              const std::vector<std::string>& joint_names,

tesseract_motion_planners/core/src/planner.cpp

@@ -37,17 +37,25 @@ MotionPlanner::MotionPlanner(std::string name) : name_(std::move(name))
 const std::string& MotionPlanner::getName() const { return name_; }
 
 bool MotionPlanner::checkRequest(const PlannerRequest& request)
+{
+  std::string reason;
+  return checkRequest(request, reason);
+}
+
+bool MotionPlanner::checkRequest(const PlannerRequest& request, std::string& reason)
 {
   // Check that parameters are valid
   if (request.env == nullptr)
   {
-    CONSOLE_BRIDGE_logError("In TrajOptPlannerUniversalConfig: tesseract is a required parameter and has not been set");
+    reason = "PlannerRequest environment is nullptr";
+    CONSOLE_BRIDGE_logError(reason.c_str());
     return false;
   }
 
   if (request.instructions.empty())
   {
-    CONSOLE_BRIDGE_logError("TrajOptPlannerUniversalConfig requires at least one instruction");
+    reason = "PlannerRequest instruction is empty";
+    CONSOLE_BRIDGE_logError(reason.c_str());
     return false;
   }
 

tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp

@@ -48,7 +48,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/utils.h>
 
 constexpr auto SOLUTION_FOUND{ "Found valid solution" };
-constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" };
+constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
 constexpr auto ERROR_FAILED_TO_BUILD_GRAPH{ "Failed to build graph" };
 constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
 

tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp

@@ -46,8 +46,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/utils.h>
 
 constexpr auto SOLUTION_FOUND{ "Found valid solution" };
-constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" };
-constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
+constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
+constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " };
 
 namespace tesseract_planning
 {
@@ -100,10 +100,11 @@ bool OMPLMotionPlanner::terminate()
 PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
 {
   PlannerResponse response;
-  if (!checkRequest(request))  // NOLINT
+  std::string reason;
+  if (!checkRequest(request, reason))
   {
     response.successful = false;
-    response.message = ERROR_INVALID_INPUT;
+    response.message = std::string(ERROR_INVALID_INPUT) + reason;
     return response;
   }
   std::vector<OMPLProblemConfig> problems;
@@ -121,7 +122,7 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
     {
       CONSOLE_BRIDGE_logError("OMPLPlanner failed to generate problem: %s.", e.what());
       response.successful = false;
-      response.message = ERROR_INVALID_INPUT;
+      response.message = std::string(ERROR_INVALID_INPUT) + e.what();
       return response;
     }
 
@@ -171,7 +172,8 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
 
           if (!pdef->hasOptimizationObjective())
           {
-            CONSOLE_BRIDGE_logDebug("Terminating early since there is no optimization objective specified");
+            reason = "Terminating early since there is no optimization objective specified";
+            CONSOLE_BRIDGE_logDebug(reason.c_str());
             break;
           }
 
@@ -180,23 +182,27 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
 
           if (pdef->getOptimizationObjective()->isSatisfied(obj_cost))
           {
-            CONSOLE_BRIDGE_logDebug("Terminating early since solution path satisfies the optimization objective");
+            reason = "Terminating early since solution path satisfies the optimization objective";
+            CONSOLE_BRIDGE_logDebug(reason.c_str());
             break;
           }
 
           if (pdef->getSolutionCount() >= static_cast<std::size_t>(p->max_solutions))
           {
-            CONSOLE_BRIDGE_logDebug("Terminating early since %u solutions were generated", p->max_solutions);
+            reason = "Terminating early since " + std::to_string(p->max_solutions) + " solutions were generated";
+            CONSOLE_BRIDGE_logDebug(reason.c_str());
             break;
           }
         }
       }
+      if (ompl::time::now() >= end)
+        reason = "Exceeded allowed time";
     }
 
     if (status != ompl::base::PlannerStatus::EXACT_SOLUTION)
     {
       response.successful = false;
-      response.message = ERROR_FAILED_TO_FIND_VALID_SOLUTION;
+      response.message = std::string(ERROR_FAILED_TO_FIND_VALID_SOLUTION) + reason;
       return response;
     }
 

tesseract_motion_planners/simple/src/simple_motion_planner.cpp

@@ -41,8 +41,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_motion_planners/planner_utils.h>
 
 constexpr auto SOLUTION_FOUND{ "Found valid solution" };
-constexpr auto ERROR_INVALID_INPUT{ "Input to planner is invalid. Check that instructions and seed are compatible" };
-constexpr auto FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
+constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
+constexpr auto FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " };
 
 namespace tesseract_planning
 {
@@ -61,10 +61,11 @@ MotionPlanner::Ptr SimpleMotionPlanner::clone() const { return std::make_shared<
 PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const
 {
   PlannerResponse response;
-  if (!checkRequest(request))
+  std::string reason;
+  if (!checkRequest(request, reason))  // NOLINT
   {
     response.successful = false;
-    response.message = ERROR_INVALID_INPUT;
+    response.message = std::string(ERROR_INVALID_INPUT) + reason;
     return response;
   }
 
@@ -93,7 +94,7 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const
   {
     CONSOLE_BRIDGE_logError("SimplePlanner failed to generate problem: %s.", e.what());
     response.successful = false;
-    response.message = FAILED_TO_FIND_VALID_SOLUTION;
+    response.message = std::string(FAILED_TO_FIND_VALID_SOLUTION) + e.what();
     return response;
   }
 

tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp

@@ -45,8 +45,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/utils.h>
 
 constexpr auto SOLUTION_FOUND{ "Found valid solution" };
-constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" };
-constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
+constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
+constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution: " };
 
 using namespace trajopt;
 
@@ -67,10 +67,11 @@ MotionPlanner::Ptr TrajOptMotionPlanner::clone() const { return std::make_shared
 PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
 {
   PlannerResponse response;
-  if (!checkRequest(request))
+  std::string reason;
+  if (!checkRequest(request, reason))
   {
     response.successful = false;
-    response.message = ERROR_INVALID_INPUT;
+    response.message = std::string(ERROR_INVALID_INPUT) + reason;
     return response;
   }
 
@@ -89,7 +90,7 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
     {
       CONSOLE_BRIDGE_logError("TrajOptPlanner failed to generate problem: %s.", e.what());
       response.successful = false;
-      response.message = ERROR_INVALID_INPUT;
+      response.message = std::string(ERROR_INVALID_INPUT) + e.what();
       return response;
     }
 
@@ -126,8 +127,12 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
   if (opt->results().status != sco::OptStatus::OPT_CONVERGED)
   {
     response.successful = false;
-    response.message = ERROR_FAILED_TO_FIND_VALID_SOLUTION;
-    return response;
+    response.message = std::string(ERROR_FAILED_TO_FIND_VALID_SOLUTION) + sco::statusToString(opt->results().status);
+  }
+  else
+  {
+    response.successful = true;
+    response.message = SOLUTION_FOUND;
   }
 
   const std::vector<std::string> joint_names = problem->GetKin()->getJointNames();
@@ -154,8 +159,6 @@ PlannerResponse TrajOptMotionPlanner::solve(const PlannerRequest& request) const
         move_instruction, joint_names, traj.row(static_cast<Eigen::Index>(idx)), request.format_result_as_input);
   }
 
-  response.successful = true;
-  response.message = SOLUTION_FOUND;
   return response;
 }
 

tesseract_motion_planners/trajopt/src/trajopt_utils.cpp

@@ -192,7 +192,8 @@ trajopt::TermInfo::Ptr
 createSmoothVelocityTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type)
 {
   if ((end_index - start_index) < 1)
-    throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!");
+    throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states, failed with start_index " +
+                             std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
 
   std::shared_ptr<trajopt::JointVelTermInfo> jv = std::make_shared<trajopt::JointVelTermInfo>();
   jv->coeffs = std::vector<double>(static_cast<std::size_t>(n_joints), coeff);
@@ -210,7 +211,8 @@ trajopt::TermInfo::Ptr createSmoothVelocityTermInfo(int start_index,
                                                     trajopt::TermType type)
 {
   if ((end_index - start_index) < 1)
-    throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!");
+    throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states, failed with start_index " +
+                             std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
 
   std::shared_ptr<trajopt::JointVelTermInfo> jv = std::make_shared<trajopt::JointVelTermInfo>();
   jv->coeffs = std::vector<double>(coeff.data(), coeff.data() + coeff.size());
@@ -226,7 +228,8 @@ trajopt::TermInfo::Ptr
 createSmoothAccelerationTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type)
 {
   if ((end_index - start_index) < 2)
-    throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!");
+    throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states, failed with start_index " +
+                             std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
 
   std::shared_ptr<trajopt::JointAccTermInfo> ja = std::make_shared<trajopt::JointAccTermInfo>();
   ja->coeffs = std::vector<double>(static_cast<std::size_t>(n_joints), coeff);
@@ -244,7 +247,8 @@ trajopt::TermInfo::Ptr createSmoothAccelerationTermInfo(int start_index,
                                                         trajopt::TermType type)
 {
   if ((end_index - start_index) < 2)
-    throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!");
+    throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states, failed with start_index " +
+                             std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
 
   std::shared_ptr<trajopt::JointAccTermInfo> ja = std::make_shared<trajopt::JointAccTermInfo>();
   ja->coeffs = std::vector<double>(coeff.data(), coeff.data() + coeff.size());
@@ -260,7 +264,8 @@ trajopt::TermInfo::Ptr
 createSmoothJerkTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type)
 {
   if ((end_index - start_index) < 4)
-    throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!");
+    throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states, failed with start_index " +
+                             std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
 
   std::shared_ptr<trajopt::JointJerkTermInfo> jj = std::make_shared<trajopt::JointJerkTermInfo>();
   jj->coeffs = std::vector<double>(static_cast<std::size_t>(n_joints), coeff);
@@ -278,7 +283,8 @@ trajopt::TermInfo::Ptr createSmoothJerkTermInfo(int start_index,
                                                 trajopt::TermType type)
 {
   if ((end_index - start_index) < 4)
-    throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!");
+    throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states, failed with start_index " +
+                             std::to_string(start_index) + " & end_index " + std::to_string(end_index) + "!");
 
   std::shared_ptr<trajopt::JointJerkTermInfo> jj = std::make_shared<trajopt::JointJerkTermInfo>();
   jj->coeffs = std::vector<double>(coeff.data(), coeff.data() + coeff.size());

tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp

@@ -43,7 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/utils.h>
 
 constexpr auto SOLUTION_FOUND{ "Found valid solution" };
-constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input" };
+constexpr auto ERROR_INVALID_INPUT{ "Failed invalid input: " };
 constexpr auto ERROR_FAILED_TO_FIND_VALID_SOLUTION{ "Failed to find valid solution" };
 
 using namespace trajopt_ifopt;
@@ -66,10 +66,11 @@ MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const
 PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) const
 {
   PlannerResponse response;
-  if (!checkRequest(request))
+  std::string reason;
+  if (!checkRequest(request, reason))
   {
     response.successful = false;
-    response.message = ERROR_INVALID_INPUT;
+    response.message = std::string(ERROR_INVALID_INPUT) + reason;
     return response;
   }
 

tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp

@@ -157,14 +157,13 @@ class MotionPlannerTask : public TaskComposerTask
     if (console_bridge::getLogLevel() == console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG)
       request.verbose = true;
     PlannerResponse response = planner_->solve(request);
+    context.data_storage->setData(output_keys_[0], response.results);
 
     // --------------------
     // Verify Success
     // --------------------
     if (response)
     {
-      context.data_storage->setData(output_keys_[0], response.results);
-
       info->return_value = 1;
       info->color = "green";
       info->message = response.message;