From c958fdd0c10cce997fd280cbe0b83fd7c4fd9610 Mon Sep 17 00:00:00 2001
From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>
Date: Fri, 23 Dec 2022 10:03:40 +0900
Subject: [PATCH] feat(behavior_velocity_planner): change planner data update
 (#2531)

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>
---
 .../behavior_velocity_planner/node.hpp        |  3 +++
 .../behavior_velocity_planner/src/node.cpp    | 23 ++++++++++++-------
 2 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp
index 40155ae90f4fb..371c0cedfb174 100644
--- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp
+++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp
@@ -40,6 +40,7 @@
 
 namespace behavior_velocity_planner
 {
+using autoware_auto_mapping_msgs::msg::HADMapBin;
 using tier4_planning_msgs::msg::VelocityLimit;
 class BehaviorVelocityPlannerNode : public rclcpp::Node
 {
@@ -109,6 +110,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
   PlannerData planner_data_;
   BehaviorVelocityPlannerManager planner_manager_;
   bool is_driving_forward_{true};
+  HADMapBin::ConstSharedPtr map_ptr_{nullptr};
+  bool has_received_map_;
 
   // mutex for planner_data_
   std::mutex mutex_;
diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp
index 5fea6f0a13e82..ab5d31e01c224 100644
--- a/planning/behavior_velocity_planner/src/node.cpp
+++ b/planning/behavior_velocity_planner/src/node.cpp
@@ -238,12 +238,7 @@ bool BehaviorVelocityPlannerNode::isDataReady(
     RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for pointcloud");
     return false;
   }
-  if (!d.route_handler_) {
-    RCLCPP_INFO_THROTTLE(
-      get_logger(), clock, 3000, "Waiting for the initialization of route_handler");
-    return false;
-  }
-  if (!d.route_handler_->isMapMsgReady()) {
+  if (!map_ptr_) {
     RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for the initialization of map");
     return false;
   }
@@ -344,8 +339,8 @@ void BehaviorVelocityPlannerNode::onLaneletMap(
 {
   std::lock_guard<std::mutex> lock(mutex_);
 
-  // Load map
-  planner_data_.route_handler_ = std::make_shared<route_handler::RouteHandler>(*msg);
+  map_ptr_ = msg;
+  has_received_map_ = true;
 }
 
 void BehaviorVelocityPlannerNode::onTrafficSignals(
@@ -419,6 +414,18 @@ void BehaviorVelocityPlannerNode::onTrigger(
     return;
   }
 
+  // Load map and check route handler
+  if (has_received_map_) {
+    planner_data_.route_handler_ = std::make_shared<route_handler::RouteHandler>(*map_ptr_);
+    has_received_map_ = false;
+  }
+  if (!planner_data_.route_handler_) {
+    RCLCPP_INFO_THROTTLE(
+      get_logger(), *get_clock(), 3000, "Waiting for the initialization of route_handler");
+    mutex_.unlock();
+    return;
+  }
+
   // NOTE: planner_data must not be referenced for multithreading
   const auto planner_data = planner_data_;
   mutex_.unlock();