From 680b783256f78ad96df67a7b46213183752141b2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= Date: Tue, 5 Mar 2024 10:11:40 +0100 Subject: [PATCH] Do not overwrite the error code in planWithSinglePipeline (#2723) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Do not overwrite the error code in planWithSinglePipeline Return the `MotionPlanResponse` as-is. Signed-off-by: Gaël Écorchard * 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 --------- Signed-off-by: Gaël Écorchard Co-authored-by: Gaël Écorchard --- .../src/planning_pipeline_interfaces.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 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 index 73abd5fe4c..2e0e56223b 100644 --- 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; }