diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 8d63414a4d1b8..ad2bb9cdafe70 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -60,6 +60,7 @@ class LaneChangeBase prev_module_reference_path_ = std::make_shared(); prev_module_path_ = std::make_shared(); prev_drivable_area_info_ = std::make_shared(); + prev_turn_signal_info_ = std::make_shared(); } LaneChangeBase(const LaneChangeBase &) = delete; @@ -107,6 +108,11 @@ class LaneChangeBase } } + virtual void setPreviousTurnSignalInfo(const TurnSignalInfo & prev_turn_signal_info) + { + *prev_turn_signal_info_ = prev_turn_signal_info; + } + virtual void updateSpecialData() {} const LaneChangeStatus & getLaneChangeStatus() const { return status_; } @@ -232,6 +238,7 @@ class LaneChangeBase std::shared_ptr prev_module_reference_path_{}; std::shared_ptr prev_module_path_{}; std::shared_ptr prev_drivable_area_info_{}; + std::shared_ptr prev_turn_signal_info_{}; PathWithLaneId prev_approved_path_{}; 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 1ab26c99f59a2..e15c5294fd5cf 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 @@ -2783,7 +2783,18 @@ BehaviorModuleOutput AvoidanceModule::plan() } BehaviorModuleOutput output; - output.turn_signal_info = calcTurnSignalInfo(avoidance_path); + + // turn signal info + { + const auto original_signal = getPreviousModuleOutput().turn_signal_info; + const auto new_signal = calcTurnSignalInfo(avoidance_path); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(avoidance_path.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + avoidance_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + } + // sparse resampling for computational cost { avoidance_path.path = diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 829134411e39b..f2a3610c7d56f 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -707,7 +707,13 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set hazard and turn signal if (status_.has_decided_path) { - output.turn_signal_info = calcTurnSignalInfo(); + 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.path, getEgoPose(), current_seg_idx, original_signal, new_signal, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); } const auto distance_to_path_change = calcDistanceToPathChange(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 2e3dfe7545ca7..a407a872d4279 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -131,6 +131,7 @@ BehaviorModuleOutput LaneChangeInterface::plan() } module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); + module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); auto output = module_type_->generateOutput(); path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c5d1f0eb25bdd..ac81db8552492 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -105,6 +105,16 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() output.reference_path = std::make_shared(getReferencePath()); output.turn_signal_info = updateOutputTurnSignal(); + if (!prev_turn_signal_info_) { + return 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.path, getEgoPose(), current_seg_idx, *prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + return output; } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 893d15f9bb5ea..49d19c91c93c1 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -187,7 +187,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj) { double min_distance = std::numeric_limits::max(); - double max_distance = std::numeric_limits::min(); + double max_distance = std::numeric_limits::lowest(); for (const auto & p : obj.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); const double arc_length = motion_utils::calcSignedArcLength(path.points, ego_pos, point);