Fix descartes planner check if the graph built

tesseract-robotics · May 30, 2024 · cf0a863 · cf0a863
1 parent a7371ce
commit cf0a863
Showing 1 changed file with 10 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,16 @@ 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);
+
+    // Build Graph
+    if (!solver.build(problem->samplers, problem->edge_evaluators, problem->state_evaluators))
+    {
+      response.successful = false;
+      response.message = ERROR_FAILED_TO_BUILD_GRAPH;
+      return response;
+    }
+
+    // Search Graph
     descartes_result = solver.search();
     if (descartes_result.trajectory.empty())
     {