From b29e841ed4fa801fe9c168947b1b1e3d62eb8eb1 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 2 Oct 2024 16:43:14 +0900 Subject: [PATCH] Refactor turn signal decider logic and add support for detecting turn signals in turn lanes Signed-off-by: Kyoichi Sugahara --- .../src/turn_signal_decider.cpp | 25 ++++++++++++++++--- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index f808aee2bde2e..16537620a29a6 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -136,6 +136,13 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( const size_t current_seg_idx, const RouteHandler & route_handler, const double nearest_dist_threshold, const double nearest_yaw_threshold) { + const auto requires_turn_signal = [¤t_vel]( + const auto & turn_direction, const bool is_in_turn_lane) { + constexpr double stop_velocity_threshold = 0.1; + return ( + turn_direction == "right" || turn_direction == "left" || + (turn_direction == "straight" && current_vel < stop_velocity_threshold && !is_in_turn_lane)); + }; // base search distance const double base_search_distance = intersection_search_time_ * current_vel + intersection_search_distance_; @@ -152,6 +159,19 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } } + bool is_in_turn_lane = false; + for (const auto & lane_id : unique_lane_ids) { + const auto lanelet = route_handler.getLaneletsFromId(lane_id); + const std::string turn_direction = lanelet.attributeOr("turn_direction", "none"); + if (turn_direction == "left" || turn_direction == "right") { + const auto & position = current_pose.position; + const lanelet::BasicPoint2d point(position.x, position.y); + if (lanelet::geometry::inside(lanelet, point)) { + is_in_turn_lane = true; + break; + } + } + } // combine consecutive lanes of the same turn direction // stores lanes that have already been combine std::set processed_lanes; @@ -258,11 +278,8 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( continue; } - constexpr double stop_velocity_threshold = 0.1; if (dist_to_front_point < search_distance) { - if ( - lane_attribute == "right" || lane_attribute == "left" || - (lane_attribute == "straight" && current_vel < stop_velocity_threshold)) { + if (requires_turn_signal(lane_attribute, is_in_turn_lane)) { // update map if necessary if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { desired_start_point_map_.emplace(lane_id, current_pose);