feat(autoware_behavior_path_planner_common): disable feature of turni…

…ng off blinker at low velocity (autowarefoundation#9005)

* feat(turn_signal_decider): disable feature of turning off blinker at low velocity

Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Signed-off-by: prakash-kannaiah <prakashkanan.pk@gmail.com>

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp

@@ -138,11 +138,12 @@ 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 = [&](const auto & lane_attribute) {
+  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 (
-      lane_attribute == "right" || lane_attribute == "left" ||
-      (lane_attribute == "straight" && current_vel < stop_velocity_threshold));
+      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 =
@@ -160,6 +161,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;
@@ -175,7 +189,7 @@ std::optional<TurnSignalInfo> TurnSignalDecider::getIntersectionTurnSignalInfo(
     // Get the lane and its attribute
     const std::string lane_attribute =
       current_lane.attributeOr("turn_direction", std::string("none"));
-    if (!requires_turn_signal(lane_attribute)) continue;
+    if (!requires_turn_signal(lane_attribute, is_in_turn_lane)) continue;
 
     do {
       processed_lanes.insert(current_lane.id());
@@ -256,7 +270,7 @@ std::optional<TurnSignalInfo> TurnSignalDecider::getIntersectionTurnSignalInfo(
     } else if (search_distance <= dist_to_front_point) {
       continue;
     }
-    if (requires_turn_signal(lane_attribute)) {
+    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);