feat(behavior_velocity_planner): change planner data update (#2531)

Signed-off-by: yutaka <[email protected]> Signed-off-by: yutaka <[email protected]>
autowarefoundation · Dec 23, 2022 · c958fdd · c958fdd
1 parent 1f41e67
commit c958fdd
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 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
@@ -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
@@ -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();