Changes due to rebase and formatting

moveit · Aug 12, 2022 · eab6255 · eab6255
1 parent 56737e9
commit eab6255
Showing 1 changed file with 25 additions and 22 deletions.
diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
@@ -41,7 +41,6 @@
 #include <moveit/planning_pipeline/planning_pipeline.h>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <moveit/robot_state/conversions.h>
-#include <moveit/metrics/robot_trajectory_metrics.hpp>
 #include <thread>
 
 namespace moveit_cpp
@@ -232,30 +231,34 @@ PlanningComponent::plan(const PlanningComponent::MultiPipelinePlanRequestParamet
     RCLCPP_DEBUG(LOGGER, "No custom solution selection callback provided, automatically choose shortest path");
     auto const& solutions = planning_solutions.getSolutions();
 
-    for( auto solution : solutions ){
-      if(solution.trajectory != nullptr){
-      RCLCPP_INFO_STREAM(LOGGER, "Solution path length is: " << moveit::core::calculateTrajectoryLength(*solution.trajectory));}
+    for (auto solution : solutions)
+    {
+      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(),
-        [](PlanningComponent::PlanSolution const& solution_a, PlanningComponent::PlanSolution const& solution_b) {
-          // If both solutions were successful, check which path is shorter
-          if (solution_a && solution_b)
-          {
-            return moveit::core::calculateTrajectoryLength(*solution_a.trajectory) <
-                   moveit::core::calculateTrajectoryLength(*solution_b.trajectory);
-          }
-          // If only a is successful, return a
-          else if (solution_a)
-          {
-            return true;
-          }
-          // Else return b, either because it is successful or not
-          return false;
-        });
-    RCLCPP_INFO_STREAM(LOGGER, "Choosen solution with path length: " << moveit::core::calculateTrajectoryLength(*shortest_trajectory->trajectory));
+    auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(),
+                                                      [](PlanningComponent::PlanSolution const& solution_a,
+                                                         PlanningComponent::PlanSolution 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;
+                                                      });
+    RCLCPP_INFO_STREAM(LOGGER, "Chosen solution with path length: "
+                                   << robot_trajectory::path_length(*shortest_trajectory->trajectory));
     return *shortest_trajectory;
   }