Fix motion planner task to support error branching

tesseract-robotics · Jan 12, 2025 · 770480a · 770480a
1 parent 2f3cbfd
commit 770480a
Showing 1 changed file with 4 additions and 2 deletions.
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
@@ -174,7 +174,8 @@ class MotionPlannerTask : public TaskComposerTask
 
     // Make a non-const copy of the input instructions to update the start/end
     auto& instructions = input_data_poly.template as<CompositeInstruction>();
-    assert(!(input_manip_info.empty() && instructions.getManipulatorInfo().empty()));
+    if (input_manip_info.empty() && instructions.getManipulatorInfo().empty())
+      throw std::runtime_error("Missing manipulator information");
     instructions.setManipulatorInfo(instructions.getManipulatorInfo().getCombined(input_manip_info));
 
     // --------------------
@@ -193,13 +194,14 @@ class MotionPlannerTask : public TaskComposerTask
     if (console_bridge::getLogLevel() == console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG)
       request.verbose = true;
     PlannerResponse response = planner_->solve(request);
-    setData(*context.data_storage, INOUT_PROGRAM_PORT, response.results);
 
     // --------------------
     // Verify Success
     // --------------------
     if (response)
     {
+      // Should only set on success to support error branching
+      setData(*context.data_storage, INOUT_PROGRAM_PORT, response.results);
       info->return_value = 1;
       info->color = "green";
       info->status_code = 1;