From 14b183f780148f03658f009739f03f84bea1d3af Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 16 May 2023 21:27:43 +0900 Subject: [PATCH] fix(behavior_path_planner): add isInLaneletWithYawThreshold() (#3726) fix(behavior_path_planner): add isInLaneletWithYawThreshold Signed-off-by: Fumiya Watanabe --- .../behavior_path_planner/utils/utils.hpp | 4 ++++ .../behavior_path_planner/src/utils/utils.cpp | 17 ++++++++++++++--- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 323a6fcbe4646..b8e2024e6bdd3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -301,6 +301,10 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr & modified_goal, const std::shared_ptr & route_handler); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 4babccc812dbe..5ce87ef21e30c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1035,6 +1035,19 @@ bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes) return false; } +bool isInLaneletWithYawThreshold( + const Pose & current_pose, const lanelet::ConstLanelet & lanelet, const double yaw_threshold, + const double radius) +{ + const double pose_yaw = tf2::getYaw(current_pose.orientation); + const double lanelet_angle = lanelet::utils::getLaneletAngle(lanelet, current_pose.position); + const double angle_diff = + std::abs(tier4_autoware_utils::normalizeRadian(lanelet_angle - pose_yaw)); + + return (angle_diff < std::abs(yaw_threshold)) && + lanelet::utils::isInLanelet(current_pose, lanelet, radius); +} + bool isEgoOutOfRoute( const Pose & self_pose, const std::optional & modified_goal, const std::shared_ptr & route_handler) @@ -1058,10 +1071,8 @@ bool isEgoOutOfRoute( } // If ego vehicle is over goal on goal lane, return true - lanelet::ConstLanelet closest_lane; const double yaw_threshold = tier4_autoware_utils::deg2rad(90); - if (lanelet::utils::query::getClosestLaneletWithConstrains( - {goal_lane}, self_pose, &closest_lane, 0.0, yaw_threshold)) { + if (isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) { constexpr double buffer = 1.0; const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose); const auto goal_arc_coord =