From 1a135cd12931783cb4909773a8adb6d79a51fc05 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 5 Mar 2024 14:23:07 +0900 Subject: [PATCH 1/2] fix(bpp): overwrite turn signal by latter module Signed-off-by: satoshi-ota --- .../src/scene.cpp | 2 +- .../src/goal_planner_module.cpp | 2 +- .../src/interface.cpp | 2 +- .../src/scene.cpp | 6 ++--- .../turn_signal_decider.hpp | 5 ++++ .../src/turn_signal_decider.cpp | 26 +++++++++++++++++++ 6 files changed, 37 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index eed1f324727fb..3c308b9ad152b 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -942,7 +942,7 @@ BehaviorModuleOutput AvoidanceModule::plan() linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_, planner_data_); 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 ce0fc8cfbece1..4f516a54db4fe 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 @@ -971,7 +971,7 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const 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/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 0e2aaed2b38f6..f71e115ebf1ca 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -441,7 +441,7 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( } // check the priority of turn signals - return module_type_->getTurnSignalDecider().use_prior_turn_signal( + return module_type_->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); } diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2021630f1ae34..d0ed5a9b2e98c 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -148,7 +148,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const output.turn_signal_info = prev_turn_signal_info_; 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_turn_signal_info_, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -183,7 +183,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_turn_signal_info_, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -226,7 +226,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_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 cd639b5e81092..df96876973470 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 @@ -82,6 +82,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); + 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 6ddc0df4eebf4..97c97c8fbf0d1 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,32 @@ 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 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); + }; + + const auto & new_desired_start_point = new_signal.desired_start_point; + const auto & new_required_start_point = new_signal.required_start_point; + + const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_; + const double dist_to_new_required_start = + get_distance(new_required_start_point) - base_link2front_; + + if (dist_to_new_desired_start > 0.0 && dist_to_new_required_start > 0.0) { + 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, From 55460350d7e9de90b9f06e74e8c605605001472c Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 7 Mar 2024 17:05:27 +0900 Subject: [PATCH 2/2] fix(avoidance): return previous module turn signal if it's in success state Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 3c308b9ad152b..5ad8f33bf75a1 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -888,7 +888,7 @@ BehaviorModuleOutput AvoidanceModule::plan() updatePathShifter(data.safe_shift_line); if (data.success) { - removeRegisteredShiftLines(); + return getPreviousModuleOutput(); } if (data.yield_required) {