feat(behavior_path_planner): change planner data update method

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -112,6 +112,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
   Scenario::SharedPtr current_scenario_{nullptr};
 
+  HADMapBin::ConstSharedPtr map_ptr_{nullptr};
+  LaneletRoute::ConstSharedPtr route_ptr_{nullptr};
+  bool has_received_map_{false};
+  bool has_received_route_{false};
+
   TurnSignalDecider turn_signal_decider_;
 
   std::mutex mutex_pd_;  // mutex for planner_data_
@@ -120,6 +125,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // setup
   bool isDataReady();
 
+  // update planner data
+  std::shared_ptr<PlannerData> createLatestPlannerData();
+
   // parameters
   std::shared_ptr<AvoidanceParameters> avoidance_param_ptr;
   std::shared_ptr<LaneChangeParameters> lane_change_param_ptr;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -567,19 +567,21 @@ bool BehaviorPathPlannerNode::isDataReady()
 {
   const auto missing = [this](const auto & name) {
     RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", name);
-    mutex_pd_.unlock();
     return false;
   };
 
-  mutex_pd_.lock();  // for planner_data_
   if (!current_scenario_) {
     return missing("scenario_topic");
   }
 
-  if (!planner_data_->route_handler->isHandlerReady()) {
+  if (!route_ptr_) {
     return missing("route");
   }
 
+  if (!map_ptr_) {
+    return missing("map");
+  }
+
   if (!planner_data_->dynamic_object) {
     return missing("dynamic_object");
   }
@@ -597,10 +599,39 @@ bool BehaviorPathPlannerNode::isDataReady()
     return missing("self_pose");
   }
 
-  mutex_pd_.unlock();
   return true;
 }
 
+std::shared_ptr<PlannerData> BehaviorPathPlannerNode::createLatestPlannerData()
+{
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
+
+  // update self
+  planner_data_->self_pose = self_pose_listener_.getCurrentPose();
+
+  // update map
+  if (has_received_map_) {
+    planner_data_->route_handler->setMap(*map_ptr_);
+    has_received_map_ = false;
+  }
+
+  // update route
+  const bool is_first_time = !(planner_data_->route_handler->isHandlerReady());
+  if (has_received_route_) {
+    planner_data_->route_handler->setRoute(*route_ptr_);
+    // Reset behavior tree when new route is received,
+    // so that the each modules do not have to care about the "route jump".
+    if (!is_first_time) {
+      RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree.");
+      bt_manager_->resetBehaviorTree();
+    }
+
+    has_received_route_ = false;
+  }
+
+  return std::make_shared<PlannerData>(*planner_data_);
+}
+
 void BehaviorPathPlannerNode::run()
 {
   if (!isDataReady()) {
@@ -609,22 +640,15 @@ void BehaviorPathPlannerNode::run()
 
   RCLCPP_DEBUG(get_logger(), "----- BehaviorPathPlannerNode start -----");
   mutex_bt_.lock();  // for bt_manager_
-  mutex_pd_.lock();  // for planner_data_
 
   // behavior_path_planner runs only in LANE DRIVING scenario.
   if (current_scenario_->current_scenario != Scenario::LANEDRIVING) {
     mutex_bt_.unlock();  // for bt_manager_
-    mutex_pd_.unlock();  // for planner_data_
     return;
   }
 
-  // update planner data
-  planner_data_->self_pose = self_pose_listener_.getCurrentPose();
-
-  const auto planner_data = std::make_shared<PlannerData>(*planner_data_);
-
-  // unlock planner data
-  mutex_pd_.unlock();
+  // create latest planner data
+  const auto planner_data = createLatestPlannerData();
 
   // run behavior planner
   const auto output = bt_manager_->run(planner_data);
@@ -839,34 +863,34 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection(
 
 void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->self_odometry = msg;
 }
 void BehaviorPathPlannerNode::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->self_acceleration = msg;
 }
 void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->dynamic_object = msg;
 }
 void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->occupancy_grid = msg;
 }
 void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->approval.is_approved.data = msg->approval;
   // TODO(wep21): Replace msg stamp after {stamp: now} is implemented in ros2 topic pub
   planner_data_->approval.is_approved.stamp = this->now();
 }
 void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
   auto getModuleName = [](PathChangeModuleId module) {
     if (module.type == PathChangeModuleId::FORCE_LANE_CHANGE) {
       return "ForceLaneChange";
@@ -879,22 +903,15 @@ void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstShare
 }
 void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
-  planner_data_->route_handler->setMap(*msg);
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
+  map_ptr_ = msg;
+  has_received_map_ = true;
 }
 void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_pd_);
-  const bool is_first_time = !(planner_data_->route_handler->isHandlerReady());
-
-  planner_data_->route_handler->setRoute(*msg);
-
-  // Reset behavior tree when new route is received,
-  // so that the each modules do not have to care about the "route jump".
-  if (!is_first_time) {
-    RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree.");
-    bt_manager_->resetBehaviorTree();
-  }
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
+  route_ptr_ = msg;
+  has_received_route_ = true;
 }
 
 SetParametersResult BehaviorPathPlannerNode::onSetParam(