From 1a135cd12931783cb4909773a8adb6d79a51fc05 Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
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 <satoshi.ota928@gmail.com>
---
.../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 <satoshi.ota928@gmail.com>
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 <satoshi.ota928@gmail.com>
---
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) {