Simplify ModelBasedPlanningContext::solve()

Gaël Écorchard · Gaël Écorchard · commit 6fb20ee04912 · 2024-03-05T06:37:20.000+01:00
Signed-off-by: Gaël Écorchard &lt;gael@km-robotics.cz&gt;
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
@@ -800,12 +800,10 @@ void ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& re
 
 void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res)
 {
-  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 = moveit_result;
     return;
   }
 
@@ -843,7 +841,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)

Original file line number	Diff line number	Diff line change
`@@ -800,12 +800,10 @@ void ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& re`
`800`	`800`
`801`	`801`	`void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res)`
`802`	`802`	`{`
`803`		`- moveit_msgs::msg::MoveItErrorCodes moveit_result =`
`804`		`- solve(request_.allowed_planning_time, request_.num_planning_attempts);`
`805`		`- if (moveit_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)`
	`803`	`+ res.error_code = solve(request_.allowed_planning_time, request_.num_planning_attempts);`
	`804`	`+ if (res.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)`
`806`	`805`	`{`
`807`	`806`	`RCLCPP_INFO(getLogger(), "Unable to solve the planning problem");`
`808`		`- res.error_code = moveit_result;`
`809`	`807`	`return;`
`810`	`808`	`}`
`811`	`809`
`@@ -843,7 +841,6 @@ void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResp`
`843`	`841`
`844`	`842`	`RCLCPP_DEBUG(getLogger(), "%s: Returning successful solution with %lu states", getName().c_str(),`
`845`	`843`	`getOMPLSimpleSetup()->getSolutionPath().getStateCount());`
`846`		`- res.error_code.val = moveit_result.val;`
`847`	`844`	`}`
`848`	`845`
`849`	`846`	`const moveit_msgs::msg::MoveItErrorCodes ModelBasedPlanningContext::solve(double timeout, unsigned int count)`