Do not overwrite the error code in planWithSinglePipeline (#2723)

* Do not overwrite the error code in planWithSinglePipeline Return the `MotionPlanResponse` as-is. Signed-off-by: Gaël Écorchard <[email protected]> * Do not rely on generatePlan() to set error code Do not rely on generatePlan() to set the error code in all cases and ensure that the error code is set to FAILURE if `generatePlan()` returns false. Signed-off-by: Gaël Écorchard <[email protected]> --------- Signed-off-by: Gaël Écorchard <[email protected]> Co-authored-by: Gaël Écorchard <[email protected]>
moveit · Mar 5, 2024 · 680b783 · 680b783
1 parent 57fd700
commit 680b783
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp
@@ -65,7 +65,13 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla
   const planning_pipeline::PlanningPipelinePtr pipeline = it->second;
   if (!pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response))
   {
-    motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE;
+    if ((motion_plan_response.error_code.val == moveit::core::MoveItErrorCode::SUCCESS) ||
+        (motion_plan_response.error_code.val == moveit::core::MoveItErrorCode::UNDEFINED))
+    {
+      RCLCPP_ERROR(getLogger(), "Planning pipeline '%s' failed to plan, but did not set an error code",
+                   motion_plan_request.pipeline_id.c_str());
+      motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE;
+    }
   }
   return motion_plan_response;
 }