feat(autoware_behavior_path_planner_common): disable feature of turni…

…ng off blinker at low velocity (#1571)

Refactor turn signal decider logic and add support for detecting turn signals in turn lanes

Signed-off-by: Kyoichi Sugahara <[email protected]>

planning/behavior_path_planner_common/src/turn_signal_decider.cpp

@@ -136,6 +136,13 @@ std::optional<TurnSignalInfo> 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 = [&current_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<TurnSignalInfo> 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<int> processed_lanes;
@@ -258,11 +278,8 @@ std::optional<TurnSignalInfo> 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);