From 665b31b4e7e40327d091d3007a1815918ea49538 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 4 Aug 2022 00:43:55 +0900 Subject: [PATCH] fix(behavior_path_planner): fix turn singal output in a avoidance sequence (#1511) * remove search distance for turn signal Signed-off-by: tomoya.kimura * set distance to max when a lane_attriute is straight Signed-off-by: tomoya.kimura --- .../behavior_path_planner/lane_change/lane_change.param.yaml | 1 - .../config/lane_change/lane_change.param.yaml | 1 - .../include/behavior_path_planner/path_utilities.hpp | 2 +- .../scene_module/avoidance/avoidance_module_data.hpp | 4 ---- .../scene_module/lane_change/lane_change_module.hpp | 1 - .../behavior_path_planner/src/behavior_path_planner_node.cpp | 2 -- planning/behavior_path_planner/src/path_utilities.cpp | 5 ++--- .../src/scene_module/avoidance/avoidance_module.cpp | 3 +-- .../src/scene_module/lane_change/lane_change_module.cpp | 3 +-- planning/behavior_path_planner/src/turn_signal_decider.cpp | 3 +++ 10 files changed, 8 insertions(+), 17 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml index 50934a74542ea..86226a95ce561 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/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 index 50934a74542ea..86226a95ce561 100644 --- 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 index 7b7cc3b186f03..eb35749e5c69d 100644 --- 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 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/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 790417222651e..5f444ae1a0995 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_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/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index fcf9e635f4595..8b06b5dc75820 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_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 index 3b0376d059329..a5c843b98518f 100644 --- 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 index a8ed63359b9ea..8fdc3ddd44224 100644 --- 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 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 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 index 43e8ba60f3c82..9212a077e4cbd 100644 --- 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 index 97f1833310d05..fba45e56619d5 100644 --- 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 index d4d70cb83c761..7b1fa47823e64 100644 --- 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 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::max()); } distance = distance_from_vehicle_front; }