From 7dfbd68a27cb2be7b53e4cb2e55a7cd26ba3d065 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 17 Jan 2022 23:04:13 +0900 Subject: [PATCH] make planning manager single thread --- planning/planning_manager/CMakeLists.txt | 2 +- .../planning_manager_node.hpp | 41 +++++- .../src/planning_manager_node.cpp | 130 ++++++++++++++++-- 3 files changed, 154 insertions(+), 19 deletions(-) diff --git a/planning/planning_manager/CMakeLists.txt b/planning/planning_manager/CMakeLists.txt index 9db9c69224604..5e2bc431a26cc 100644 --- a/planning/planning_manager/CMakeLists.txt +++ b/planning/planning_manager/CMakeLists.txt @@ -46,5 +46,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE # config - launch + # launch ) diff --git a/planning/planning_manager/include/planning_manager/planning_manager_node.hpp b/planning/planning_manager/include/planning_manager/planning_manager_node.hpp index 886c3f3afe61d..afc4e5c3c08d7 100644 --- a/planning/planning_manager/include/planning_manager/planning_manager_node.hpp +++ b/planning/planning_manager/include/planning_manager/planning_manager_node.hpp @@ -24,7 +24,6 @@ #include "autoware_auto_planning_msgs/msg/had_map_route.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/planning_data.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include #include @@ -32,6 +31,9 @@ #include #include +#include +#include + namespace planning_manager { using autoware_auto_planning_msgs::msg::HADMapRoute; @@ -59,6 +61,32 @@ class PlanningManagerNode : public rclcpp::Node void run(); private: + enum class Status : int { + WAITING = 0, + EXECUTING, + FINISHED, + }; + + template + struct ResultWithStatus + { + Status status = Status::WAITING; + T result; + PlanningData planning_data; + }; + + struct ModulesResult + { + ResultWithStatus behavior_path_planner; + ResultWithStatus behavior_velocity_planner; + }; + std::unordered_map modules_result_map_ = {}; + + // TODO(murooka) remove this + int current_id_ = 0; + + std::mutex mutex_; + // timer rclcpp::TimerBase::SharedPtr timer_; @@ -84,6 +112,7 @@ class PlanningManagerNode : public rclcpp::Node // NOTE: callback group for client must be member variable rclcpp::CallbackGroup::SharedPtr callback_group_services_; + rclcpp::CallbackGroup::SharedPtr callback_group_timer_; // clients rclcpp::Client::SharedPtr client_behavior_path_planner_plan_; @@ -93,13 +122,15 @@ class PlanningManagerNode : public rclcpp::Node client_behavior_velocity_planner_validate_; bool is_showing_debug_info_; + rclcpp::Time prev_plan_time_; + double planning_hz_; autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr route_; planning_manager::msg::PlanningData planning_data_; - autoware_auto_planning_msgs::msg::PathWithLaneId path_with_lane_id_; - autoware_auto_planning_msgs::msg::Path path_; - autoware_auto_planning_msgs::msg::Trajectory behavior_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory motion_trajectory_; + // autoware_auto_planning_msgs::msg::PathWithLaneId path_with_lane_id_; + // autoware_auto_planning_msgs::msg::Path path_; + // autoware_auto_planning_msgs::msg::Trajectory behavior_trajectory_; + // autoware_auto_planning_msgs::msg::Trajectory motion_trajectory_; Trajectory planTrajectory(const HADMapRoute & route); Trajectory optimizeVelocity(const Trajectory & traj); diff --git a/planning/planning_manager/src/planning_manager_node.cpp b/planning/planning_manager/src/planning_manager_node.cpp index e439288d95159..247407234a9f2 100644 --- a/planning/planning_manager/src/planning_manager_node.cpp +++ b/planning/planning_manager/src/planning_manager_node.cpp @@ -15,6 +15,7 @@ #include "planning_manager/planning_manager_node.hpp" #include +#include #include namespace @@ -56,8 +57,11 @@ PlanningManagerNode::PlanningManagerNode(const rclcpp::NodeOptions & node_option // parameter is_showing_debug_info_ = true; - const double planning_hz = + planning_hz_ = declare_parameter("planning_hz", 10.0); // TODO(murooka) remove default parameter + // TODO(murooka) 1000 Hz + const double main_hz = + declare_parameter("main_hz", 50.0); // TODO(murooka) remove default parameter // publisher traj_pub_ = this->create_publisher("~/output/trajectory", 1); @@ -83,7 +87,9 @@ PlanningManagerNode::PlanningManagerNode(const rclcpp::NodeOptions & node_option "~/input/force_approval", 1, std::bind(&PlanningManagerNode::onForceApproval, this, _1)); { - callback_group_services_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + // TODO(murooka) exclusive + callback_group_services_ = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // behavior path planner client_behavior_path_planner_plan_ = @@ -119,12 +125,15 @@ PlanningManagerNode::PlanningManagerNode(const rclcpp::NodeOptions & node_option } { // timer - const auto period = rclcpp::Rate(planning_hz).period(); + callback_group_timer_ = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + const auto period = rclcpp::Rate(main_hz).period(); auto on_timer = std::bind(&PlanningManagerNode::run, this); timer_ = std::make_shared>( this->get_clock(), period, std::move(on_timer), this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); + this->get_node_timers_interface()->add_timer(timer_, callback_group_timer_); } } @@ -176,12 +185,105 @@ void PlanningManagerNode::run() const auto traj_with_optimal_vel = optimizeVelocity(traj); - validateTrajectory(traj_with_optimal_vel); + // validateTrajectory(traj_with_optimal_vel); publishTrajectory(traj_with_optimal_vel); publishDiagnostics(); } +Trajectory PlanningManagerNode::planTrajectory(const HADMapRoute & route) +{ + RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start planTrajectory"); + if ((rclcpp::Clock().now() - prev_plan_time_).seconds() > 1.0 / planning_hz_) { + prev_plan_time_ = rclcpp::Clock().now(); + + // TODO(murooka) + current_id_++; + const int unique_id = current_id_; + modules_result_map_[unique_id] = ModulesResult(); + modules_result_map_[unique_id].behavior_path_planner.status = Status::WAITING; + + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, "%d: start new planning", unique_id); + + if (current_id_ == 10000) { + current_id_ = 0; + } + } + + for (auto itr = modules_result_map_.begin(); itr != modules_result_map_.end(); ++itr) { + const int id = itr->first; + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, "%d / %ld", id, modules_result_map_.size()); + + // behavior path planner + if (itr->second.behavior_path_planner.status == Status::WAITING) { + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, "%d: start BehaviorPathPlannerPlan", id); + itr->second.behavior_path_planner.status = Status::EXECUTING; + + auto behavior_path_request = + std::make_shared(); + behavior_path_request->route = route; + behavior_path_request->planning_data = planning_data_; + + client_behavior_path_planner_plan_->async_send_request( + behavior_path_request, + [this, itr, id](rclcpp::Client::SharedFuture future) { + // std::lock_guard lock(mutex_); + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, "%d: get BehaviorPathPlannerPlan", id); + const auto response = future.get(); + itr->second.behavior_path_planner.result = response->path_with_lane_id; + itr->second.behavior_path_planner.status = Status::FINISHED; + itr->second.behavior_velocity_planner.status = Status::WAITING; + }); + } + + // behavior velocity planner + if ( + itr->second.behavior_path_planner.status == Status::FINISHED && + itr->second.behavior_velocity_planner.status == Status::WAITING) { + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, "%d: start BehaviorVelocityPlannerPlan", id); + itr->second.behavior_velocity_planner.status = Status::EXECUTING; + + auto behavior_velocity_request = + std::make_shared(); + behavior_velocity_request->path_with_lane_id = itr->second.behavior_path_planner.result; + behavior_velocity_request->planning_data = planning_data_; + + client_behavior_velocity_planner_plan_->async_send_request( + behavior_velocity_request, + [this, itr, id](rclcpp::Client::SharedFuture future) { + // std::lock_guard lock(mutex_); + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, "%d: get BehaviorVelocityPlannerPlan", id); + const auto response = future.get(); + itr->second.behavior_velocity_planner.result = response->path; + itr->second.behavior_velocity_planner.status = Status::FINISHED; + }); + } + } + + { // TODO(murooka) use remove_if + auto itr = modules_result_map_.begin(); + while (itr != modules_result_map_.end()) { + if (itr->second.behavior_velocity_planner.status == Status::FINISHED) { + const int id = itr->first; + + RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "%d: remove planning", id); + itr = modules_result_map_.erase(itr); + } else { + itr++; + } + } + } + + return Trajectory{}; +} + +/* Trajectory PlanningManagerNode::planTrajectory(const HADMapRoute & route) { RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start planTrajectory"); @@ -221,36 +323,38 @@ Trajectory PlanningManagerNode::planTrajectory(const HADMapRoute & route) return Trajectory{}; } +*/ // TODO(murooka) optimize velocity Trajectory PlanningManagerNode::optimizeVelocity([[maybe_unused]] const Trajectory & traj) { - RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start optimizeVelocity"); + // RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start optimizeVelocity"); return Trajectory{}; } // TODO(murooka) validate void PlanningManagerNode::validateTrajectory([[maybe_unused]] const Trajectory & traj) { - RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start validateTrajectory"); + // RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start validateTrajectory"); // behavior path planner - RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start BehaviorPathPlannerValidate"); + // RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start + // BehaviorPathPlannerValidate"); auto behavior_path_request = std::make_shared(); - behavior_path_request->trajectory = motion_trajectory_; + behavior_path_request->trajectory = Trajectory{}; // motion_trajectory_; const auto behavior_path_planner_validate_result = sendRequest( client_behavior_path_planner_validate_, behavior_path_request); // behavior velocity planner - RCLCPP_INFO_EXPRESSION( - get_logger(), is_showing_debug_info_, "start BehaviorVelocityPlannerValidate"); + // RCLCPP_INFO_EXPRESSION( + // get_logger(), is_showing_debug_info_, "start BehaviorVelocityPlannerValidate"); auto behavior_velocity_request = std::make_shared(); - behavior_velocity_request->trajectory = motion_trajectory_; + behavior_velocity_request->trajectory = Trajectory{}; // motion_trajectory_; const auto behavior_velocity_planner_validate_result = sendRequest( @@ -260,7 +364,7 @@ void PlanningManagerNode::validateTrajectory([[maybe_unused]] const Trajectory & void PlanningManagerNode::publishTrajectory(const Trajectory & traj) { // TODO(murooka) use thread local for member variable? - RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start publishTrajectory"); + // RCLCPP_INFO_EXPRESSION(get_logger(), is_showing_debug_info_, "start publishTrajectory"); traj_pub_->publish(traj); } void PlanningManagerNode::publishDiagnostics() {}