feat(route_handler-bpp): consider lane-ego angle diff in lane followi…

…ng module

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

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

@@ -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/utilities.hpp

@@ -454,7 +454,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());
 
 Polygon2d convertBoundingBoxObjectToGeometryPolygon(
   const Pose & current_pose, const double & base_to_front, const double & base_to_rear,

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

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

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

@@ -110,7 +110,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)
@@ -127,8 +128,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/utilities.cpp

@@ -2148,10 +2148,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

@@ -262,6 +262,9 @@ class RouteHandler
   int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const;
   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

@@ -696,6 +696,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
 {