fix(behavior_path_planner): lane change intersection stop (tier4#1480)

* fix(behavior_path_planner): lane change intersection stop fixing lane change stopping before lane change prohibited area. currently the stop function is not working so properly. Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix: change order of parameter move output to the last parameter Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
boyali · Oct 3, 2022 · 2231681 · 2231681
1 parent c01d6b6
commit 2231681
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 8 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
@@ -277,7 +277,9 @@ PathWithLaneId setDecelerationVelocity(
 
 bool checkLaneIsInIntersection(
   const RouteHandler & route_handler, const PathWithLaneId & ref,
-  const lanelet::ConstLanelets & lanelet_sequence, double & additional_length_to_add);
+  const lanelet::ConstLanelets & lanelet_sequence, const BehaviorPathPlannerParameters & parameters,
+  double & additional_length_to_add);
+
 PathWithLaneId setDecelerationVelocity(
   const PathWithLaneId & input, const double target_velocity, const Pose target_pose,
   const double buffer, const double deceleration_interval);

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
@@ -271,7 +271,7 @@ PathWithLaneId LaneChangeModule::getReferencePath() const
 
   double optional_lengths{0.0};
   const auto isInIntersection = util::checkLaneIsInIntersection(
-    *route_handler, reference_path, current_lanes, optional_lengths);
+    *route_handler, reference_path, current_lanes, common_parameters, optional_lengths);
   if (isInIntersection) {
     reference_path = util::getCenterLinePath(
       *route_handler, current_lanes, current_pose, common_parameters.backward_path_length,

diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp
@@ -103,7 +103,7 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
   {
     double optional_lengths{0.0};
     const auto isInIntersection = util::checkLaneIsInIntersection(
-      *route_handler, reference_path, current_lanes, optional_lengths);
+      *route_handler, reference_path, current_lanes, p, optional_lengths);
 
     if (isInIntersection) {
       reference_path = util::getCenterLinePath(

diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
@@ -1684,7 +1684,8 @@ PathWithLaneId getCenterLinePath(
 
 bool checkLaneIsInIntersection(
   const RouteHandler & route_handler, const PathWithLaneId & reference_path,
-  const lanelet::ConstLanelets & lanelet_sequence, double & additional_length_to_add)
+  const lanelet::ConstLanelets & lanelet_sequence, const BehaviorPathPlannerParameters & parameter,
+  double & additional_length_to_add)
 {
   if (lanelet_sequence.size() < 2 || reference_path.points.empty()) {
     return false;
@@ -1752,10 +1753,9 @@ bool checkLaneIsInIntersection(
   const auto prohibited_arc_coordinate =
     lanelet::utils::getArcCoordinates(lane_change_prohibited_lanes, end_of_route_pose);
 
-  constexpr double small_earlier_stopping_buffer = 0.2;
-  additional_length_to_add =
-    prohibited_arc_coordinate.length +
-    small_earlier_stopping_buffer;  // additional a slight "buffer so that vehicle stop earlier"
+  additional_length_to_add = prohibited_arc_coordinate.length +
+                             parameter.minimum_lane_change_length +
+                             parameter.backward_length_buffer_for_end_of_lane;
 
   return true;
 }