perf(goal_planner): reduce processing time (autowarefoundation#8195)

* perf(goal_palnner): reduce processing time

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add const& return

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* use copy getter

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* pre commit

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -228,6 +228,8 @@ class ThreadSafeData
   DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<BehaviorModuleOutput>, last_previous_module_output)
   DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data)
   DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check)
+  DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects)
+  DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects)
 
 private:
   std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
@@ -241,6 +243,8 @@ class ThreadSafeData
   std::optional<BehaviorModuleOutput> last_previous_module_output_{};
   PreviousPullOverData prev_data_{};
   CollisionCheckDebugMap collision_check_{};
+  PredictedObjects static_target_objects_{};
+  PredictedObjects dynamic_target_objects_{};
 
   std::recursive_mutex & mutex_;
   rclcpp::Clock::SharedPtr clock_;
@@ -520,9 +524,10 @@ class GoalPlannerModule : public SceneModuleInterface
     const PathWithLaneId & path,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;
   bool checkObjectsCollision(
-    const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
-    const GoalPlannerParameters & parameters, const double collision_check_margin,
-    const bool extract_static_objects, const bool update_debug_data = false) const;
+    const PathWithLaneId & path, const std::vector<double> & curvatures,
+    const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
+    const double collision_check_margin, const bool extract_static_objects,
+    const bool update_debug_data = false) const;
 
   // goal seach
   Pose calcRefinedGoal(const Pose & goal_pose) const;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp

@@ -52,41 +52,29 @@ struct PullOverPath
   size_t goal_id{};
   size_t id{};
   bool decided_velocity{false};
-  double max_curvature{0.0};  // max curvature of the parking path
 
-  PathWithLaneId getFullPath() const
+  /**
+   * @brief Set paths and start/end poses
+   * By setting partial_paths, full_path, parking_path and curvature are also set at the same time
+   * @param input_partial_paths partial paths
+   * @param input_start_pose start pose
+   * @param input_end_pose end pose
+   */
+  void setPaths(
+    const std::vector<PathWithLaneId> input_partial_paths, const Pose & input_start_pose,
+    const Pose & input_end_pose)
   {
-    PathWithLaneId path{};
-    for (size_t i = 0; i < partial_paths.size(); ++i) {
-      if (i == 0) {
-        path.points.insert(
-          path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end());
-      } else {
-        // skip overlapping point
-        path.points.insert(
-          path.points.end(), next(partial_paths.at(i).points.begin()),
-          partial_paths.at(i).points.end());
-      }
-    }
-    path.points = autoware::motion_utils::removeOverlapPoints(path.points);
+    partial_paths = input_partial_paths;
+    start_pose = input_start_pose;
+    end_pose = input_end_pose;
 
-    return path;
+    updatePathData();
   }
 
-  // path from the pull over start pose to the end pose
-  PathWithLaneId getParkingPath() const
-  {
-    const PathWithLaneId full_path = getFullPath();
-    const size_t start_idx =
-      autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position);
+  // Note: return copy value (not const&) because the value is used in multi threads
+  PathWithLaneId getFullPath() const { return full_path; }
 
-    PathWithLaneId parking_path{};
-    std::copy(
-      full_path.points.begin() + start_idx, full_path.points.end(),
-      std::back_inserter(parking_path.points));
-
-    return parking_path;
-  }
+  PathWithLaneId getParkingPath() const { return parking_path; }
 
   PathWithLaneId getCurrentPath() const
   {
@@ -108,6 +96,82 @@ struct PullOverPath
   }
 
   bool isValidPath() const { return type != PullOverPlannerType::NONE; }
+
+  std::vector<double> getFullPathCurvatures() const { return full_path_curvatures; }
+  std::vector<double> getParkingPathCurvatures() const { return parking_path_curvatures; }
+  double getFullPathMaxCurvature() const { return full_path_max_curvature; }
+  double getParkingPathMaxCurvature() const { return parking_path_max_curvature; }
+
+private:
+  void updatePathData()
+  {
+    updateFullPath();
+    updateParkingPath();
+    updateCurvatures();
+  }
+
+  void updateFullPath()
+  {
+    PathWithLaneId path{};
+    for (size_t i = 0; i < partial_paths.size(); ++i) {
+      if (i == 0) {
+        path.points.insert(
+          path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end());
+      } else {
+        // skip overlapping point
+        path.points.insert(
+          path.points.end(), next(partial_paths.at(i).points.begin()),
+          partial_paths.at(i).points.end());
+      }
+    }
+    full_path.points = autoware::motion_utils::removeOverlapPoints(path.points);
+  }
+
+  void updateParkingPath()
+  {
+    if (full_path.points.empty()) {
+      updateFullPath();
+    }
+    const size_t start_idx =
+      autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position);
+
+    PathWithLaneId path{};
+    std::copy(
+      full_path.points.begin() + start_idx, full_path.points.end(),
+      std::back_inserter(path.points));
+    parking_path = path;
+  }
+
+  void updateCurvatures()
+  {
+    const auto calculateCurvaturesAndMax =
+      [](const auto & path) -> std::pair<std::vector<double>, double> {
+      std::vector<double> curvatures = autoware::motion_utils::calcCurvature(path.points);
+      double max_curvature = 0.0;
+      if (!curvatures.empty()) {
+        max_curvature = std::abs(*std::max_element(
+          curvatures.begin(), curvatures.end(),
+          [](const double & a, const double & b) { return std::abs(a) < std::abs(b); }));
+      }
+      return std::make_pair(curvatures, max_curvature);
+    };
+    std::tie(full_path_curvatures, full_path_max_curvature) =
+      calculateCurvaturesAndMax(getFullPath());
+    std::tie(parking_path_curvatures, parking_path_max_curvature) =
+      calculateCurvaturesAndMax(getParkingPath());
+  }
+
+  // curvatures
+  std::vector<double> full_path_curvatures{};
+  std::vector<double> parking_path_curvatures{};
+  std::vector<double> current_path_curvatures{};
+  double parking_path_max_curvature{0.0};
+  double full_path_max_curvature{0.0};
+  double current_path_max_curvature{0.0};
+
+  // path
+  PathWithLaneId full_path{};
+  PathWithLaneId parking_path{};
 };
 
 class PullOverPlannerBase

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

@@ -42,6 +42,7 @@ using geometry_msgs::msg::Twist;
 using tier4_planning_msgs::msg::PathWithLaneId;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
+using Shape = autoware_perception_msgs::msg::Shape;
 using Polygon2d = autoware::universe_utils::Polygon2d;
 
 lanelet::ConstLanelets getPullOverLanes(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp

@@ -129,10 +129,8 @@ std::optional<PullOverPath> FreespacePullOver::plan(const Pose & goal_pose)
   }
 
   PullOverPath pull_over_path{};
-  pull_over_path.partial_paths = partial_paths;
   pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel;
-  pull_over_path.start_pose = current_pose;
-  pull_over_path.end_pose = goal_pose;
+  pull_over_path.setPaths(partial_paths, current_pose, goal_pose);
   pull_over_path.type = getPlannerType();
 
   return pull_over_path;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp

@@ -72,10 +72,8 @@ std::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
 
   PullOverPath pull_over_path{};
   pull_over_path.type = getPlannerType();
-  pull_over_path.partial_paths = planner_.getPaths();
   pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel();
-  pull_over_path.start_pose = planner_.getStartPose();
-  pull_over_path.end_pose = planner_.getArcEndPose();
+  pull_over_path.setPaths(planner_.getPaths(), planner_.getStartPose(), planner_.getArcEndPose());
 
   return pull_over_path;
 }