From 28f9cbeb5eea43b9cb952455ea59f2029d7f0cb0 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <42mehmetdogru42@gmail.com> Date: Tue, 4 Apr 2023 18:18:55 +0300 Subject: [PATCH] feat(route_handler-bpp): consider lane-ego angle diff in lane following module Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> style(pre-commit): autofix --- .../config/lane_following/lane_following.param.yaml | 4 ++++ .../util/lane_following/module_data.hpp | 4 ++++ .../include/behavior_path_planner/utilities.hpp | 4 +++- .../src/behavior_path_planner_node.cpp | 8 ++++++++ .../scene_module/lane_following/lane_following_module.cpp | 8 +++++--- planning/behavior_path_planner/src/utilities.cpp | 5 +++-- .../route_handler/include/route_handler/route_handler.hpp | 4 +++- planning/route_handler/src/route_handler.cpp | 8 ++++++++ 8 files changed, 38 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_following/lane_following.param.yaml b/planning/behavior_path_planner/config/lane_following/lane_following.param.yaml index b6a9574bb4800..8263622f1d936 100644 --- a/planning/behavior_path_planner/config/lane_following/lane_following.param.yaml +++ b/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] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp index 096660090c60d..0672832b98aea 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp +++ b/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 drivable_area_types_to_skip{}; + + // finding closest lanelet + double distance_threshold; + double yaw_threshold; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 9ba74c5353b1a..284a944934fb7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -366,7 +366,9 @@ lanelet::ConstLanelets getExtendedCurrentLanes( lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr 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::max(), + const double yaw_threshold = std::numeric_limits::max()); Polygon2d convertBoundingBoxObjectToGeometryPolygon( const Pose & current_pose, const double & base_to_front, const double & base_to_rear, diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 40be68fac1f87..26b8fa771e047 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -662,6 +662,14 @@ LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() "lane_following.drivable_area_types_to_skip", std::vector({"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; } 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 a76fc79601cb5..4ce82f0ca0a12 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 @@ -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, ¤t_lane)) { + if (!planner_data_->route_handler->getClosestLaneletWithConstrainsWithinRoute( + current_pose, ¤t_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); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 1bd76ac0162c5..c187c38603cac 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1995,10 +1995,11 @@ lanelet::ConstLanelets getExtendedCurrentLanes( lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr 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, ¤t_lane)) { + if (!route_handler->getClosestLaneletWithConstrainsWithinRoute( + pose, ¤t_lane, dist_threshold, yaw_threshold)) { 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 d72a11aa5ec98..0bac9c75d1b04 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -271,7 +271,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( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index a309b539617a1..5db5daa4a5bbe 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -724,6 +724,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 {