From a742e824366ac63f18fc2d304ad9e648132098a4 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Wed, 14 Aug 2024 18:28:53 +0900 Subject: [PATCH] fix(turn_signal, lane_change, goal_planner): add optional to tackle lane change turn signal and pull over turn signal (#8463) * add optional to tackle LC turn signal and pull over turn signal Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * CPP file should not re-define default value; typo in copying from internal repos Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- .../src/goal_planner_module.cpp | 4 +++- .../src/scene.cpp | 4 +++- .../behavior_path_planner_common/data_manager.hpp | 5 +++-- .../turn_signal_decider.hpp | 3 ++- .../src/turn_signal_decider.cpp | 14 +++++++++----- 5 files changed, 20 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index c4a338eb35888..6dee8d7b701bc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1957,6 +1957,8 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr bool is_driving_forward = true; constexpr bool is_pull_out = false; + constexpr bool is_lane_change = false; + constexpr bool is_pull_over = true; const bool override_ego_stopped_check = std::invoke([&]() { if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) { return false; @@ -1972,7 +1974,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, is_driving_forward, - egos_lane_is_shifted, override_ego_stopped_check, is_pull_out); + egos_lane_is_shifted, override_ego_stopped_check, is_pull_out, is_lane_change, is_pull_over); ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore); return new_signal; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 11e1c20e57adb..00c320b4ab589 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -603,10 +603,12 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's // current lane, lane change is different, so we set this flag to false. constexpr bool egos_lane_is_shifted = false; + constexpr bool is_pull_out = false; + constexpr bool is_lane_change = true; const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward, - egos_lane_is_shifted); + egos_lane_is_shifted, is_pull_out, is_lane_change); return new_signal; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp index 1b0bb240cabf2..12fb872ded9ac 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp @@ -174,7 +174,8 @@ struct PlannerData const PathWithLaneId & path, const size_t shift_start_idx, const size_t shift_end_idx, const lanelet::ConstLanelets & current_lanelets, const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, - const bool override_ego_stopped_check = false, const bool is_pull_out = false) const + const bool override_ego_stopped_check = false, const bool is_pull_out = false, + const bool is_lane_change = false, const bool is_pull_over = false) const { if (shift_start_idx + 1 > path.points.size()) { RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); @@ -211,7 +212,7 @@ struct PlannerData return turn_signal_decider.getBehaviorTurnSignalInfo( shifted_path, shift_line, current_lanelets, route_handler, parameters, self_odometry, current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check, - is_pull_out); + is_pull_out, is_lane_change, is_pull_over); } std::pair getBehaviorTurnSignalInfo( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp index bc41c77a07cea..90cf533a8589d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/turn_signal_decider.hpp @@ -134,7 +134,8 @@ class TurnSignalDecider const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check = false, - const bool is_pull_out = false) const; + const bool is_pull_out = false, const bool is_lane_change = false, + const bool is_pull_over = false) const; private: std::optional getIntersectionTurnSignalInfo( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index 2a9d3fb4d1d13..0e005166a97c0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -652,7 +652,8 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const std::shared_ptr route_handler, const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, 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 + const bool override_ego_stopped_check, const bool is_pull_out, const bool is_lane_change, + const bool is_pull_over) const { using autoware::universe_utils::getPose; @@ -770,15 +771,18 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); if ( - !is_pull_out && !existShiftSideLane( - start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, - p.turn_signal_shift_length_threshold)) { + (!is_pull_out && !is_lane_change && !is_pull_over) && + !existShiftSideLane( + start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, + p.turn_signal_shift_length_threshold)) { 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)) { + if ( + (!is_pull_out && !is_pull_over) && + !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); }