From c73205583685911defacd8c36176e61b5b561027 Mon Sep 17 00:00:00 2001 From: Jack-ReframeSystems Date: Sun, 28 Apr 2024 13:34:03 -0400 Subject: [PATCH] Fixing Descartes not aborting on collision failure --- .../descartes/impl/descartes_motion_planner.hpp | 7 ++++++- .../planning/nodes/motion_planner_task.hpp | 2 ++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp index c20a66dbf46..be5ad7111be 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp @@ -86,7 +86,12 @@ PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& r try { descartes_light::LadderGraphSolver 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/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp index 732be57c16d..d817cdb94d8 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp +++ b/tesseract_task_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(),