Fixing Descartes not aborting on collision failure

tesseract-robotics · Apr 28, 2024 · c732055 · c732055
1 parent 28752ea
commit c732055
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/...s/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/...s/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp
@@ -86,7 +86,12 @@ PlannerResponse DescartesMotionPlanner<FloatType>::solve(const PlannerRequest& r
   try
   {
     descartes_light::LadderGraphSolver<FloatType> solver(problem->num_threads);
-    solver.build(problem->samplers, problem->edge_evaluators, problem->state_evaluators);
+    if (!solver.build(problem->samplers, problem->edge_evaluators, problem->state_evaluators))
+    {
+      response.successful = false;
+      response.message = ERROR_FAILED_TO_BUILD_GRAPH;
+      return response;
+    }
     descartes_result = solver.search();
     if (descartes_result.trajectory.empty())
     {

diff --git a/..._composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/..._composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp
@@ -179,6 +179,8 @@ class MotionPlannerTask : public TaskComposerTask
       return info;
     }
 
+    context.abort(uuid_);
+
     CONSOLE_BRIDGE_logInform("%s motion planning failed (%s) for process input: %s",
                              planner_->getName().c_str(),
                              response.message.c_str(),