feat(goal_planner): divide Planners to isolated threads

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

@@ -64,7 +64,6 @@ class LaneParkingRequest
   }
   const std::optional<PullOverPath> & get_pull_over_path() const { return pull_over_path_; }
   const PathDecisionState & get_prev_data() const { return prev_data_; }
-  LaneParkingRequest clone() const;
 
 private:
   std::shared_ptr<PlannerData> planner_data_;
@@ -118,8 +117,6 @@ class FreespaceParkingRequest
   }
   bool is_stopped() const { return is_stopped_; }
 
-  FreespaceParkingRequest clone() const;
-
 private:
   std::shared_ptr<PlannerData> planner_data_;
   ModuleStatus current_status_;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -91,13 +91,12 @@
   const auto lane_parking_period_ns = rclcpp::Rate(1.0).period();
   lane_parking_timer_cb_group_ =
     node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  auto lane_parking_planner = std::make_unique<LaneParkingPlanner>(
-    node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_,
-    is_lane_parking_cb_running_, getLogger(), *parameters_);
   lane_parking_timer_ = rclcpp::create_timer(
     &node, clock_, lane_parking_period_ns,
-    [lane_parking_planner_bind = std::move(lane_parking_planner)]() {
-      lane_parking_planner_bind->onTimer();
+    [lane_parking_executor = std::make_unique<LaneParkingPlanner>(
+       node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_,
+       is_lane_parking_cb_running_, getLogger(), *parameters_)]() {
+      lane_parking_executor->onTimer();
     },
     lane_parking_timer_cb_group_);
 
@@ -107,13 +106,12 @@
     const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period();
     freespace_parking_timer_cb_group_ =
       node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-    auto freespace_parking_planner = std::make_unique<FreespaceParkingPlanner>(
-      freespace_parking_mutex_, freespace_parking_request_, freespace_parking_response_,
-      is_freespace_parking_cb_running_, getLogger(), clock_, freespace_planner);
     freespace_parking_timer_ = rclcpp::create_timer(
       &node, clock_, freespace_parking_period_ns,
-      [freespace_parking_planner_bind = std::move(freespace_parking_planner)]() {
-        freespace_parking_planner_bind->onTimer();
+      [freespace_parking_executor = std::make_unique<FreespaceParkingPlanner>(
+         freespace_parking_mutex_, freespace_parking_request_, freespace_parking_response_,
+         is_freespace_parking_cb_running_, getLogger(), clock_, freespace_planner)]() {
+        freespace_parking_executor->onTimer();
       },
       freespace_parking_timer_cb_group_);
   }
@@ -280,60 +278,60 @@
   return is_stopped;
 }
 
 LaneParkingPlanner::LaneParkingPlanner(
   rclcpp::Node & node, std::mutex & lane_parking_mutex,
   const std::optional<LaneParkingRequest> & request, LaneParkingResponse & response,
   std::atomic<bool> & is_lane_parking_cb_running, const rclcpp::Logger & logger,
   const GoalPlannerParameters & parameters)
 : mutex_(lane_parking_mutex),
   request_(request),
   response_(response),
   is_lane_parking_cb_running_(is_lane_parking_cb_running),
   logger_(logger)
 {
   const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
   LaneDepartureChecker lane_departure_checker{};
   lane_departure_checker.setVehicleInfo(vehicle_info);
   lane_departure_checker::Param lane_departure_checker_params;
   lane_departure_checker_params.footprint_extra_margin =
     parameters.lane_departure_check_expansion_margin;
   lane_departure_checker.setParam(lane_departure_checker_params);
 
   for (const std::string & planner_type : parameters.efficient_path_order) {
     if (planner_type == "SHIFT" && parameters.enable_shift_parking) {
       pull_over_planners_.push_back(
         std::make_shared<ShiftPullOver>(node, parameters, lane_departure_checker));
     } else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) {
       pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
         node, parameters, lane_departure_checker, /*is_forward*/ true));
     } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) {
       pull_over_planners_.push_back(std::make_shared<GeometricPullOver>(
         node, parameters, lane_departure_checker, /*is_forward*/ false));
     }
   }
 
   if (pull_over_planners_.empty()) {
     RCLCPP_ERROR(logger_, "Not found enabled planner");
   }
 }
 
 // generate pull over candidate paths
 void LaneParkingPlanner::onTimer()
 {
   const ScopedFlag flag(is_lane_parking_cb_running_);
 
   std::optional<LaneParkingRequest> local_request_opt;
 
   // begin of critical section
   {
     std::lock_guard<std::mutex> guard(mutex_);
     if (request_) {
       auto & request = request_.value();
       local_request_opt.emplace(request);
     }
   }
   // end of critical section
   if (!local_request_opt) {
     RCLCPP_ERROR(logger_, "main thread has not yet set request for LaneParkingPlanner");
     return;
   }
@@ -475,80 +473,80 @@
   }
 }
 
 void FreespaceParkingPlanner::onTimer()
 {
   const ScopedFlag flag(is_freespace_parking_cb_running_);
 
   std::optional<FreespaceParkingRequest> local_request_opt;
 
   // begin of critical section
   {
     std::lock_guard<std::mutex> guard(mutex_);
     if (request_) {
       auto & request = request_.value();
       local_request_opt.emplace(request);
     }
   }
   // end of critical section
   if (!local_request_opt) {
     RCLCPP_ERROR(logger_, "main thread has not yet set request for FreespaceParkingPlanner");
     return;
   }
   const auto & local_request = local_request_opt.value();
   const auto & parameters = local_request.parameters_;
   const auto & local_planner_data = local_request.get_planner_data();
   const auto & current_status = local_request.get_current_status();
   const auto & pull_over_path_opt = local_request.get_pull_over_path();
   const auto & last_path_update_time = local_request.get_last_path_update_time();
   const auto & occupancy_grid_map = local_request.get_occupancy_grid_map();
 
   if (current_status == ModuleStatus::IDLE) {
     return;
   }
 
   if (!local_planner_data->costmap) {
     return;
   }
   // fixed goal planner do not use freespace planner
   if (!utils::isAllowedGoalModification(local_planner_data->route_handler)) {
     return;
   }
 
   const std::optional<GoalCandidate> modified_goal_opt =
     pull_over_path_opt
       ? std::make_optional<GoalCandidate>(pull_over_path_opt.value().modified_goal())
       : std::nullopt;
   if (isOnModifiedGoal(
         local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) {
     return;
   }
 
   const double vehicle_width = local_planner_data->parameters.vehicle_width;
   const bool left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE;
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *(local_planner_data->route_handler), left_side_parking, parameters.backward_goal_search_length,
     parameters.forward_goal_search_length);
   const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon(
     pull_over_lanes, left_side_parking, parameters.detection_bound_offset,
     parameters.margin_from_boundary + parameters.max_lateral_offset + vehicle_width);
 
   PredictedObjects dynamic_target_objects{};
   for (const auto & object : local_planner_data->dynamic_object->objects) {
     const auto object_polygon = universe_utils::toPolygon2d(object);
     if (
       objects_extraction_polygon.has_value() &&
       boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) {
       dynamic_target_objects.objects.push_back(object);
     }
   }
   const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
     dynamic_target_objects, parameters.th_moving_object_velocity);
 
   const bool is_new_costmap =
     (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0;
   constexpr double path_update_duration = 1.0;
   if (
     isStuck(static_target_objects, dynamic_target_objects, local_request) && is_new_costmap &&
     needPathUpdate(
       local_planner_data->self_odometry->pose.pose, path_update_duration, clock_->now(),
       modified_goal_opt, last_path_update_time, parameters)) {
@@ -567,17 +565,15 @@
   // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run().
   // Then module_ptr->run() invokes GoalPlannerModule::updateData and then
   // planWaitingApproval()/plan(), so we can copy latest current_status/previous_module_output to
-  // gp_planner_data_ here
+  // lane_parking_request/freespace_parking_request
 
   std::optional<PullOverPath> pull_over_path =
     context_data_ ? context_data_.value().pull_over_path_opt : std::nullopt;
   std::optional<rclcpp::Time> last_path_update_time =
     context_data_ ? context_data_.value().last_path_update_time : std::nullopt;
 
-  // NOTE: onTimer/onFreespaceParkingTimer copies gp_planner_data_ to their local clone, so we need
-  // to lock gp_planner_data_ here to avoid data race. But the following clone process is
-  // lightweight because most of the member variables of PlannerData/RouteHandler is
-  // shared_ptrs/bool
+  // NOTE: Following clone process is rather lightweight because most of the member variables of
+  // PlannerData/RouteHandler is shared_ptrs
   // begin of critical section
   LaneParkingResponse lane_parking_response;
   {
@@ -586,14 +582,14 @@
       lane_parking_request_.emplace(
         *parameters_, vehicle_footprint_, goal_candidates_, getPreviousModuleOutput());
     }
-    auto & lane_parking_request = lane_parking_request_.value();
     // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that
     // planner_data_ is not nullptr, so it is OK to copy as value
     // By copying PlannerData as value, the internal shared member variables are also copied
-    // (reference count is incremented), so `gp_planner_data_.foo` is now thread-safe from the
-    // **re-pointing** by `planner_data_->foo = msg` in behavior_path_planner::onCallbackFor(msg)
-    // and if these two coincided, only the reference count is affected
-    lane_parking_request.update(
+    // (reference count is incremented), so `lane_parking_request_local.planner_data_.foo` is now
+    // thread-safe from the **re-pointing** by `planner_data_->foo = msg` in
+    // behavior_path_planner::onCallbackFor(msg) and if these two coincided, only the reference
+    // count is affected
+    lane_parking_request_.value().update(
       *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path,
       path_decision_controller_.get_current_state());
     // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as
@@ -603,10 +599,10 @@
     // to copy route_handler as value to use lanelet_map_ptr_/routing_graph_ptr_ thread-safely in
     // onTimer/onFreespaceParkingTimer
     // TODO(Mamoru Sobue): If the copy of RouteHandler.road_lanelets/shoulder_lanelets is not
-    // lightweight, we should update gp_planner_data_.route_handler only when
+    // lightweight, we should update lane_parking_request.planner_data.route_handler only when
     // `planner_data_.is_route_handler_updated` variable is set true by behavior_path_planner
-    // (although this flag is not implemented yet). In that case, gp_planner_data members except
-    // for route_handler should be copied from planner_data_
+    // (although this flag is not implemented yet). In that case, lane_parking_request members
+    // except for route_handler should be copied from planner_data_
     lane_parking_response = lane_parking_response_;
   }
 
@@ -617,19 +613,12 @@
       freespace_parking_request_.emplace(
         *parameters_, vehicle_footprint_, goal_candidates_, *planner_data_);
     }
-    auto & freespace_parking_request = freespace_parking_request_.value();
     constexpr double stuck_time = 5.0;
-    freespace_parking_request.update(
+    freespace_parking_request_.value().update(
       *planner_data_, getCurrentStatus(), pull_over_path, last_path_update_time,
       isStopped(
         odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time,
         parameters_->th_stopped_velocity));
-    // GoalPlannerModule::occupancy_grid_map_ and gp_planner_data.occupancy_grid_map share the
-    // ownership, and gp_planner_data.occupancy_grid_map maybe also shared by the local
-    // planner_data on onFreespaceParkingTimer thread local memory space. So following operation
-    // is thread-safe because gp_planner_data.occupancy_grid_map is only re-pointed here and its
-    // prior resource is still owned by the onFreespaceParkingTimer thread locally.
-    occupancy_grid_map_ = freespace_parking_request.get_occupancy_grid_map();
     freespace_parking_response = freespace_parking_response_;
   }
   // end of critical section
@@ -670,79 +659,80 @@
     goal_candidates_ = generateGoalCandidates();
   }
 
+  occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid));
   auto [lane_parking_response, freespace_parking_response] = syncWithThreads();
 
   if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) {
     return;
   }
 
   resetPathCandidate();
   resetPathReference();
   path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
 
   const bool found_pull_over_path =
     context_data_ ? context_data_.value().pull_over_path_opt.has_value() : false;
   std::optional<PullOverPath> pull_over_path_recv =
     found_pull_over_path
       ? std::make_optional<PullOverPath>(context_data_.value().pull_over_path_opt.value())
       : std::nullopt;
 
   const auto modified_goal_pose = [&]() -> std::optional<GoalCandidate> {
     if (!pull_over_path_recv) {
       return std::nullopt;
     }
     return pull_over_path_recv.value().modified_goal();
   }();
 
   // save "old" state
   const auto prev_decision_state = path_decision_controller_.get_current_state();
   const auto [is_current_safe, collision_check_map] = isSafePath(
     planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state, *parameters_,
     ego_predicted_path_params_, objects_filtering_params_, safety_check_params_);
   debug_data_.collision_check = collision_check_map;
   // update to latest state
   path_decision_controller_.transit_state(
     found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects,
     modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_,
     goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded);
 
   if (context_data_) {
     context_data_.value().update(
       path_decision_controller_.get_current_state().is_stable_safe, static_target_objects,
       dynamic_target_objects, prev_decision_state,
       isStopped(
         odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time,
         parameters_->th_stopped_velocity),
       lane_parking_response, freespace_parking_response);
   } else {
     context_data_.emplace(
       path_decision_controller_.get_current_state().is_stable_safe, static_target_objects,
       dynamic_target_objects, prev_decision_state,
       isStopped(
         odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time,
         parameters_->th_stopped_velocity),
       lane_parking_response, freespace_parking_response);
   }
   auto & ctx_data = context_data_.value();
 
   if (!isActivated()) {
     return;
   }
 
   if (hasFinishedCurrentPath(ctx_data)) {
     if (ctx_data.pull_over_path_opt) {
       auto & pull_over_path = ctx_data.pull_over_path_opt.value();
       if (pull_over_path.incrementPathIndex()) {
         ctx_data.last_path_idx_increment_time = clock_->now();
       }
     }
   }
 
   if (!last_approval_data_) {
     last_approval_data_ =
       std::make_unique<LastApprovalData>(clock_->now(), planner_data_->self_odometry->pose.pose);
     // TODO(soblin): do not "plan" in updateData
     if (ctx_data.pull_over_path_opt) decideVelocity(ctx_data.pull_over_path_opt.value());
   }
 }
 
@@ -896,7 +886,7 @@
 
   return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length);
 }
 
 bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & context_data)
 {
   // return only before starting free space parking
@@ -1438,106 +1428,106 @@
   return output;
 }
 
 BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData & context_data)
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
   std::chrono::system_clock::time_point start;
   start = std::chrono::system_clock::now();
 
   // if pull over path candidates generation is not finished, use previous module output
   if (context_data.lane_parking_response.pull_over_path_candidates.empty()) {
     return getPreviousModuleOutput();
   }
 
   /**
      NOTE(soblin): this path originates from the previously selected(by main thread) pull_over_path
      which was originally generated by either road_parking or freespace thread
    */
   auto pull_over_path_with_velocity_opt = context_data.pull_over_path_opt;
   const bool is_freespace =
     pull_over_path_with_velocity_opt &&
     pull_over_path_with_velocity_opt.value().type() == PullOverPlannerType::FREESPACE;
   const std::optional<GoalCandidate> modified_goal_opt =
     pull_over_path_with_velocity_opt
       ? std::make_optional<GoalCandidate>(pull_over_path_with_velocity_opt.value().modified_goal())
       : std::nullopt;
   const auto & last_path_update_time = context_data.last_path_update_time;
   if (
     path_decision_controller_.get_current_state().state ==
       PathDecisionState::DecisionKind::NOT_DECIDED &&
     !is_freespace &&
     needPathUpdate(
       planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(),
       modified_goal_opt, last_path_update_time, *parameters_)) {
     // if the final path is not decided and enough time has passed since last path update,
     // select safe path from lane parking pull over path candidates
     // and set it to thread_safe_data_
     RCLCPP_DEBUG(getLogger(), "Update pull over path candidates");
 
     context_data.pull_over_path_opt = std::nullopt;
     context_data.last_path_update_time = std::nullopt;
     context_data.last_path_idx_increment_time = std::nullopt;
 
     // update goal candidates
     auto goal_candidates = goal_candidates_;
     goal_searcher_->update(
       goal_candidates, occupancy_grid_map_, planner_data_, context_data.static_target_objects);
 
     // Select a path that is as safe as possible and has a high priority.
     const auto & pull_over_path_candidates =
       context_data.lane_parking_response.pull_over_path_candidates;
     auto lane_pull_over_path_opt =
       selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates);
 
     // update thread_safe_data_
     const auto & pull_over_path_opt =
       lane_pull_over_path_opt ? lane_pull_over_path_opt
                               : context_data.freespace_parking_response.freespace_pull_over_path;
     if (pull_over_path_opt) {
       const auto & pull_over_path = pull_over_path_opt.value();
       /** TODO(soblin): since thread_safe_data::pull_over_path was used as a global variable, old
        * code was setting deceleration to thread_safe_data::pull_over_path and setOutput() accessed
        * to the velocity profile in thread_safe_data::pull_over_path, which is a very bad usage of
        * member variable
        *
        * set this selected pull_over_path to ThreadSafeData, but actually RoadParking thread does
        * not use pull_over_path, but only FreespaceParking thread use this selected pull_over_path.
        * As the next action item, only set this selected pull_over_path to only
        * FreespaceThreadSafeData.
        */
       context_data.pull_over_path_opt = pull_over_path;
       context_data.last_path_update_time = clock_->now();
       context_data.last_path_idx_increment_time = std::nullopt;
 
       if (pull_over_path_with_velocity_opt) {
         auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value();
         // copy the path for later setOutput()
         pull_over_path_with_velocity = pull_over_path;
         // modify the velocity for latest setOutput()
         deceleratePath(pull_over_path_with_velocity);
       }
       RCLCPP_DEBUG(
         getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(),
         pull_over_path.modified_goal().id);
     }
   }
 
   // set output and status
   BehaviorModuleOutput output{};
   setOutput(pull_over_path_with_velocity_opt, context_data, output);
 
   // return to lane parking if it is possible
   if (is_freespace && canReturnToLaneParking(context_data)) {
     if (pull_over_path_with_velocity_opt) {
       context_data.pull_over_path_opt = pull_over_path_with_velocity_opt;
       context_data.last_path_update_time = clock_->now();
       context_data.last_path_idx_increment_time = std::nullopt;
     }
   }
 
   // For debug
   setDebugData(context_data);
 
   if (!pull_over_path_with_velocity_opt) {
     return output;
@@ -1773,46 +1763,46 @@
   return stop_path;
 }
 
 bool FreespaceParkingPlanner::isStuck(
   const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
   const FreespaceParkingRequest & req) const
 {
   const auto & parameters = req.parameters_;
   const auto & planner_data = req.get_planner_data();
   const std::optional<GoalCandidate> modified_goal_opt =
     req.get_pull_over_path()
       ? std::make_optional<GoalCandidate>(req.get_pull_over_path().value().modified_goal())
       : std::nullopt;
   if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) {
     return false;
   }
 
   if (!req.is_stopped()) {
     return false;
   }
 
   if (!req.get_pull_over_path()) {
     return true;
   }
 
   if (parameters.use_object_recognition) {
     const auto & path = req.get_pull_over_path().value().getCurrentPath();
     const auto curvatures = autoware::motion_utils::calcCurvature(path.points);
     std::vector<Polygon2d> ego_polygons_expanded;
     if (goal_planner_utils::checkObjectsCollision(
           path, curvatures, static_target_objects, dynamic_target_objects, planner_data->parameters,
           parameters.object_recognition_collision_check_hard_margins.back(),
           /*extract_static_objects=*/false, parameters.maximum_deceleration,
           parameters.object_recognition_collision_check_max_extra_stopping_margin,
           ego_polygons_expanded)) {
       return true;
     }
   }
 
   if (
     parameters.use_occupancy_grid_for_path_collision_check &&
     checkOccupancyGridCollision(
       req.get_pull_over_path().value().getCurrentPath(), req.get_occupancy_grid_map())) {
     return true;
   }
 
@@ -2588,5 +2578,5 @@
     add(planner_type_marker_array);
   }
 }
 
 }  // namespace autoware::behavior_path_planner

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp

@@ -19,29 +19,20 @@
 namespace autoware::behavior_path_planner
 {
 
 void LaneParkingRequest::update(
   const PlannerData & planner_data, const ModuleStatus & current_status,
   const BehaviorModuleOutput & previous_module_output,
   const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data)
 {
   planner_data_ = std::make_shared<PlannerData>(planner_data);
   planner_data_->route_handler = std::make_shared<RouteHandler>(*(planner_data.route_handler));
   current_status_ = current_status;
   last_previous_module_output_ = previous_module_output_;
   previous_module_output_ = previous_module_output;
   pull_over_path_ = pull_over_path;
   prev_data_ = prev_data;
 }
 
-LaneParkingRequest LaneParkingRequest::clone() const
-{
-  LaneParkingRequest request(
-    parameters_, vehicle_footprint_, goal_candidates_, previous_module_output_);
-  request.update(
-    *planner_data_, current_status_, previous_module_output_, pull_over_path_, prev_data_);
-  return request;
-}
-
 void FreespaceParkingRequest::initializeOccupancyGridMap(
   const PlannerData & planner_data, const GoalPlannerParameters & parameters)
 {
@@ -58,27 +49,18 @@
   occupancy_grid_map_->setParam(occupancy_grid_map_param);
 }
 
 void FreespaceParkingRequest::update(
   const PlannerData & planner_data, const ModuleStatus & current_status,
   const std::optional<PullOverPath> & pull_over_path,
   const std::optional<rclcpp::Time> & last_path_update_time, const bool is_stopped)
 {
   planner_data_ = std::make_shared<PlannerData>(planner_data);
   planner_data_->route_handler = std::make_shared<RouteHandler>(*(planner_data.route_handler));
   current_status_ = current_status;
   occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid));
   pull_over_path_ = pull_over_path;
   last_path_update_time_ = last_path_update_time;
   is_stopped_ = is_stopped;
 }
 
-FreespaceParkingRequest FreespaceParkingRequest::clone() const
-{
-  FreespaceParkingRequest request(
-    parameters_, vehicle_footprint_, goal_candidates_, *planner_data_);
-  request.update(
-    *planner_data_, current_status_, pull_over_path_, last_path_update_time_, is_stopped_);
-  return request;
-}
-
 }  // namespace autoware::behavior_path_planner