refactor(behavior_velocity_planner): new framework for creating virtual wall markers(#3851)

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -46,5 +46,7 @@
       aborting_time: 5.0                         # [s]
       abort_max_lateral_jerk: 1000.0             # [m/s3]
 
+      finish_judge_lateral_threshold: 0.2        # [m]
+
       # debug
       publish_debug_marker: false

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -59,6 +59,8 @@ struct LaneChangeParameters
   double aborting_time{5.0};
   double abort_max_lateral_jerk{10.0};
 
+  double finish_judge_lateral_threshold{0.2};
+
   // debug marker
   bool publish_debug_marker{false};
 };

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -767,6 +767,9 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.aborting_time = declare_parameter<double>(parameter("aborting_time"));
   p.abort_max_lateral_jerk = declare_parameter<double>(parameter("abort_max_lateral_jerk"));
 
+  p.finish_judge_lateral_threshold =
+    declare_parameter<double>("lane_change.finish_judge_lateral_threshold");
+
   // debug marker
   p.publish_debug_marker = declare_parameter<bool>(parameter("publish_debug_marker"));
 

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -48,15 +48,15 @@ std::shared_ptr<SceneModuleInterface> LaneChangeModuleManager::createNewSceneMod
     std::make_unique<ExternalRequestLaneChange>(parameters_, direction_));
 }
 
-void LaneChangeModuleManager::updateModuleParams(
-  [[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters)
+void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Parameter> & parameters)
 {
   using tier4_autoware_utils::updateParam;
 
-  [[maybe_unused]] auto p = parameters_;
+  auto p = parameters_;
 
-  [[maybe_unused]] const std::string ns = name_ + ".";
-  // updateParam<bool>(parameters, ns + ..., ...);
+  const std::string ns = name_ + ".";
+  updateParam<double>(
+    parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold);
 
   std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) {
     m->updateModuleParams(p);

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -269,10 +269,22 @@ bool NormalLaneChange::hasFinishedLaneChange() const
   const auto & lane_change_end = status_.lane_change_path.shift_line.end;
   const double dist_to_lane_change_end = motion_utils::calcSignedArcLength(
     lane_change_path.points, current_pose.position, lane_change_end.position);
-  if (dist_to_lane_change_end + lane_change_parameters_->lane_change_finish_judge_buffer < 0.0) {
-    return true;
+
+  const auto reach_lane_change_end =
+    dist_to_lane_change_end + lane_change_parameters_->lane_change_finish_judge_buffer < 0.0;
+  if (!reach_lane_change_end) {
+    return false;
   }
-  return false;
+
+  const auto arc_length =
+    lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose);
+  const auto reach_target_lane =
+    std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold;
+  if (!reach_target_lane) {
+    return false;
+  }
+
+  return true;
 }
 
 bool NormalLaneChange::isAbleToReturnCurrentLane() const