From afb183698a3892da4906899ef6c98a2180175b48 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 29 Sep 2022 15:58:36 +0200 Subject: [PATCH] Move getShortestSolution callback into own function --- .../moveit/moveit_cpp/planning_component.h | 11 ++- .../moveit_cpp/src/planning_component.cpp | 80 ++++++++----------- 2 files changed, 42 insertions(+), 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 index 90f91f943b3..e0e476f354b 100644 --- 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 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 index 7e1055420f5..f955a696e13 100644 --- 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 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: '%d'", + robot_trajectory::path_length(*shortest_trajectory->trajectory_)); + } + else + { + RCLCPP_INFO_STREAM(LOGGER, "Could not determine shortest path"); + } + return *shortest_trajectory; +} } // namespace moveit_cpp