fix(behavior_path_planner): additional buffer during lane change for …

…intersection (tier4#532)

* Fix: Add additional buffer in intersection

This is so that if lane change is performed on restricted areas, the final lane
change point will be before the restricted areas.

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* ci(pre-commit): autofix

* Chore: Change variable names

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix: change lanelet attribute check and intersection check

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* chore: fix spelling mistake;

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -315,7 +315,7 @@ void shiftPose(Pose * pose, double shift_length);
 PathWithLaneId getCenterLinePath(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelet_sequence,
   const Pose & pose, const double backward_path_length, const double forward_path_length,
-  const BehaviorPathPlannerParameters & parameter);
+  const BehaviorPathPlannerParameters & parameter, double optional_length = 0.0);
 
 PathWithLaneId setDecelerationVelocity(
   const RouteHandler & route_handler, const PathWithLaneId & input,
@@ -328,6 +328,10 @@ PathWithLaneId setDecelerationVelocity(
   const double pullover_distance_min, const double distance_before_pull_over,
   const double deceleration_interval, Pose goal_pose);
 
+bool checkLaneIsInIntersection(
+  const RouteHandler & route_handler, const PathWithLaneId & ref,
+  const lanelet::ConstLanelets & lanelet_sequence, double & additional_length_to_add);
+
 // object label
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 

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

@@ -237,19 +237,31 @@ PathWithLaneId LaneChangeModule::getReferencePath() const
     return reference_path;
   }
 
+  if (reference_path.points.empty()) {
+    reference_path = util::getCenterLinePath(
+      *route_handler, current_lanes, current_pose, common_parameters.backward_path_length,
+      common_parameters.forward_path_length, common_parameters);
+  }
+
+  double optional_lengths{0.0};
+  const auto isInIntersection = util::checkLaneIsInIntersection(
+    *route_handler, reference_path, current_lanes, optional_lengths);
+  if (isInIntersection) {
+    reference_path = util::getCenterLinePath(
+      *route_handler, current_lanes, current_pose, common_parameters.backward_path_length,
+      common_parameters.forward_path_length, common_parameters, optional_lengths);
+  }
   const double buffer =
     common_parameters.backward_length_buffer_for_end_of_lane;  // buffer for min_lane_change_length
   const int num_lane_change =
     std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back()));
   const double lane_change_buffer =
     num_lane_change * (common_parameters.minimum_lane_change_length + buffer);
 
-  reference_path = util::getCenterLinePath(
-    *route_handler, current_lanes, current_pose, common_parameters.backward_path_length,
-    common_parameters.forward_path_length, common_parameters);
   reference_path = util::setDecelerationVelocity(
     *route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration,
     lane_change_buffer);
+
   reference_path.drivable_area = util::generateDrivableArea(
     current_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length,
     planner_data_);

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -97,11 +97,22 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
     *route_handler, current_lanes, current_pose, p.backward_path_length, p.forward_path_length, p);
 
   {
+    double optional_lengths{0.0};
+    const auto isInIntersection = util::checkLaneIsInIntersection(
+      *route_handler, reference_path, current_lanes, optional_lengths);
+
+    if (isInIntersection) {
+      reference_path = util::getCenterLinePath(
+        *route_handler, current_lanes, current_pose, p.backward_path_length, p.forward_path_length,
+        p, optional_lengths);
+    }
+
     // buffer for min_lane_change_length
-    const double buffer = p.backward_length_buffer_for_end_of_lane;
+    const double buffer = p.backward_length_buffer_for_end_of_lane + optional_lengths;
     const int num_lane_change =
       std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back()));
     const double lane_change_buffer = num_lane_change * (p.minimum_lane_change_length + buffer);
+
     reference_path = util::setDecelerationVelocity(
       *route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration,
       lane_change_buffer);

planning/behavior_path_planner/src/utilities.cpp

@@ -1468,10 +1468,10 @@ double getDistanceToShoulderBoundary(
 }
 
 double getArcLengthToTargetLanelet(
-  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelet & target_lane,
+  const lanelet::ConstLanelets & lanelet_sequence, const lanelet::ConstLanelet & target_lane,
   const Pose & pose)
 {
-  const auto arc_pose = lanelet::utils::getArcCoordinates(current_lanes, pose);
+  const auto arc_pose = lanelet::utils::getArcCoordinates(lanelet_sequence, pose);
 
   const auto target_center_line = target_lane.centerline().basicLineString();
 
@@ -1495,8 +1495,8 @@ double getArcLengthToTargetLanelet(
     back_pose.orientation = tf2::toMsg(tf_quat);
   }
 
-  const auto arc_front = lanelet::utils::getArcCoordinates(current_lanes, front_pose);
-  const auto arc_back = lanelet::utils::getArcCoordinates(current_lanes, back_pose);
+  const auto arc_front = lanelet::utils::getArcCoordinates(lanelet_sequence, front_pose);
+  const auto arc_back = lanelet::utils::getArcCoordinates(lanelet_sequence, back_pose);
 
   return std::max(
     std::min(arc_front.length - arc_pose.length, arc_back.length - arc_pose.length), 0.0);
@@ -1599,17 +1599,17 @@ std::shared_ptr<PathWithLaneId> generateCenterLinePath(
     return {};  // TODO(Horibe) What should be returned?
   }
 
-  // For current_lanes with desired length
-  lanelet::ConstLanelets current_lanes = route_handler->getLaneletSequence(
+  // For lanelet_sequence with desired length
+  lanelet::ConstLanelets lanelet_sequence = route_handler->getLaneletSequence(
     current_lane, pose->pose, p.backward_path_length, p.forward_path_length);
 
   *centerline_path = getCenterLinePath(
-    *route_handler, current_lanes, pose->pose, p.backward_path_length, p.forward_path_length, p);
+    *route_handler, lanelet_sequence, pose->pose, p.backward_path_length, p.forward_path_length, p);
 
   centerline_path->header = route_handler->getRouteHeader();
 
   centerline_path->drivable_area = util::generateDrivableArea(
-    current_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data);
+    lanelet_sequence, p.drivable_area_resolution, p.vehicle_length, planner_data);
 
   return centerline_path;
 }
@@ -1671,7 +1671,7 @@ void shiftPose(Pose * pose, double shift_length)
 PathWithLaneId getCenterLinePath(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelet_sequence,
   const Pose & pose, const double backward_path_length, const double forward_path_length,
-  const BehaviorPathPlannerParameters & parameter)
+  const BehaviorPathPlannerParameters & parameter, double optional_length)
 {
   PathWithLaneId reference_path;
 
@@ -1684,8 +1684,8 @@ PathWithLaneId getCenterLinePath(
   const double s_backward = std::max(0., s - backward_path_length);
   double s_forward = s + forward_path_length;
 
-  const double buffer =
-    parameter.backward_length_buffer_for_end_of_lane;  // buffer for min_lane_change_length
+  const double buffer = parameter.backward_length_buffer_for_end_of_lane +
+                        optional_length;  // buffer for min_lane_change_length
   const int num_lane_change =
     std::abs(route_handler.getNumLaneToPreferredLane(lanelet_sequence.back()));
   const double lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence);
@@ -1705,6 +1705,80 @@ PathWithLaneId getCenterLinePath(
   return route_handler.getCenterLinePath(lanelet_sequence, s_backward, s_forward, true);
 }
 
+bool checkLaneIsInIntersection(
+  const RouteHandler & route_handler, const PathWithLaneId & reference_path,
+  const lanelet::ConstLanelets & lanelet_sequence, double & additional_length_to_add)
+{
+  if (lanelet_sequence.empty()) {
+    return false;
+  }
+
+  const auto & path_points = reference_path.points;
+  auto end_of_route_pose = path_points.back().point.pose;
+
+  lanelet::ConstLanelet check_lane;
+  if (!lanelet::utils::query::getClosestLanelet(lanelet_sequence, end_of_route_pose, &check_lane)) {
+    return false;
+  }
+
+  lanelet::ConstLanelets lane_change_prohibited_lanes{check_lane};
+
+  const auto checking_rev_iter = std::find_if(
+    lanelet_sequence.crbegin(), lanelet_sequence.crend(),
+    [&check_lane](const lanelet::ConstLanelet & lanelet) noexcept {
+      return lanelet.id() == check_lane.id();
+    });
+  if (checking_rev_iter != lanelet_sequence.crend()) {
+    const auto prev_lane = std::next(checking_rev_iter);
+
+    if (prev_lane != lanelet_sequence.crend()) {
+      const auto lanes = route_handler.getNextLanelets(*prev_lane);
+      const auto isHaveNeighborWithTurnDirection =
+        [&](const lanelet::ConstLanelets & lanes) noexcept {
+          return std::any_of(lanes.cbegin(), lanes.cend(), [](const lanelet::ConstLanelet & lane) {
+            return lane.hasAttribute("turn_direction");
+          });
+        };
+
+      if (isHaveNeighborWithTurnDirection(lanes)) {
+        // lambdas
+        const auto checkAttribute = [](const lanelet::ConstLineString3d & linestring) noexcept {
+          const auto & attribute_name = lanelet::AttributeNamesString::LaneChange;
+          if (linestring.hasAttribute(attribute_name)) {
+            const auto attr = linestring.attribute(attribute_name);
+            if (attr.value() == std::string("yes")) {
+              return true;
+            }
+          }
+          return false;
+        };
+        const auto isLaneChangeAttributeYes =
+          [checkAttribute](const lanelet::ConstLanelet & lanelet) noexcept {
+            return (checkAttribute(lanelet.rightBound()) || checkAttribute(lanelet.leftBound()));
+          };
+
+        for (auto prev_ll_itr = prev_lane; prev_ll_itr != lanelet_sequence.crend(); ++prev_ll_itr) {
+          if (!isLaneChangeAttributeYes(*prev_ll_itr)) {
+            lane_change_prohibited_lanes.push_back(*prev_ll_itr);
+          } else {
+            break;
+          }
+        }
+      }
+    }
+  }
+  std::reverse(lane_change_prohibited_lanes.begin(), lane_change_prohibited_lanes.end());
+  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"
+
+  return true;
+}
+
 PathWithLaneId setDecelerationVelocity(
   const RouteHandler & route_handler, const PathWithLaneId & input,
   const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration,