From 7419866ed62544cefabe1bd02e6636b6c9ac04eb Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <42mehmetdogru42@gmail.com> Date: Mon, 13 Mar 2023 18:14:48 +0300 Subject: [PATCH] feat(route_handler-bpp): consider lane-ego angle diff in lane following module Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- .../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 | 3 +++ planning/route_handler/src/route_handler.cpp | 8 ++++++++ 8 files changed, 38 insertions(+), 6 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..e57df5885f450 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] \ No newline at end of file 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 c0598ed7f04d8..ca4aeae01bc10 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -454,7 +454,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 5ff95f26fdcad..53081cbe88004 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/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({"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 4cdca4dd09e53..2c125d55cd3b1 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 @@ -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, ¤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) @@ -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); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 2547a0e1cd681..325b8635ea756 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2148,10 +2148,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 52f8082f125e4..acd7f27343af7 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/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( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index d2ea4c65adaa9..fae0638f7b434 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 {