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

…ng module Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>
autowarefoundation · Jan 24, 2023 · bdc80e6 · bdc80e6
1 parent 403713d
commit bdc80e6
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 5 deletions.
diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml
@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
-    lanelet2_map_projector_type: MGRS   # Options: MGRS, UTM
-    latitude: 40.816617984672746        # Latitude of map_origin, using in UTM
-    longitude: 29.360491808334285       # Longitude of map_origin, using in UTM
+    lanelet2_map_projector_type: UTM   # Options: MGRS, UTM
+    latitude: 40.81187906        # Latitude of map_origin, using in UTM
+    longitude: 29.35810110          # Longitude of map_origin, using in UTM
 
     center_line_resolution: 5.0         # [m]
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
@@ -109,12 +109,22 @@ 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, 5.0, 1.57)) {
     RCLCPP_ERROR_THROTTLE(
       getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
     return reference_path;  // TODO(Horibe)
   }
 
+  std::cout << "current_lane.id: " << current_lane.id() << std::endl;
+
+  //  if (!planner_data_->route_handler->getClosestLaneletWithinRoute(current_pose, &current_lane))
+  //  {
+  //    RCLCPP_ERROR_THROTTLE(
+  //      getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!");
+  //    return reference_path;  // TODO(Horibe)
+  //  }
+
   // For current_lanes with desired length
   const auto current_lanes = planner_data_->route_handler->getLaneletSequence(
     current_lane, current_pose, p.backward_path_length, p.forward_path_length);

diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
@@ -2052,7 +2052,7 @@ lanelet::ConstLanelets calcLaneAroundPose(
   const double backward_length)
 {
   lanelet::ConstLanelet current_lane;
-  if (!route_handler->getClosestLaneletWithinRoute(pose, &current_lane)) {
+  if (!route_handler->getClosestLaneletWithConstrainsWithinRoute(pose, &current_lane, 5.0, 1.57)) {
     RCLCPP_ERROR(
       rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
       "failed to find closest lanelet within route!!!");

diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -240,6 +240,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(

diff --git a/planning/route_handler/src/route_handler.cpp b/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
 {