From e3d4e759f8e6a775ef4a956f9806dbf1fd3a0c75 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 18 Jan 2024 11:10:42 +0900 Subject: [PATCH 1/2] refactor(goal_planner): remove duplicated execution condition (#6087) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 7 ------- 1 file changed, 7 deletions(-) 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 3f765c99cffa9..a13344ee87e7c 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 @@ -458,13 +458,6 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } - // if goal modification is not allowed - // 1) goal_pose is in current_lanes, plan path to the original fixed goal - // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!utils::isAllowedGoalModification(route_handler)) { - return goal_is_in_current_lanes; - } - // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes From 3239b9912401ae4d663a3acff0aa12239c69ae63 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 18 Jan 2024 12:58:32 +0900 Subject: [PATCH 2/2] fix(behavior_velocity_planner, behavior_path_planner): refresh raw traffic light id map every callback (#6061) Signed-off-by: Mamoru Sobue --- .../src/behavior_path_planner_node.cpp | 1 + planning/behavior_velocity_planner/src/node.cpp | 14 +++++++++++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9b3fbedc37203..3eb5eca5954fb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -784,6 +784,7 @@ void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSh { std::lock_guard lock(mutex_pd_); + planner_data_->traffic_light_id_map.clear(); for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index e2c29ef868257..9fa5634c6dd65 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -323,6 +323,11 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( { std::lock_guard lock(mutex_); + // clear previous observation + planner_data_.traffic_light_id_map_raw_.clear(); + const auto traffic_light_id_map_last_observed_old = + planner_data_.traffic_light_id_map_last_observed_; + planner_data_.traffic_light_id_map_last_observed_.clear(); for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; @@ -334,9 +339,12 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( }); // if the observation is UNKNOWN and past observation is available, only update the timestamp // and keep the body of the info - if ( - is_unknown_observation && - planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) { + const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { + // copy last observation + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + old_data->second; + // update timestamp planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = msg->stamp; } else {