feat(start_planner): add length_ratio_for_turn_signal_deactivation_ne…

…ar_intersection (autowarefoundation#4026)

* feat(start_planner): add length_ratio_for_turn_signal_deactivation_near_intersection

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_path_planner/config/start_planner/start_planner.param.yaml

@@ -4,7 +4,6 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
-      th_blinker_on_lateral_offset: 1.0
       collision_check_margin: 1.0
       collision_check_distance_from_end: 1.0
       # shift pull out
@@ -30,3 +29,7 @@
       backward_search_resolution: 2.0
       backward_path_update_duration: 3.0
       ignore_distance_from_lane_end: 15.0
+      # turns signal
+      th_turn_signal_on_lateral_offset: 1.0
+      intersection_search_length: 30.0
+      length_ratio_for_turn_signal_deactivation_near_intersection: 0.5

planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md

@@ -49,14 +49,16 @@ PullOutPath --o PullOutPlannerBase
 
 ## General parameters for start_planner
 
-| Name                              | Unit  | Type   | Description                                                          | Default value |
-| :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ |
-| th_arrived_distance_m             | [m]   | double | distance threshold for arrival of path termination                   | 1.0           |
-| th_stopped_velocity_mps           | [m/s] | double | velocity threshold for arrival of path termination                   | 0.01          |
-| th_stopped_time_sec               | [s]   | double | time threshold for arrival of path termination                       | 1.0           |
-| th_blinker_on_lateral_offset      | [m]   | double | lateral distance threshold for turning on blinker                    | 1.0           |
-| collision_check_margin            | [m]   | double | Obstacle collision check margin                                      | 1.0           |
-| collision_check_distance_from_end | [m]   | double | collision check distance from end point. currently only for pull out | 15.0          |
+| Name                                                        | Unit  | Type   | Description                                                          | Default value |
+| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ |
+| th_arrived_distance_m                                       | [m]   | double | distance threshold for arrival of path termination                   | 1.0           |
+| th_stopped_velocity_mps                                     | [m/s] | double | velocity threshold for arrival of path termination                   | 0.01          |
+| th_stopped_time_sec                                         | [s]   | double | time threshold for arrival of path termination                       | 1.0           |
+| th_turn_signal_on_lateral_offset                            | [m]   | double | lateral distance threshold for turning on blinker                    | 1.0           |
+| intersection_search_length                                  | [m]   | double | check if intersections exist within this length                      | 30.0          |
+| length_ratio_for_turn_signal_deactivation_near_intersection | [m]   | double | deactivate turn signal of this module near intersection              | 0.5           |
+| collision_check_margin                                      | [m]   | double | Obstacle collision check margin                                      | 1.0           |
+| collision_check_distance_from_end                           | [m]   | double | collision check distance from end point. currently only for pull out | 15.0          |
 
 ## **Safe check with obstacles in shoulder lane**
 

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp

@@ -28,7 +28,9 @@ struct StartPlannerParameters
   double th_arrived_distance;
   double th_stopped_velocity;
   double th_stopped_time;
-  double th_blinker_on_lateral_offset;
+  double th_turn_signal_on_lateral_offset;
+  double intersection_search_length;
+  double length_ratio_for_turn_signal_deactivation_near_intersection;
   double collision_check_margin;
   double collision_check_distance_from_end;
   // shift pull out

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1020,7 +1020,11 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam()
   p.th_arrived_distance = declare_parameter<double>(ns + "th_arrived_distance");
   p.th_stopped_velocity = declare_parameter<double>(ns + "th_stopped_velocity");
   p.th_stopped_time = declare_parameter<double>(ns + "th_stopped_time");
-  p.th_blinker_on_lateral_offset = declare_parameter<double>(ns + "th_blinker_on_lateral_offset");
+  p.th_turn_signal_on_lateral_offset =
+    declare_parameter<double>(ns + "th_turn_signal_on_lateral_offset");
+  p.intersection_search_length = declare_parameter<double>(ns + "intersection_search_length");
+  p.length_ratio_for_turn_signal_deactivation_near_intersection =
+    declare_parameter<double>(ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
   p.collision_check_margin = declare_parameter<double>(ns + "collision_check_margin");
   p.collision_check_distance_from_end =
     declare_parameter<double>(ns + "collision_check_distance_from_end");

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -801,20 +801,64 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
   const auto lane = planner_data_->route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id);
   const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(lane, start_pose);
 
-  if (distance_from_end < 0.0 && lateral_offset > parameters_->th_blinker_on_lateral_offset) {
+  if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) {
     turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
   } else if (
-    distance_from_end < 0.0 && lateral_offset < -parameters_->th_blinker_on_lateral_offset) {
+    distance_from_end < 0.0 && lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) {
     turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
   } else {
     turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE;
   }
 
   turn_signal.desired_start_point = start_pose;
   turn_signal.required_start_point = start_pose;
-  turn_signal.required_end_point = end_pose;
   turn_signal.desired_end_point = end_pose;
 
+  // check if intersection exists within search length
+  const bool is_near_intersection = std::invoke([&]() {
+    const double check_length = parameters_->intersection_search_length;
+    double accumulated_length = 0.0;
+    for (size_t i = 0; i < path.points.size() - 1; ++i) {
+      const auto & p = path.points.at(i);
+      for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) {
+        const std::string turn_direction = lane.attributeOr("turn_direction", "else");
+        if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
+          return true;
+        }
+      }
+      accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1));
+      if (accumulated_length > check_length) {
+        return false;
+      }
+    }
+    return false;
+  });
+
+  if (is_near_intersection) {
+    // offset required end pose with ration to activate turn signal for intersection
+    turn_signal.required_end_point = std::invoke([&]() {
+      const double length_start_to_end =
+        motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
+      const auto ratio = std::clamp(
+        parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
+
+      const double required_end_length = length_start_to_end * ratio;
+      double accumulated_length = 0.0;
+      const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
+      for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
+        accumulated_length +=
+          tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
+        if (accumulated_length > required_end_length) {
+          return path.points.at(i).point.pose;
+        }
+      }
+      // not found required end point
+      return end_pose;
+    });
+  } else {
+    turn_signal.required_end_point = end_pose;
+  }
+
   return turn_signal;
 }