From 172550a8fbb0aa6ddf31ffb35431508ef98976d7 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 19 May 2022 16:32:29 +0900 Subject: [PATCH] feat(behavior_path): use multi thread (#45) * feat(behavior_path): use multi thread 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 --- .../behavior_path_planner_node.hpp | 18 +++--- .../src/behavior_path_planner_node.cpp | 60 +++++++++++++------ 2 files changed, 51 insertions(+), 27 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 2f9f10f9941f8..769a4569b1d36 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 @@ -98,6 +98,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(); @@ -135,22 +138,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 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 planner_data); /** * @brief publish behavior module status mainly for the user interface */ - void publishModuleStatus(const std::vector> & statuses); - - /** - * @brief update current pose on the planner_data_ - */ - void updateCurrentPose(); + void publishModuleStatus( + const std::vector> & statuses, + const std::shared_ptr planner_data); // debug 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 7eac56ac9604b..2026030f00efd 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -84,6 +84,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // behavior tree manager { + mutex_bt_.lock(); + bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); auto side_shift_module = @@ -117,6 +119,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bt_manager_->registerSceneModule(pull_out_module); bt_manager_->createBehaviorTree(); + + mutex_bt_.unlock(); } // turn signal decider @@ -427,10 +431,13 @@ BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam() void BehaviorPathPlannerNode::waitForData() { // wait until mandatory data is ready + mutex_pd_.lock(); // for planner_data_ while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) { + mutex_pd_.unlock(); RCLCPP_WARN_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()) { @@ -442,26 +449,38 @@ void BehaviorPathPlannerNode::waitForData() "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_ // 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); @@ -485,7 +504,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; } @@ -496,19 +515,21 @@ void BehaviorPathPlannerNode::run() } // for remote operation - publishModuleStatus(bt_manager_->getModulesStatus()); + publishModuleStatus(bt_manager_->getModulesStatus(), planner_data); publishDebugMarker(bt_manager_->getDebugMarkers()); + 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"); @@ -516,11 +537,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.", @@ -529,7 +550,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") { @@ -561,7 +583,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) { @@ -611,28 +633,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"; @@ -645,10 +665,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);