Merge branch 'main' into refactor/calculate_expanded_drivable_lanes_e…

…fficiently
autowarefoundation · Jan 18, 2024 · ea1fe4f · ea1fe4f
2 parents fd075ac + 3239b99
commit ea1fe4f
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 10 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
@@ -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

diff --git 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<std::mutex> 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
@@ -323,6 +323,11 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
 {
   std::lock_guard<std::mutex> 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 {