From 734f25bb534db67eaca930fc767d32a65d771a65 Mon Sep 17 00:00:00 2001
From: Fumiya Watanabe <rej55.g@gmail.com>
Date: Thu, 31 Aug 2023 14:45:43 +0900
Subject: [PATCH] feat(behavior_path_planner): add virtual stop wall for lane
 change (#4808)

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
---
 .../scene_module/lane_change/base_class.hpp               | 5 +++++
 .../scene_module/lane_change/normal.hpp                   | 2 ++
 .../src/scene_module/lane_change/interface.cpp            | 6 ++++++
 .../src/scene_module/lane_change/normal.cpp               | 8 ++++++++
 4 files changed, 21 insertions(+)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
index f30bf31d4e010..1a7350d82aa4e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -209,6 +209,10 @@ class LaneChangeBase
     return direction_;
   }
 
+  boost::optional<Pose> getStopPose() const { return lane_change_stop_pose_; }
+
+  void resetStopPose() { lane_change_stop_pose_ = boost::none; }
+
 protected:
   virtual lanelet::ConstLanelets getCurrentLanes() const = 0;
 
@@ -244,6 +248,7 @@ class LaneChangeBase
   PathWithLaneId prev_module_path_{};
   DrivableAreaInfo prev_drivable_area_info_{};
   TurnSignalInfo prev_turn_signal_info_{};
+  boost::optional<Pose> lane_change_stop_pose_{boost::none};
 
   PathWithLaneId prev_approved_path_{};
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
index 664f547edb92d..747c75babb573 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
@@ -153,6 +153,8 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;
 
+  void setStopPose(const Pose & stop_pose);
+
   rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
 };
 }  // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
index 0ceab21573316..5e47116117103 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -203,6 +203,7 @@ void LaneChangeInterface::updateData()
   module_type_->setPreviousModulePaths(
     getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
   module_type_->updateSpecialData();
+  module_type_->resetStopPose();
 }
 
 BehaviorModuleOutput LaneChangeInterface::plan()
@@ -221,6 +222,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   *prev_approved_path_ = *getPreviousModuleOutput().path;
   module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path);
 
+  stop_pose_ = module_type_->getStopPose();
+
   updateSteeringFactorPtr(output);
   clearWaitingApproval();
 
@@ -249,6 +252,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 
   path_reference_ = getPreviousModuleOutput().reference_path;
 
+  stop_pose_ = module_type_->getStopPose();
+
   if (!module_type_->isValidPath()) {
     removeRTCStatus();
     path_candidate_ = std::make_shared<PathWithLaneId>();
@@ -286,6 +291,7 @@ void LaneChangeInterface::updateModuleParams(const std::any & parameters)
 
 void LaneChangeInterface::setData(const std::shared_ptr<const PlannerData> & data)
 {
+  planner_data_ = data;
   module_type_->setData(data);
 }
 
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
index f2c068f07d2fa..dea6073654ae2 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -138,6 +138,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
     const auto stop_dist =
       -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
     const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path);
+    setStopPose(stop_point.point.pose);
   }
 
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
@@ -198,6 +199,7 @@ void NormalLaneChange::insertStopPoint(
   const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
   if (stopping_distance > 0.0) {
     const auto stop_point = utils::insertStopPoint(stopping_distance, path);
+    setStopPose(stop_point.point.pose);
   }
 }
 
@@ -1455,4 +1457,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
 
   return path_safety_status;
 }
+
+void NormalLaneChange::setStopPose(const Pose & stop_pose)
+{
+  lane_change_stop_pose_ = stop_pose;
+}
+
 }  // namespace behavior_path_planner