From bdc80e6ee7a7bb66e76e54446bea95d711bda3f3 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <42mehmetdogru42@gmail.com> Date: Thu, 22 Dec 2022 09:18:30 +0300 Subject: [PATCH] feat(route_handler-bpp): consider lane-ego angle diff in lane following module Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- map/map_loader/config/lanelet2_map_loader.param.yaml | 6 +++--- .../lane_following/lane_following_module.cpp | 12 +++++++++++- planning/behavior_path_planner/src/utilities.cpp | 2 +- .../include/route_handler/route_handler.hpp | 3 +++ planning/route_handler/src/route_handler.cpp | 8 ++++++++ 5 files changed, 26 insertions(+), 5 deletions(-) diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 55f810b0e9ff9..b074db82a9b6c 100755 --- 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 index 46e96690ba5ee..a2c4bce59ca0e 100644 --- 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, ¤t_lane)) { + if (!planner_data_->route_handler->getClosestLaneletWithConstrainsWithinRoute( + current_pose, ¤t_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, ¤t_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 index 156bca488cc58..4bdbfeba649a2 100644 --- 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, ¤t_lane)) { + if (!route_handler->getClosestLaneletWithConstrainsWithinRoute(pose, ¤t_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 index 1a340c3d031c0..4e25585c672ad 100644 --- 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 index b1fc3c33a572b..60efb03d9cb52 100644 --- 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 {