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 <gael@km-robotics.cz>

* 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 <gael@km-robotics.cz>

---------

Signed-off-by: Gaël Écorchard <gael@km-robotics.cz>
Co-authored-by: Gaël Écorchard <gael@km-robotics.cz>

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;
 }