apply mutex to bt_manager_

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
autowarefoundation · Apr 26, 2022 · e4b82a3 · e4b82a3
1 parent 9ecea85
commit e4b82a3
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -104,8 +104,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   TurnSignalDecider turn_signal_decider_;
 
-  // mutex for planner_data_
-  std::mutex mutex_pd_;
+  std::mutex mutex_pd_;  // mutex for planner_data_
+  std::mutex mutex_bt_;  // mutex for bt_manager_
 
   // setup
   void waitForData();

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -116,6 +116,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
 
   // behavior tree manager
   {
+    mutex_bt_.lock();
+
     bt_manager_ = std::make_shared<BehaviorTreeManager>(*this, getBehaviorTreeManagerParam());
 
     auto side_shift_module =
@@ -149,6 +151,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     bt_manager_->registerSceneModule(pull_out_module);
 
     bt_manager_->createBehaviorTree();
+
+    mutex_bt_.unlock();
   }
 
   // turn signal decider
@@ -510,6 +514,7 @@ void BehaviorPathPlannerNode::waitForData()
 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.
@@ -579,6 +584,7 @@ void BehaviorPathPlannerNode::run()
     debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines);
   }
 
+  mutex_bt_.unlock();
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }