From 9ac2af72899e31c8798452269b26ae0c3c8fff21 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <42mehmetdogru42@gmail.com> Date: Mon, 10 Apr 2023 16:04:47 +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 update copyright years Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> remove default values for declare_parameter Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- .../config/lane_following/lane_following.param.yaml | 4 ++++ .../util/lane_following/module_data.hpp | 6 +++++- .../include/behavior_path_planner/util/utils.hpp | 6 ++++-- .../src/behavior_path_planner_node.cpp | 8 ++++++++ .../lane_following/lane_following_module.cpp | 10 ++++++---- planning/behavior_path_planner/src/util/utils.cpp | 7 ++++--- .../include/route_handler/route_handler.hpp | 6 ++++-- planning/route_handler/src/route_handler.cpp | 10 +++++++++- 8 files changed, 44 insertions(+), 13 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..ed28007d7b913 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 @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2021-2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -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/util/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/utils.hpp index 48db5bb9b3448..6c06676bbc5e9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021-2023 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -328,7 +328,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()); std::vector getPredictedPathFromObj( const PredictedObject & obj, const bool & is_use_all_predicted_path); 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 804498a99915e..2fd323c07320e 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -725,6 +725,14 @@ LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() declare_parameter>("lane_following.drivable_area_types_to_skip"); p.lane_change_prepare_duration = declare_parameter("lane_following.lane_change_prepare_duration"); + + // finding closest lanelet + { + p.distance_threshold = + declare_parameter("lane_following.closest_lanelet.distance_threshold"); + p.yaw_threshold = declare_parameter("lane_following.closest_lanelet.yaw_threshold"); + } + 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 b5744fa72707b..b7134a3a3f8c5 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 @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021-2023 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -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/util/utils.cpp b/planning/behavior_path_planner/src/util/utils.cpp index a1089a0d14543..70b885b50261a 100644 --- a/planning/behavior_path_planner/src/util/utils.cpp +++ b/planning/behavior_path_planner/src/util/utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021-2023 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -1849,10 +1849,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 4746d2e75b053..7e690e2443215 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021-2023 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -272,7 +272,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 0218165997e1a..ef56e47999bd5 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2021-2023 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -735,6 +735,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 {