From dafa36fbebde6cea46d0c673c34bf1bf5ad7da3d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= Date: Tue, 5 Mar 2024 18:49:53 +0100 Subject: [PATCH] Do not overwrite the error code with OMPL interface (#2725) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit In case of failure, set the error code to the one returned by the planning pipeline's `solve` method rather than overwriting it with `PLANNING_FAILED`. Signed-off-by: Gaël Écorchard --- .../ompl_interface/src/model_based_planning_context.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index e086875fc0..fb6aee5345 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -802,12 +802,10 @@ void ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& re void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { res.planner_id = request_.planner_id; - moveit_msgs::msg::MoveItErrorCodes moveit_result = - solve(request_.allowed_planning_time, request_.num_planning_attempts); - if (moveit_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + res.error_code = solve(request_.allowed_planning_time, request_.num_planning_attempts); + if (res.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { RCLCPP_INFO(getLogger(), "Unable to solve the planning problem"); - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return; } @@ -845,7 +843,6 @@ void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResp RCLCPP_DEBUG(getLogger(), "%s: Returning successful solution with %lu states", getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); - res.error_code.val = moveit_result.val; } const moveit_msgs::msg::MoveItErrorCodes ModelBasedPlanningContext::solve(double timeout, unsigned int count)