perf(goal_planner): remove unnecessary call to setMap on freespace planning (#9644)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

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

@@ -18,6 +18,7 @@
 #include "autoware/behavior_path_goal_planner_module/decision_state.hpp"
 #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp"
 #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
+#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp"
 #include "autoware/behavior_path_goal_planner_module/thread_data.hpp"
 #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
@@ -151,7 +152,7 @@ bool checkOccupancyGridCollision(
 // freespace parking
 std::optional<PullOverPath> planFreespacePath(
   const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects,
-  std::shared_ptr<PullOverPlannerBase> freespace_planner);
+  std::shared_ptr<FreespacePullOver> freespace_planner);
 
 bool isStopped(
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer,
@@ -198,7 +199,7 @@ class FreespaceParkingPlanner
     std::mutex & freespace_parking_mutex, const std::optional<FreespaceParkingRequest> & request,
     FreespaceParkingResponse & response, std::atomic<bool> & is_freespace_parking_cb_running,
     const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock,
-    const std::shared_ptr<PullOverPlannerBase> freespace_planner)
+    const std::shared_ptr<FreespacePullOver> freespace_planner)
   : mutex_(freespace_parking_mutex),
     request_(request),
     response_(response),
@@ -218,7 +219,7 @@ class FreespaceParkingPlanner
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
 
-  std::shared_ptr<PullOverPlannerBase> freespace_planner_;
+  std::shared_ptr<FreespacePullOver> freespace_planner_;
 
   bool isStuck(
     const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp

@@ -36,6 +36,8 @@ class FreespacePullOver : public PullOverPlannerBase
 
   PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; }
 
+  void setMap(const nav_msgs::msg::OccupancyGrid & costmap) { planner_->setMap(costmap); }
+
   std::optional<PullOverPath> plan(
     const GoalCandidate & modified_goal_pose, const size_t id,
     const std::shared_ptr<const PlannerData> planner_data,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -16,7 +16,6 @@
 
 #include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp"
 #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp"
-#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp"
 #include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp"
 #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp"
 #include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp"
@@ -225,7 +224,7 @@ bool checkOccupancyGridCollision(
 
 std::optional<PullOverPath> planFreespacePath(
   const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects,
-  std::shared_ptr<PullOverPlannerBase> freespace_planner)
+  std::shared_ptr<FreespacePullOver> freespace_planner)
 {
   auto goal_candidates = req.goal_candidates_;
   auto goal_searcher = std::make_shared<GoalSearcher>(req.parameters_, req.vehicle_footprint_);
@@ -238,8 +237,8 @@ std::optional<PullOverPath> planFreespacePath(
     if (!goal_candidate.is_safe) {
       continue;
     }
-    // TODO(soblin): this calls setMap() in freespace_planner in the for-loop, which is very
-    // inefficient
+
+    freespace_planner->setMap(*(req.get_planner_data()->costmap));
     const auto freespace_path = freespace_planner->plan(
       goal_candidate, 0, req.get_planner_data(), BehaviorModuleOutput{}
       // NOTE: not used so passing {} is OK

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp

@@ -64,8 +64,6 @@ std::optional<PullOverPath> FreespacePullOver::plan(
 {
   const Pose & current_pose = planner_data->self_odometry->pose.pose;
 
-  planner_->setMap(*planner_data->costmap);
-
   // offset goal pose to make straight path near goal for improving parking precision
   // todo: support straight path when using back
   constexpr double straight_distance = 1.0;