fix(behavior_path_planner): fix turn singal output in a avoidance seq…

…uence (tier4#1511) * remove search distance for turn signal Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * set distance to max when a lane_attriute is straight Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
boyali · Oct 19, 2022 · a938ad1 · a938ad1
1 parent 71d3975
commit a938ad1
Show file tree

Hide file tree

Showing 10 changed files with 8 additions and 17 deletions.
diff --git a/...g/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/...g/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml
@@ -17,4 +17,3 @@
       use_predicted_path_outside_lanelet: false
       use_all_predicted_path: false
       enable_blocked_by_obstacle: false
-      lane_change_search_distance: 30.0
diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
@@ -17,4 +17,3 @@
       use_predicted_path_outside_lanelet: false
       use_all_predicted_path: false
       enable_blocked_by_obstacle: false
-      lane_change_search_distance: 30.0
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp
@@ -67,7 +67,7 @@ void clipPathLength(
 std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path,
   const ShiftPoint & shift_point, const Pose & pose, const double & velocity,
-  const BehaviorPathPlannerParameters & common_parameter, const double & search_distance);
+  const BehaviorPathPlannerParameters & common_parameter);
 
 }  // namespace util
 }  // namespace behavior_path_planner

diff --git a/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
@@ -144,10 +144,6 @@ struct AvoidanceParameters
   // acceleration.
   double max_avoidance_acceleration;
 
-  // if distance between vehicle front and shift end point is larger than this length,
-  // turn signal is not turned on.
-  double avoidance_search_distance;
-
   // The avoidance path generation is performed when the shift distance of the
   // avoidance points is greater than this threshold.
   // In multiple targets case: if there are multiple vehicles in a row to be avoided, no new

diff --git a/...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
@@ -60,7 +60,6 @@ struct LaneChangeParameters
   bool use_predicted_path_outside_lanelet;
   bool use_all_predicted_path;
   bool enable_blocked_by_obstacle;
-  double lane_change_search_distance;
 };
 
 struct LaneChangeStatus

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -279,7 +279,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
 
   p.min_avoidance_speed_for_acc_prevention = dp("min_avoidance_speed_for_acc_prevention", 3.0);
   p.max_avoidance_acceleration = dp("max_avoidance_acceleration", 0.5);
-  p.avoidance_search_distance = dp("avoidance_search_distance", 30.0);
 
   p.publish_debug_marker = dp("publish_debug_marker", false);
   p.print_debug_info = dp("print_debug_info", false);
@@ -338,7 +337,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
     dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0));
   p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3);
   p.enable_blocked_by_obstacle = dp("enable_blocked_by_obstacle", false);
-  p.lane_change_search_distance = dp("lane_change_search_distance", 30.0);
 
   // validation of parameters
   if (p.lane_change_sampling_num < 1) {

diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp
@@ -233,7 +233,7 @@ void clipPathLength(
 std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path,
   const ShiftPoint & shift_point, const Pose & pose, const double & velocity,
-  const BehaviorPathPlannerParameters & common_parameter, const double & search_distance)
+  const BehaviorPathPlannerParameters & common_parameter)
 {
   TurnIndicatorsCommand turn_signal;
   turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
@@ -341,8 +341,7 @@ std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
       arc_position_shift_end.length - arc_position_current_pose.length - base_link2front;
   }
 
-  // Output distance when the distance is positive and smaller than search distance
-  if (distance_from_vehicle_front >= 0.0 && distance_from_vehicle_front <= search_distance) {
+  if (distance_from_vehicle_front >= 0.0) {
     return std::make_pair(turn_signal, distance_from_vehicle_front);
   }
 

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -2653,8 +2653,7 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con
 
   const auto turn_info = util::getPathTurnSignal(
     avoidance_data_.current_lanelets, path, latest_shift_point, planner_data_->self_pose->pose,
-    planner_data_->self_odometry->twist.twist.linear.x, planner_data_->parameters,
-    parameters_.avoidance_search_distance);
+    planner_data_->self_odometry->twist.twist.linear.x, planner_data_->parameters);
 
   // Set turn signal if the vehicle across the lane.
   if (!path.shift_length.empty()) {

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
@@ -166,8 +166,7 @@ BehaviorModuleOutput LaneChangeModule::plan()
   const auto turn_signal_info = util::getPathTurnSignal(
     status_.current_lanes, status_.lane_change_path.shifted_path,
     status_.lane_change_path.shift_point, planner_data_->self_pose->pose,
-    planner_data_->self_odometry->twist.twist.linear.x, planner_data_->parameters,
-    parameters_.lane_change_search_distance);
+    planner_data_->self_odometry->twist.twist.linear.x, planner_data_->parameters);
   output.turn_signal_info.turn_signal.command = turn_signal_info.first.command;
   output.turn_signal_info.signal_distance = turn_signal_info.second;
   return output;

diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp
@@ -98,6 +98,9 @@ std::pair<TurnIndicatorsCommand, double> TurnSignalDecider::getIntersectionTurnS
           turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
         } else if (lane_attribute == std::string("right")) {
           turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
+        } else {
+          // when lane_attribute is straight, return the turn signal with max distance
+          return std::make_pair(turn_signal, std::numeric_limits<double>::max());
         }
         distance = distance_from_vehicle_front;
       }