From 8752b3749c5351b2edf72c82be0d5bf129ca551b Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 28 Apr 2022 18:18:56 +0900 Subject: [PATCH] feat(behavior_path): use multi thread (#657) * feat(behavior_path): use multi thread Signed-off-by: tomoya.kimura * fix mutex Signed-off-by: tomoya.kimura * fix double unlock of mutex Signed-off-by: tomoya.kimura * fix multi access to planner_data_ Signed-off-by: tomoya.kimura * rename mutex Signed-off-by: tomoya.kimura * apply mutex to bt_manager_ Signed-off-by: tomoya.kimura * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../behavior_path_planner_node.hpp | 1 + .../src/behavior_path_planner_node.cpp | 133 ++++++++++++------ 2 files changed, 91 insertions(+), 43 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 index 08d4f703c5548..62779c3d5924d 100644 --- 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 @@ -53,6 +53,7 @@ #include #include +#include #include #include diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 7e0bf94402163..da30b6d2e0d41 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -30,6 +30,20 @@ #include #include +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( - "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1)); - perception_subscriber_ = create_subscription( - "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1)); - scenario_subscriber_ = create_subscription( - "~/input/scenario", 1, [this](const Scenario::ConstSharedPtr msg) { - current_scenario_ = std::make_shared(*msg); - }); - external_approval_subscriber_ = create_subscription( - "~/input/external_approval", 1, - std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1)); - force_approval_subscriber_ = create_subscription( - "~/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( - "~/input/vector_map", qos_transient_local, - std::bind(&BehaviorPathPlannerNode::onMap, this, _1)); - route_subscriber_ = create_subscription( - "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1)); - // publisher path_publisher_ = create_publisher("~/output/path", 1); path_candidate_publisher_ = create_publisher("~/output/path_candidate", 1); @@ -94,8 +86,40 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod create_publisher("~/drivable_area_boundary", 1); } + // subscriber + velocity_subscriber_ = create_subscription( + "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1), + createSubscriptionOptions(this)); + perception_subscriber_ = create_subscription( + "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1), + createSubscriptionOptions(this)); + scenario_subscriber_ = create_subscription( + "~/input/scenario", 1, + [this](const Scenario::ConstSharedPtr msg) { + current_scenario_ = std::make_shared(*msg); + }, + createSubscriptionOptions(this)); + external_approval_subscriber_ = create_subscription( + "~/input/external_approval", 1, + std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1), + createSubscriptionOptions(this)); + force_approval_subscriber_ = create_subscription( + "~/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( + "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1), + createSubscriptionOptions(this)); + route_subscriber_ = create_subscription( + "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1), + createSubscriptionOptions(this)); + // behavior tree manager { + mutex_bt_.lock(); + bt_manager_ = std::make_shared(*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,31 +484,40 @@ 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) { @@ -490,15 +525,23 @@ void BehaviorPathPlannerNode::run() } // 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,25 +576,28 @@ 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 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"); @@ -559,11 +605,11 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( - const BehaviorModuleOutput & bt_output) + const BehaviorModuleOutput & bt_output, const std::shared_ptr planner_data) { auto path_candidate = bt_output.path_candidate ? bt_output.path_candidate : std::make_shared(); - 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> & statuses) + const std::vector> & statuses, + const std::shared_ptr 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 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 lock(mutex_pd_); planner_data_->self_odometry = msg; } void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg) { + std::lock_guard lock(mutex_pd_); planner_data_->dynamic_object = msg; } void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg) { + std::lock_guard 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 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 lock(mutex_pd_); planner_data_->route_handler->setMap(*msg); } void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg) { + std::lock_guard lock(mutex_pd_); const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); planner_data_->route_handler->setRoute(*msg);