feat(behavior_path): use multi thread

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -53,6 +53,7 @@
 #include <lanelet2_traffic_rules/TrafficRulesFactory.h>
 
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
@@ -109,6 +110,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   TurnSignalDecider turn_signal_decider_;
 
+  std::mutex mutex_pd_;  // mutex for planner_data_
+  std::mutex mutex_bt_;  // mutex for bt_manager_
+
   // setup
   void waitForData();
 
@@ -146,22 +150,21 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   /**
    * @brief extract path from behavior tree output
    */
-  PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out);
+  PathWithLaneId::SharedPtr getPath(
+    const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> planner_data);
 
   /**
    * @brief extract path candidate from behavior tree output
    */
-  PathWithLaneId::SharedPtr getPathCandidate(const BehaviorModuleOutput & bt_out);
+  PathWithLaneId::SharedPtr getPathCandidate(
+    const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> planner_data);
 
   /**
    * @brief publish behavior module status mainly for the user interface
    */
-  void publishModuleStatus(const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses);
-
-  /**
-   * @brief update current pose on the planner_data_
-   */
-  void updateCurrentPose();
+  void publishModuleStatus(
+    const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses,
+    const std::shared_ptr<PlannerData> planner_data);
 
   // debug
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -30,6 +30,20 @@
 #include <utility>
 #include <vector>
 
+namespace
+{
+rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr)
+{
+  rclcpp::CallbackGroup::SharedPtr callback_group =
+    node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto sub_opt = rclcpp::SubscriptionOptions();
+  sub_opt.callback_group = callback_group;
+
+  return sub_opt;
+}
+}  // namespace
+
 namespace behavior_path_planner
 {
 using tier4_planning_msgs::msg::PathChangeModuleId;
@@ -47,28 +61,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     planner_data_->parameters = getCommonParam();
   }
 
-  velocity_subscriber_ = create_subscription<Odometry>(
-    "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1));
-  perception_subscriber_ = create_subscription<PredictedObjects>(
-    "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1));
-  scenario_subscriber_ = create_subscription<Scenario>(
-    "~/input/scenario", 1, [this](const Scenario::ConstSharedPtr msg) {
-      current_scenario_ = std::make_shared<Scenario>(*msg);
-    });
-  external_approval_subscriber_ = create_subscription<ApprovalMsg>(
-    "~/input/external_approval", 1,
-    std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1));
-  force_approval_subscriber_ = create_subscription<PathChangeModule>(
-    "~/input/force_approval", 1, std::bind(&BehaviorPathPlannerNode::onForceApproval, this, _1));
-
-  // route_handler
-  auto qos_transient_local = rclcpp::QoS{1}.transient_local();
-  vector_map_subscriber_ = create_subscription<HADMapBin>(
-    "~/input/vector_map", qos_transient_local,
-    std::bind(&BehaviorPathPlannerNode::onMap, this, _1));
-  route_subscriber_ = create_subscription<HADMapRoute>(
-    "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1));
-
   // publisher
   path_publisher_ = create_publisher<PathWithLaneId>("~/output/path", 1);
   path_candidate_publisher_ = create_publisher<Path>("~/output/path_candidate", 1);
@@ -94,8 +86,40 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
       create_publisher<MarkerArray>("~/drivable_area_boundary", 1);
   }
 
+  // subscriber
+  velocity_subscriber_ = create_subscription<Odometry>(
+    "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1),
+    createSubscriptionOptions(this));
+  perception_subscriber_ = create_subscription<PredictedObjects>(
+    "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1),
+    createSubscriptionOptions(this));
+  scenario_subscriber_ = create_subscription<Scenario>(
+    "~/input/scenario", 1,
+    [this](const Scenario::ConstSharedPtr msg) {
+      current_scenario_ = std::make_shared<Scenario>(*msg);
+    },
+    createSubscriptionOptions(this));
+  external_approval_subscriber_ = create_subscription<ApprovalMsg>(
+    "~/input/external_approval", 1,
+    std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1),
+    createSubscriptionOptions(this));
+  force_approval_subscriber_ = create_subscription<PathChangeModule>(
+    "~/input/force_approval", 1, std::bind(&BehaviorPathPlannerNode::onForceApproval, this, _1),
+    createSubscriptionOptions(this));
+
+  // route_handler
+  auto qos_transient_local = rclcpp::QoS{1}.transient_local();
+  vector_map_subscriber_ = create_subscription<HADMapBin>(
+    "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1),
+    createSubscriptionOptions(this));
+  route_subscriber_ = create_subscription<HADMapRoute>(
+    "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1),
+    createSubscriptionOptions(this));
+
   // behavior tree manager
   {
+    mutex_bt_.lock();
+
     bt_manager_ = std::make_shared<BehaviorTreeManager>(*this, getBehaviorTreeManagerParam());
 
     auto side_shift_module =
@@ -129,6 +153,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     bt_manager_->registerSceneModule(pull_out_module);
 
     bt_manager_->createBehaviorTree();
+
+    mutex_bt_.unlock();
   }
 
   // turn signal decider
@@ -458,47 +484,64 @@ void BehaviorPathPlannerNode::waitForData()
     rclcpp::Rate(100).sleep();
   }
 
+  mutex_pd_.lock();  // for planner_data_
   while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) {
+    mutex_pd_.unlock();
     RCLCPP_INFO_SKIPFIRST_THROTTLE(
       get_logger(), *get_clock(), 5000, "waiting for route to be ready");
     rclcpp::spin_some(this->get_node_base_interface());
     rclcpp::Rate(100).sleep();
+    mutex_pd_.lock();
   }
 
   while (rclcpp::ok()) {
     if (planner_data_->dynamic_object && planner_data_->self_odometry) {
       break;
     }
+
+    mutex_pd_.unlock();
     RCLCPP_INFO_SKIPFIRST_THROTTLE(
       get_logger(), *get_clock(), 5000,
       "waiting for vehicle pose, vehicle_velocity, and obstacles");
     rclcpp::spin_some(this->get_node_base_interface());
     rclcpp::Rate(100).sleep();
+    mutex_pd_.lock();
   }
 
   self_pose_listener_.waitForFirstPose();
   planner_data_->self_pose = self_pose_listener_.getCurrentPose();
+  mutex_pd_.unlock();
 }
 
 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) {
     return;
   }
 
   // update planner data
-  updateCurrentPose();
+  planner_data_->self_pose = self_pose_listener_.getCurrentPose();
+
+  // NOTE: planner_data must not be referenced for multithreading
+  const auto planner_data = planner_data_;
+  mutex_pd_.unlock();
 
   // run behavior planner
-  const auto output = bt_manager_->run(planner_data_);
+  const auto output = bt_manager_->run(planner_data);
 
   // path handling
-  const auto path = getPath(output);
-  const auto path_candidate = getPathCandidate(output);
+  const auto path = getPath(output, planner_data);
+  const auto path_candidate = getPathCandidate(output, planner_data);
+
+  // update planner data
+  mutex_pd_.lock();  // for planner_data_
   planner_data_->prev_output_path = path;
+  mutex_pd_.unlock();
 
   auto clipped_path = modifyPathForSmoothGoalConnection(*path);
   clipPathLength(clipped_path);
@@ -522,7 +565,7 @@ void BehaviorPathPlannerNode::run()
       hazard_signal.command = output.turn_signal_info.hazard_signal.command;
     } else {
       turn_signal = turn_signal_decider_.getTurnSignal(
-        *path, planner_data_->self_pose->pose, *(planner_data_->route_handler),
+        *path, planner_data->self_pose->pose, *(planner_data->route_handler),
         output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance);
       hazard_signal.command = HazardLightsCommand::DISABLE;
     }
@@ -533,37 +576,40 @@ void BehaviorPathPlannerNode::run()
   }
 
   // for remote operation
-  publishModuleStatus(bt_manager_->getModulesStatus());
+  publishModuleStatus(bt_manager_->getModulesStatus(), planner_data);
   debug_avoidance_msg_array_publisher_->publish(bt_manager_->getAvoidanceDebugMsgArray());
 
   publishDebugMarker(bt_manager_->getDebugMarkers());
 
-  if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
+  if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
     const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray(
-      util::getDrivableAreaForAllSharedLinestringLanelets(planner_data_));
+      util::getDrivableAreaForAllSharedLinestringLanelets(planner_data));
     debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines);
   }
+
+  mutex_bt_.unlock();
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
-PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output)
+PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(
+  const BehaviorModuleOutput & bt_output, const std::shared_ptr<PlannerData> planner_data)
 {
   // TODO(Horibe) do some error handling when path is not available.
 
-  auto path = bt_output.path ? bt_output.path : planner_data_->prev_output_path;
-  path->header = planner_data_->route_handler->getRouteHeader();
+  auto path = bt_output.path ? bt_output.path : planner_data->prev_output_path;
+  path->header = planner_data->route_handler->getRouteHeader();
   path->header.stamp = this->now();
   RCLCPP_DEBUG(
     get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND");
   return path;
 }
 
 PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
-  const BehaviorModuleOutput & bt_output)
+  const BehaviorModuleOutput & bt_output, const std::shared_ptr<PlannerData> planner_data)
 {
   auto path_candidate =
     bt_output.path_candidate ? bt_output.path_candidate : std::make_shared<PathWithLaneId>();
-  path_candidate->header = planner_data_->route_handler->getRouteHeader();
+  path_candidate->header = planner_data->route_handler->getRouteHeader();
   path_candidate->header.stamp = this->now();
   RCLCPP_DEBUG(
     get_logger(), "BehaviorTreeManager: path candidate is %s.",
@@ -572,7 +618,8 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
 }
 
 void BehaviorPathPlannerNode::publishModuleStatus(
-  const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses)
+  const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses,
+  const std::shared_ptr<PlannerData> planner_data)
 {
   auto getModuleType = [](std::string name) {
     if (name == "LaneChange") {
@@ -604,7 +651,7 @@ void BehaviorPathPlannerNode::publishModuleStatus(
       running_modules.modules.push_back(module);
     }
     if (status->module_name == "LaneChange") {
-      const auto force_approval = planner_data_->approval.is_force_approved;
+      const auto force_approval = planner_data->approval.is_force_approved;
       if (
         force_approval.module_name == "ForceLaneChange" &&
         (now - force_approval.stamp).seconds() < 0.5) {
@@ -654,28 +701,26 @@ void BehaviorPathPlannerNode::publishDebugMarker(const std::vector<MarkerArray>
   debug_marker_publisher_->publish(msg);
 }
 
-void BehaviorPathPlannerNode::updateCurrentPose()
-{
-  auto self_pose = self_pose_listener_.getCurrentPose();
-  planner_data_->self_pose = self_pose;
-}
-
 void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg)
 {
+  std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->self_odometry = msg;
 }
 void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg)
 {
+  std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->dynamic_object = msg;
 }
 void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg)
 {
+  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_);
   auto getModuleName = [](PathChangeModuleId module) {
     if (module.type == PathChangeModuleId::FORCE_LANE_CHANGE) {
       return "ForceLaneChange";
@@ -688,10 +733,12 @@ 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);
 }
 void BehaviorPathPlannerNode::onRoute(const HADMapRoute::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);