add turn_signal_remaining_shift_length_threshold

danielsanchezaran · saka1-s · commit 947960a9fa8d · 2024-05-29T19:14:51.000+09:00
Signed-off-by: Daniel Sanchez &lt;danielsanchezaran@gmail.com&gt;
diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml
@@ -18,6 +18,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/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner/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/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -259,6 +259,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/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
@@ -58,6 +58,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
@@ -622,7 +622,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;
@@ -681,7 +680,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);
   }
 

Original file line number	Diff line number	Diff line change
`@@ -622,7 +622,6 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(`
`622`	`622`	`const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted,`
`623`	`623`	`const bool override_ego_stopped_check, const bool is_pull_out) const`
`624`	`624`	`{`
`625`		`- constexpr double THRESHOLD = 0.1;`
`626`	`625`	`const auto & p = parameters;`
`627`	`626`	`const auto & rh = route_handler;`
`628`	`627`	`const auto & ego_pose = self_odometry->pose.pose;`
`@@ -681,7 +680,9 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(`
`681`	`680`	`}`
`682`	`681`
`683`	`682`	`// If the vehicle does not shift anymore, we turn off the blinker`
`684`		`- if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) {`
	`683`	`+ if (`
	`684`	`+ std::fabs(end_shift_length - current_shift_length) <`
	`685`	`+ p.turn_signal_remaining_shift_length_threshold) {`
`685`	`686`	`return std::make_pair(TurnSignalInfo{}, true);`
`686`	`687`	`}`
`687`	`688`