Map ompl's APPROXIMATE_SOLUTION -> TIMED_OUT / PLANNING_FAILED (backport moveit#2455) (moveit#2460)

ompl's APPROXIMATE_SOLUTION is not suitable for actual execution. It just states that we got closer to the goal...
The most prominent reason for an approximate solution is a timeout. Thus, return TIMED_OUT and print the used timeouts for convenience.

(cherry picked from commit c0c6baf)

Co-authored-by: Robert Haschke <[email protected]>

Author: 2 people

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -860,7 +860,7 @@ const moveit_msgs::msg::MoveItErrorCodes ompl_interface::ModelBasedPlanningConte
     RCLCPP_DEBUG(LOGGER, "%s: Solving the planning problem once...", name_.c_str());
     ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start);
     registerTerminationCondition(ptc);
-    result.val = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION;
+    std::ignore = ompl_simple_setup_->solve(ptc);
     last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime();
     unregisterTerminationCondition();
     // fill the result status code
@@ -976,7 +976,7 @@ void ompl_interface::ModelBasedPlanningContext::unregisterTerminationCondition()
 int32_t ompl_interface::ModelBasedPlanningContext::logPlannerStatus(const og::SimpleSetupPtr& ompl_simple_setup)
 {
   auto result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
-  ompl::base::PlannerStatus ompl_status = ompl_simple_setup->getLastPlannerStatus();
+  const ompl::base::PlannerStatus ompl_status = ompl_simple_setup->getLastPlannerStatus();
   switch (ompl::base::PlannerStatus::StatusType(ompl_status))
   {
     case ompl::base::PlannerStatus::UNKNOWN:
@@ -996,12 +996,23 @@ int32_t ompl_interface::ModelBasedPlanningContext::logPlannerStatus(const og::Si
       result = moveit_msgs::msg::MoveItErrorCodes::UNRECOGNIZED_GOAL_TYPE;
       break;
     case ompl::base::PlannerStatus::TIMEOUT:
-      RCLCPP_WARN(LOGGER, "Timed out");
+      RCLCPP_WARN(LOGGER, "Timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(),
+                  request_.allowed_planning_time);
       result = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT;
       break;
     case ompl::base::PlannerStatus::APPROXIMATE_SOLUTION:
-      RCLCPP_WARN(LOGGER, "Solution is approximate");
-      result = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
+      // timeout is a common reason for APPROXIMATE_SOLUTION
+      if (ompl_simple_setup->getLastPlanComputationTime() > request_.allowed_planning_time)
+      {
+        RCLCPP_WARN(LOGGER, "Planning timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(),
+                    request_.allowed_planning_time);
+        result = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT;
+      }
+      else
+      {
+        RCLCPP_WARN(LOGGER, "Solution is approximate");
+        result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
+      }
       break;
     case ompl::base::PlannerStatus::EXACT_SOLUTION:
       result = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;