feat(behavior_path_planner_common,turn_signal_decider): add turn_sign…

…al_remaining_shift_length_threshold (autowarefoundation#7160) add turn_signal_remaining_shift_length_threshold Signed-off-by: Daniel Sanchez <[email protected]>
Interplai · Jun 3, 2024 · e00e6b0 · e00e6b0
1 parent af805ef
commit e00e6b0
Show file tree

Hide file tree

Showing 5 changed files with 16 additions and 10 deletions.
diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml
@@ -17,6 +17,7 @@
     turn_signal_minimum_search_distance: 10.0
     turn_signal_search_time: 3.0
     turn_signal_shift_length_threshold: 0.3
+    turn_signal_remaining_shift_length_threshold: 0.1
     turn_signal_on_swerving: true
 
     enable_akima_spline_first: false

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -260,6 +260,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.turn_signal_search_time = declare_parameter<double>("turn_signal_search_time");
   p.turn_signal_shift_length_threshold =
     declare_parameter<double>("turn_signal_shift_length_threshold");
+  p.turn_signal_remaining_shift_length_threshold =
+    declare_parameter<double>("turn_signal_remaining_shift_length_threshold");
   p.turn_signal_on_swerving = declare_parameter<bool>("turn_signal_on_swerving");
 
   p.enable_akima_spline_first = declare_parameter<bool>("enable_akima_spline_first");

diff --git a/...g/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/...g/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md
@@ -20,14 +20,15 @@ Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in c
 
 ## Parameters for turn signal decider
 
-| Name                                            | Unit | Type   | Description                                                                  | Default value |
-| :---------------------------------------------- | :--- | :----- | :--------------------------------------------------------------------------- | :------------ |
-| turn_signal_intersection_search_distance        | [m]  | double | constant search distance to decide activation of blinkers at intersections   | 30            |
-| turn_signal_intersection_angle_threshold_degree | deg  | double | angle threshold to determined the end point of intersection required section | 15            |
-| turn_signal_minimum_search_distance             | [m]  | double | minimum search distance for avoidance and lane change                        | 10            |
-| turn_signal_search_time                         | [s]  | double | search time for to decide activation of blinkers                             | 3.0           |
-| turn_signal_shift_length_threshold              | [m]  | double | shift length threshold to decide activation of blinkers                      | 0.3           |
-| turn_signal_on_swerving                         | [-]  | bool   | flag to activate blinkers when swerving                                      | true          |
+| Name                                            | Unit | Type   | Description                                                                                                                     | Default value |
+| :---------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------ | :------------ |
+| turn_signal_intersection_search_distance        | [m]  | double | constant search distance to decide activation of blinkers at intersections                                                      | 30            |
+| turn_signal_intersection_angle_threshold_degree | deg  | double | angle threshold to determined the end point of intersection required section                                                    | 15            |
+| turn_signal_minimum_search_distance             | [m]  | double | minimum search distance for avoidance and lane change                                                                           | 10            |
+| turn_signal_search_time                         | [s]  | double | search time for to decide activation of blinkers                                                                                | 3.0           |
+| turn_signal_shift_length_threshold              | [m]  | double | shift length threshold to decide activation of blinkers                                                                         | 0.3           |
+| turn_signal_remaining_shift_length_threshold    | [m]  | double | When the ego's current shift length minus its end shift length is less than this threshold, the turn signal will be turned off. | 0.1           |
+| turn_signal_on_swerving                         | [-]  | bool   | flag to activate blinkers when swerving                                                                                         | true          |
 
 Note that the default values for `turn_signal_intersection_search_distance` and `turn_signal_search_time` is strictly followed by [Japanese Road Traffic Laws](https://www.japaneselawtranslation.go.jp/ja/laws/view/2962). So if your country does not allow to use these default values, you should change these values in configuration files.
 

diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp
@@ -54,6 +54,7 @@ struct BehaviorPathPlannerParameters
   double turn_signal_search_time;
   double turn_signal_minimum_search_distance;
   double turn_signal_shift_length_threshold;
+  double turn_signal_remaining_shift_length_threshold;
   bool turn_signal_on_swerving;
 
   bool enable_akima_spline_first;

diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp
@@ -615,7 +615,6 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
   const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted,
   const bool override_ego_stopped_check, const bool is_pull_out) const
 {
-  constexpr double THRESHOLD = 0.1;
   const auto & p = parameters;
   const auto & rh = route_handler;
   const auto & ego_pose = self_odometry->pose.pose;
@@ -674,7 +673,9 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
   }
 
   // If the vehicle does not shift anymore, we turn off the blinker
-  if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) {
+  if (
+    std::fabs(end_shift_length - current_shift_length) <
+    p.turn_signal_remaining_shift_length_threshold) {
     return std::make_pair(TurnSignalInfo{}, true);
   }