From 9d9741699ad1a784cf2dc8b9d02c23bb04129258 Mon Sep 17 00:00:00 2001
From: kosuke55 <kosuke.tnp@gmail.com>
Date: Fri, 9 Sep 2022 02:02:55 +0900
Subject: [PATCH] generate drivable area at last in pull_out

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../scene_module/pull_out/pull_out_module.hpp |  1 +
 .../scene_module/pull_out/pull_out_module.cpp | 27 ++++++++++++-------
 2 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
index fd94ef35cd4ef..cfbb63c02e073 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
@@ -52,6 +52,7 @@ struct PullOutStatus
   PathWithLaneId backward_path;
   lanelet::ConstLanelets current_lanes;
   lanelet::ConstLanelets pull_out_lanes;
+  lanelet::ConstLanelets lanes;
   std::vector<uint64_t> lane_follow_lane_ids;
   std::vector<uint64_t> pull_out_lane_ids;
   bool is_safe = false;
diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
index 31942a0ab61e7..d7f2df9dea01f 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
@@ -168,6 +168,9 @@ BehaviorModuleOutput PullOutModule::plan()
   } else {
     path = status_.backward_path;
   }
+  path.drivable_area = util::generateDrivableArea(
+    path, status_.lanes, planner_data_->parameters.drivable_area_resolution,
+    planner_data_->parameters.vehicle_length, planner_data_);
 
   output.path = std::make_shared<PathWithLaneId>(path);
   output.turn_signal_info =
@@ -235,9 +238,14 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()
 {
   BehaviorModuleOutput output;
   const auto current_lanes = getCurrentLanes();
-  const auto shoulder_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
+  const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
+  auto lanes = current_lanes;
+  lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
 
-  const auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
+  auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
+  candidate_path.drivable_area = util::generateDrivableArea(
+    candidate_path, lanes, planner_data_->parameters.drivable_area_resolution,
+    planner_data_->parameters.vehicle_length, planner_data_);
   auto stop_path = candidate_path;
   for (auto & p : stop_path.points) {
     p.point.longitudinal_velocity_mps = 0.0;
@@ -354,7 +362,6 @@ void PullOutModule::planWithPriorityOnShortBackDistance(
 void PullOutModule::updatePullOutStatus()
 {
   const auto & route_handler = planner_data_->route_handler;
-  const auto & common_parameters = planner_data_->parameters;
   const auto & current_pose = planner_data_->self_pose->pose;
   const auto & goal_pose = planner_data_->route_handler->getGoalPose();
 
@@ -365,6 +372,10 @@ void PullOutModule::updatePullOutStatus()
   const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
   status_.pull_out_lanes = pull_out_lanes;
 
+  // combine road and shoulder lanes
+  status_.lanes = current_lanes;
+  status_.lanes.insert(status_.lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
+
   // search pull out start candidates backward
   std::vector<Pose> start_pose_candidates;
   {
@@ -399,9 +410,6 @@ void PullOutModule::updatePullOutStatus()
     status_.backward_path = pull_out_utils::getBackwardPath(
       *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose,
       parameters_.backward_velocity);
-    status_.backward_path.drivable_area = util::generateDrivableArea(
-      status_.backward_path, pull_out_lanes, common_parameters.drivable_area_resolution,
-      common_parameters.vehicle_length, planner_data_);
   }
 
   // Update status
@@ -432,14 +440,13 @@ lanelet::ConstLanelets PullOutModule::getCurrentLanes() const
 std::vector<Pose> PullOutModule::searchBackedPoses()
 {
   const auto current_pose = planner_data_->self_pose->pose;
-  const auto current_lanes = getCurrentLanes();
-  const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
 
   // get backward shoulder path
-  const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose);
+  const auto arc_position_pose =
+    lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose);
   const double check_distance = parameters_.max_back_distance + 30.0;  // buffer
   auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath(
-    pull_out_lanes, arc_position_pose.length - check_distance,
+    status_.pull_out_lanes, arc_position_pose.length - check_distance,
     arc_position_pose.length + check_distance);
 
   // lateral shift to current_pose