Move getShortestSolution callback into own function

moveit · Sep 29, 2022 · 3427631 · 3427631
1 parent 79e913a
commit 3427631
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 49 deletions.
diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h
@@ -49,6 +49,10 @@ namespace moveit_cpp
 {
 MOVEIT_CLASS_FORWARD(PlanningComponent);  // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc
 
+/// \brief A function to choose the solution with the shortest path from a vector of solutions
+planning_interface::MotionPlanResponse
+getShortestSolution(std::vector<planning_interface::MotionPlanResponse> const& solutions);
+
 class PlanningComponent
 {
 public:
@@ -227,9 +231,10 @@ class PlanningComponent
 
   /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the
    * provided PlanRequestParameters. */
-  planning_interface::MotionPlanResponse plan(const MultiPipelinePlanRequestParameters& parameters,
-                                              SolutionCallbackFunction solution_selection_callback = nullptr,
-                                              StoppingCriterionFunction stopping_criterion_callback = nullptr);
+  planning_interface::MotionPlanResponse
+  plan(const MultiPipelinePlanRequestParameters& parameters,
+       SolutionCallbackFunction solution_selection_callback = &getShortestSolution,
+       StoppingCriterionFunction stopping_criterion_callback = nullptr);
 
   /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates
    * after the execution is complete. The execution can be run in background by setting blocking to false. */

diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
@@ -290,52 +290,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const MultiPipeli
     }
   }
 
-  // Select solution
-  // If no custom solution selection callback is provided, choose the shortest path available
-  if (solution_selection_callback == nullptr)
-  {
-    RCLCPP_DEBUG(LOGGER, "No custom solution selection callback provided, automatically choose shortest path");
-    auto const& solutions = planning_solutions.getSolutions();
-
-    // TODO Make debug before merge
-    for (auto const& solution : solutions)
-    {
-      RCLCPP_INFO_STREAM(LOGGER,
-                         "Solution error code is: " << moveit::core::error_code_to_string(solution.error_code_.val));
-      if (solution.trajectory_ != nullptr)
-      {
-        RCLCPP_INFO_STREAM(LOGGER, "Solution path length is: " << robot_trajectory::path_length(*solution.trajectory_));
-      }
-    }
-
-    // Find trajectory with minimal path
-    auto const shortest_trajectory =
-        std::min_element(solutions.begin(), solutions.end(),
-                         [](planning_interface::MotionPlanResponse const& solution_a,
-                            planning_interface::MotionPlanResponse const& solution_b) {
-                           // If both solutions were successful, check which path is shorter
-                           if (solution_a && solution_b)
-                           {
-                             return robot_trajectory::path_length(*solution_a.trajectory_) <
-                                    robot_trajectory::path_length(*solution_b.trajectory_);
-                           }
-                           // If only solution a is successful, return a
-                           else if (solution_a)
-                           {
-                             return true;
-                           }
-                           // Else return solution b, either because it is successful or not
-                           return false;
-                         });
-    if (shortest_trajectory->trajectory_ != nullptr)
-    {
-      RCLCPP_INFO_STREAM(LOGGER, "Chosen solution with path length: "
-                                     << robot_trajectory::path_length(*shortest_trajectory->trajectory_));
-    }
-    return *shortest_trajectory;
-  }
-
-  // Return best solution determined by user defined callback
+  // Return best solution determined by user defined callback (Default: Shortest path)
   return solution_selection_callback(planning_solutions.getSolutions());
 }
 
@@ -460,4 +415,37 @@ planning_interface::MotionPlanResponse const& PlanningComponent::getLastMotionPl
 {
   return last_plan_solution_;
 }
+
+planning_interface::MotionPlanResponse
+getShortestSolution(std::vector<planning_interface::MotionPlanResponse> const& solutions)
+{
+  // Find trajectory with minimal path
+  auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(),
+                                                    [](planning_interface::MotionPlanResponse const& solution_a,
+                                                       planning_interface::MotionPlanResponse const& solution_b) {
+                                                      // If both solutions were successful, check which path is shorter
+                                                      if (solution_a && solution_b)
+                                                      {
+                                                        return robot_trajectory::path_length(*solution_a.trajectory_) <
+                                                               robot_trajectory::path_length(*solution_b.trajectory_);
+                                                      }
+                                                      // If only solution a is successful, return a
+                                                      else if (solution_a)
+                                                      {
+                                                        return true;
+                                                      }
+                                                      // Else return solution b, either because it is successful or not
+                                                      return false;
+                                                    });
+  if (shortest_trajectory->trajectory_ != nullptr)
+  {
+    RCLCPP_INFO(LOGGER, "Chosen solution with shortest path length: '%f'",
+                robot_trajectory::path_length(*shortest_trajectory->trajectory_));
+  }
+  else
+  {
+    RCLCPP_INFO_STREAM(LOGGER, "Could not determine shortest path");
+  }
+  return *shortest_trajectory;
+}
 }  // namespace moveit_cpp