From b553c4973c7507499bb92dbf099d8b3ee5e2271d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 5 Jun 2024 23:18:40 +0900 Subject: [PATCH] fix(bpp): overwrite turn signal by latter module (#7045) * fix(bpp): overwrite turn signal by latter module Signed-off-by: satoshi-ota * fix(avoidance): return previous module turn signal if it's in success state Signed-off-by: satoshi-ota * fix(avoidance): don't output turn signal if there is huge lateral deveation Signed-off-by: satoshi-ota * refactor: small update Signed-off-by: satoshi-ota * chore: use lower case Signed-off-by: satoshi-ota * refactor: remove redundant function call Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/scene.cpp | 14 ++++- .../src/goal_planner_module.cpp | 2 +- .../src/scene.cpp | 8 +-- .../turn_signal_decider.hpp | 16 ++++++ .../src/turn_signal_decider.cpp | 55 ++++++++++++++++--- 5 files changed, 81 insertions(+), 14 deletions(-) diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 0a6d490f7fbf1..48dae73231b9f 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -867,10 +867,12 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() if (data.state == AvoidanceState::SUCCEEDED) { removeRegisteredShiftLines(State::SUCCEEDED); + return getPreviousModuleOutput(); } if (data.state == AvoidanceState::CANCEL) { removeRegisteredShiftLines(State::FAILED); + return getPreviousModuleOutput(); } if (data.yield_required) { @@ -915,11 +917,21 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt; }; + const auto is_large_deviation = [this](const auto & path) { + constexpr double threshold = 1.0; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto lateral_deviation = + motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); + return std::abs(lateral_deviation) > threshold; + }; + // turn signal info if (path_shifter_.getShiftLines().empty()) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else if (is_large_deviation(spline_shift_path.path)) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; @@ -930,7 +942,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 9bfef90668e8e..4acc4dce743ad 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1092,7 +1092,7 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2d88a820e0fae..e1c4289b57612 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -204,7 +204,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() } // check the priority of turn signals - return getTurnSignalDecider().use_prior_turn_signal( + return getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); } @@ -226,7 +226,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const output.path = abort_path_->path; extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -252,7 +252,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -297,7 +297,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index da39bf0d0d0ba..7e335b09e4fd3 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -64,6 +64,17 @@ struct TurnSignalInfo hazard_signal.command = HazardLightsCommand::NO_COMMAND; } + TurnSignalInfo(const Pose & start, const Pose & end) + { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + hazard_signal.command = HazardLightsCommand::NO_COMMAND; + + desired_start_point = start; + desired_end_point = end; + required_start_point = start; + required_end_point = end; + } + // desired turn signal TurnIndicatorsCommand turn_signal; HazardLightsCommand hazard_signal; @@ -93,6 +104,11 @@ class TurnSignalDecider const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info, const double nearest_dist_threshold, const double nearest_yaw_threshold); + TurnSignalInfo overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const; + TurnSignalInfo use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index fb15391550980..146fb03cc00f2 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -392,6 +392,40 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( return intersection_signal_info.turn_signal; } +TurnSignalInfo TurnSignalDecider::overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const +{ + if (original_signal.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) { + return new_signal; + } + + if (original_signal.turn_signal.command == TurnIndicatorsCommand::DISABLE) { + return new_signal; + } + + const auto get_distance = [&](const Pose & input_point) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, + nearest_seg_idx) - + base_link2front_; + }; + + const auto & original_desired_end_point = original_signal.desired_end_point; + const auto & new_desired_start_point = new_signal.desired_start_point; + + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_new_desired_start = get_distance(new_desired_start_point); + if (dist_to_new_desired_start > dist_to_original_desired_end) { + return original_signal; + } + + return new_signal; +} + TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, @@ -615,6 +649,8 @@ std::pair 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 { + using tier4_autoware_utils::getPose; + const auto & p = parameters; const auto & rh = route_handler; const auto & ego_pose = self_odometry->pose.pose; @@ -667,16 +703,19 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const auto relative_shift_length = end_shift_length - start_shift_length; + const auto p_path_start = getPose(path.path.points.front()); + const auto p_path_end = getPose(path.path.points.back()); + // If shift length is shorter than the threshold, it does not need to turn on blinkers if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the vehicle does not shift anymore, we turn off the blinker if ( std::fabs(end_shift_length - current_shift_length) < p.turn_signal_remaining_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto get_command = [](const auto & shift_length) { @@ -691,7 +730,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( p.vehicle_info.max_longitudinal_offset_m; if (signal_prepare_distance < ego_front_to_shift_start) { - return std::make_pair(TurnSignalInfo{}, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; @@ -708,13 +747,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( turn_signal_info.turn_signal.command = get_command(relative_shift_length); if (!p.turn_signal_on_swerving) { - return std::make_pair(turn_signal_info, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } lanelet::ConstLanelet lanelet; const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start; if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); @@ -729,13 +768,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( !is_pull_out && !existShiftSideLane( start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // Check if the ego will cross lane bounds. // Note that pull out requires blinkers, even if the ego does not cross lane bounds if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the ego has stopped and its close to completing its shift, turn off the blinkers @@ -744,7 +783,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( if (isNearEndOfShift( start_shift_length, end_shift_length, ego_pose.position, current_lanelets, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } }