feat(behavior_path_planner_common,turn_signal_decider): add turn_signal_remaining_shift_length_threshold (#7160)

danielsanchezaran · web-flow · commit 6e5d597e7b7b · 2024-05-30T14:45:42.000+09:00
add turn_signal_remaining_shift_length_threshold

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
@@ -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/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/planning/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);
   }
 

Original file line number	Diff line number	Diff line change
`@@ -615,7 +615,6 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(`
`615`	`615`	`const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted,`
`616`	`616`	`const bool override_ego_stopped_check, const bool is_pull_out) const`
`617`	`617`	`{`
`618`		`- constexpr double THRESHOLD = 0.1;`
`619`	`618`	`const auto & p = parameters;`
`620`	`619`	`const auto & rh = route_handler;`
`621`	`620`	`const auto & ego_pose = self_odometry->pose.pose;`
`@@ -674,7 +673,9 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(`
`674`	`673`	`}`
`675`	`674`
`676`	`675`	`// If the vehicle does not shift anymore, we turn off the blinker`
`677`		`- if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) {`
	`676`	`+ if (`
	`677`	`+ std::fabs(end_shift_length - current_shift_length) <`
	`678`	`+ p.turn_signal_remaining_shift_length_threshold) {`
`678`	`679`	`return std::make_pair(TurnSignalInfo{}, true);`
`679`	`680`	`}`
`680`	`681`