feat(route_handler-bpp): consider lane-ego angle diff in lane following module

planning/behavior_path_planner/config/lane_following/lane_following.param.yaml

@@ -2,3 +2,7 @@
   ros__parameters:
     lane_following:
       lane_change_prepare_duration: 2.0
+
+      closest_lanelet:
+        distance_threshold: 5.0 # [m]
+        yaw_threshold: 0.79 # [rad]

planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2021-2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -28,6 +28,10 @@ struct LaneFollowingParameters
   double drivable_area_right_bound_offset;
   double drivable_area_left_bound_offset;
   std::vector<std::string> drivable_area_types_to_skip{};
+
+  // finding closest lanelet
+  double distance_threshold;
+  double yaw_threshold;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/util/utils.hpp

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -328,7 +328,9 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
 
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<RouteHandler> route_handler, const geometry_msgs::msg::Pose & pose,
-  const double forward_length, const double backward_length);
+  const double forward_length, const double backward_length,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max());
 
 std::vector<PredictedPath> getPredictedPathFromObj(
   const PredictedObject & obj, const bool & is_use_all_predicted_path);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -725,6 +725,14 @@ LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam()
     declare_parameter<std::vector<std::string>>("lane_following.drivable_area_types_to_skip");
   p.lane_change_prepare_duration =
     declare_parameter<double>("lane_following.lane_change_prepare_duration");
+
+  // finding closest lanelet
+  {
+    p.distance_threshold =
+      declare_parameter<double>("lane_following.closest_lanelet.distance_threshold");
+    p.yaw_threshold = declare_parameter<double>("lane_following.closest_lanelet.yaw_threshold");
+  }
+
   return p;
 }
 

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

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -118,7 +118,8 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
   reference_path.header = route_handler->getRouteHeader();
 
   lanelet::ConstLanelet current_lane;
-  if (!planner_data_->route_handler->getClosestLaneletWithinRoute(current_pose, &current_lane)) {
+  if (!planner_data_->route_handler->getClosestLaneletWithConstrainsWithinRoute(
+        current_pose, &current_lane, parameters_->distance_threshold, parameters_->yaw_threshold)) {
     RCLCPP_ERROR_THROTTLE(
       getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
     return reference_path;  // TODO(Horibe)
@@ -135,8 +136,9 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
   // calculate path with backward margin to avoid end points' instability by spline interpolation
   constexpr double extra_margin = 10.0;
   const double backward_length = p.backward_path_length + extra_margin;
-  const auto current_lanes_with_backward_margin =
-    util::calcLaneAroundPose(route_handler, current_pose, p.forward_path_length, backward_length);
+  const auto current_lanes_with_backward_margin = util::calcLaneAroundPose(
+    route_handler, current_pose, p.forward_path_length, backward_length,
+    parameters_->distance_threshold, parameters_->yaw_threshold);
   reference_path = util::getCenterLinePath(
     *route_handler, current_lanes_with_backward_margin, current_pose, backward_length,
     p.forward_path_length, p);

planning/behavior_path_planner/src/util/utils.cpp

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -1849,10 +1849,11 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
 
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<RouteHandler> route_handler, const Pose & pose, const double forward_length,
-  const double backward_length)
+  const double backward_length, const double dist_threshold, const double yaw_threshold)
 {
   lanelet::ConstLanelet current_lane;
-  if (!route_handler->getClosestLaneletWithinRoute(pose, &current_lane)) {
+  if (!route_handler->getClosestLaneletWithConstrainsWithinRoute(
+        pose, &current_lane, dist_threshold, yaw_threshold)) {
     RCLCPP_ERROR(
       rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
       "failed to find closest lanelet within route!!!");

planning/route_handler/include/route_handler/route_handler.hpp

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -272,7 +272,9 @@ class RouteHandler
 
   bool getClosestLaneletWithinRoute(
     const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const;
-
+  bool getClosestLaneletWithConstrainsWithinRoute(
+    const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
+    const double yaw_threshold) const;
   lanelet::ConstLanelet getLaneletsFromId(const lanelet::Id id) const;
   lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids & ids) const;
   lanelet::ConstLanelets getLaneletSequence(

planning/route_handler/src/route_handler.cpp

@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -735,6 +735,14 @@ bool RouteHandler::getClosestLaneletWithinRoute(
   return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet);
 }
 
+bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute(
+  const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold,
+  const double yaw_threshold) const
+{
+  return lanelet::utils::query::getClosestLaneletWithConstrains(
+    route_lanelets_, search_pose, closest_lanelet, dist_threshold, yaw_threshold);
+}
+
 bool RouteHandler::getNextLaneletWithinRoute(
   const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const
 {