From fe4ac6d6be374c34a4d9aab2f8d69b7133b83801 Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 15 Dec 2022 23:57:08 +0900 Subject: [PATCH 1/7] feat(behavior_path_planner): change planner data update method Signed-off-by: yutaka --- .../behavior_path_planner_node.hpp | 9 ++- .../src/behavior_path_planner_node.cpp | 64 ++++++++++--------- 2 files changed, 42 insertions(+), 31 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 48ac09b77e803..0ac8608e8b1b1 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 @@ -110,14 +110,21 @@ class BehaviorPathPlannerNode : public rclcpp::Node tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; Scenario::SharedPtr current_scenario_{nullptr}; + HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + LaneletRoute::ConstSharedPtr route_ptr_{nullptr}; + bool has_received_map_{false}; + bool has_received_route_{false}; + TurnSignalDecider turn_signal_decider_; - std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_bt_; // mutex for bt_manager_ // setup bool isDataReady(); + // update planner data + void updatePlannerData(); + // parameters std::shared_ptr avoidance_param_ptr; std::shared_ptr lane_change_param_ptr; 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 4da5933afa7af..a1df59cac0c98 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -564,19 +564,22 @@ bool BehaviorPathPlannerNode::isDataReady() { const auto missing = [this](const auto & name) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", name); - mutex_pd_.unlock(); return false; }; - mutex_pd_.lock(); // for planner_data_ if (!current_scenario_) { return missing("scenario_topic"); } - if (!planner_data_->route_handler->isHandlerReady()) { + // if (!planner_data_->route_handler->isHandlerReady()) { + if (!route_ptr_) { return missing("route"); } + if (!map_ptr_) { + return missing("map"); + } + if (!planner_data_->dynamic_object) { return missing("dynamic_object"); } @@ -594,10 +597,31 @@ bool BehaviorPathPlannerNode::isDataReady() return missing("self_pose"); } - mutex_pd_.unlock(); return true; } +void BehaviorPathPlannerNode::updatePlannerData() +{ + // update planner data (pose, map and route) + planner_data_->self_pose = self_pose_listener_.getCurrentPose(); + if (has_received_map_) { + planner_data_->route_handler->setMap(*map_ptr_); + has_received_map_ = false; + } + + const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); + if (has_received_route_) { + planner_data_->route_handler->setRoute(*route_ptr_); + has_received_route_ = false; + } + // Reset behavior tree when new route is received, + // so that the each modules do not have to care about the "route jump". + if (!is_first_time) { + RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); + bt_manager_->resetBehaviorTree(); + } +} + void BehaviorPathPlannerNode::run() { if (!isDataReady()) { @@ -606,23 +630,18 @@ 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) { mutex_bt_.unlock(); // for bt_manager_ - mutex_pd_.unlock(); // for planner_data_ return; } - // update planner data - planner_data_->self_pose = self_pose_listener_.getCurrentPose(); + updatePlannerData(); + // create local planner data const auto planner_data = std::make_shared(*planner_data_); - // unlock planner data - mutex_pd_.unlock(); - // run behavior planner const auto output = bt_manager_->run(planner_data); @@ -844,34 +863,28 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection( void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); planner_data_->self_odometry = msg; } void BehaviorPathPlannerNode::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); planner_data_->self_acceleration = msg; } void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); planner_data_->dynamic_object = msg; } void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); planner_data_->occupancy_grid = 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"; @@ -884,22 +897,13 @@ 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); + map_ptr_ = msg; + has_received_map_ = true; } void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) { - std::lock_guard lock(mutex_pd_); - const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); - - planner_data_->route_handler->setRoute(*msg); - - // Reset behavior tree when new route is received, - // so that the each modules do not have to care about the "route jump". - if (!is_first_time) { - RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); - bt_manager_->resetBehaviorTree(); - } + route_ptr_ = msg; + has_received_route_ = true; } SetParametersResult BehaviorPathPlannerNode::onSetParam( From 8f8323e302d0908cf379b623e2943677c91b92c5 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 16 Dec 2022 00:42:28 +0900 Subject: [PATCH 2/7] remove unnecessary code Signed-off-by: yutaka --- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 1 - 1 file changed, 1 deletion(-) 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 a1df59cac0c98..4890985094a97 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -571,7 +571,6 @@ bool BehaviorPathPlannerNode::isDataReady() return missing("scenario_topic"); } - // if (!planner_data_->route_handler->isHandlerReady()) { if (!route_ptr_) { return missing("route"); } From e3cdd4ad61e915b37fed1616b891e69fb032430c Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 16 Dec 2022 02:11:04 +0900 Subject: [PATCH 3/7] update Signed-off-by: yutaka --- .../src/behavior_path_planner_node.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) 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 4890985094a97..6ad9ff4f2839d 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -611,14 +611,15 @@ void BehaviorPathPlannerNode::updatePlannerData() const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); if (has_received_route_) { planner_data_->route_handler->setRoute(*route_ptr_); + // Reset behavior tree when new route is received, + // so that the each modules do not have to care about the "route jump". + if (!is_first_time) { + RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); + bt_manager_->resetBehaviorTree(); + } + has_received_route_ = false; } - // Reset behavior tree when new route is received, - // so that the each modules do not have to care about the "route jump". - if (!is_first_time) { - RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree."); - bt_manager_->resetBehaviorTree(); - } } void BehaviorPathPlannerNode::run() From 59d0abba02a9332d16e5fda0e72bdfc39807d5e5 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 16 Dec 2022 18:20:21 +0900 Subject: [PATCH 4/7] add mutex Signed-off-by: yutaka --- .../behavior_path_planner/behavior_path_planner_node.hpp | 1 + .../src/behavior_path_planner_node.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) 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 0ac8608e8b1b1..651d6ebcb0d00 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 @@ -118,6 +118,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node TurnSignalDecider turn_signal_decider_; std::mutex mutex_bt_; // mutex for bt_manager_ + std::mutex mutex_pd_; // mutex for planer_data_ // setup bool isDataReady(); 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 6ad9ff4f2839d..64ca83cc2bdeb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -601,13 +601,18 @@ bool BehaviorPathPlannerNode::isDataReady() void BehaviorPathPlannerNode::updatePlannerData() { - // update planner data (pose, map and route) + const std::lock_guard lock(mutex_pd_); + + // update self planner_data_->self_pose = self_pose_listener_.getCurrentPose(); + + // update map if (has_received_map_) { planner_data_->route_handler->setMap(*map_ptr_); has_received_map_ = false; } + // update route const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); if (has_received_route_) { planner_data_->route_handler->setRoute(*route_ptr_); From 711855dcbbaa85ea7a241bacf70d9dd5b42eadc3 Mon Sep 17 00:00:00 2001 From: yutaka Date: Mon, 19 Dec 2022 16:56:35 +0900 Subject: [PATCH 5/7] update Signed-off-by: yutaka --- .../behavior_path_planner/behavior_path_planner_node.hpp | 2 +- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) 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 651d6ebcb0d00..8afb2244e89db 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 @@ -117,8 +117,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node TurnSignalDecider turn_signal_decider_; - std::mutex mutex_bt_; // mutex for bt_manager_ std::mutex mutex_pd_; // mutex for planer_data_ + std::mutex mutex_bt_; // mutex for bt_manager_ // setup bool isDataReady(); 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 dcf4f7fd3603e..943d50eb2a911 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -897,11 +897,13 @@ void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstShare } void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) { + const std::lock_guard lock(mutex_pd_); map_ptr_ = msg; has_received_map_ = true; } void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) { + const std::lock_guard lock(mutex_pd_); route_ptr_ = msg; has_received_route_ = true; } From 7de7d6b4a206ef92ca354de99d01587f1212a235 Mon Sep 17 00:00:00 2001 From: yutaka Date: Mon, 19 Dec 2022 19:20:01 +0900 Subject: [PATCH 6/7] fix comment Signed-off-by: yutaka --- .../behavior_path_planner/behavior_path_planner_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 010a240446253..c792352a131c1 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 @@ -119,7 +119,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node TurnSignalDecider turn_signal_decider_; - std::mutex mutex_pd_; // mutex for planer_data_ + std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_bt_; // mutex for bt_manager_ // setup From 3677707980ebe37e0e88a102bff97f8c9324da66 Mon Sep 17 00:00:00 2001 From: yutaka Date: Mon, 19 Dec 2022 20:43:57 +0900 Subject: [PATCH 7/7] add mutex Signed-off-by: yutaka --- .../behavior_path_planner_node.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 16 +++++++++++----- 2 files changed, 12 insertions(+), 6 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 c792352a131c1..dc692f94dc02b 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 @@ -126,7 +126,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool isDataReady(); // update planner data - void updatePlannerData(); + std::shared_ptr createLatestPlannerData(); // parameters std::shared_ptr avoidance_param_ptr; 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 0ddd197153243..75615a20935da 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -602,7 +602,7 @@ bool BehaviorPathPlannerNode::isDataReady() return true; } -void BehaviorPathPlannerNode::updatePlannerData() +std::shared_ptr BehaviorPathPlannerNode::createLatestPlannerData() { const std::lock_guard lock(mutex_pd_); @@ -628,6 +628,8 @@ void BehaviorPathPlannerNode::updatePlannerData() has_received_route_ = false; } + + return std::make_shared(*planner_data_); } void BehaviorPathPlannerNode::run() @@ -645,10 +647,8 @@ void BehaviorPathPlannerNode::run() return; } - updatePlannerData(); - - // create local planner data - const auto planner_data = std::make_shared(*planner_data_); + // create latest planner data + const auto planner_data = createLatestPlannerData(); // run behavior planner const auto output = bt_manager_->run(planner_data); @@ -863,28 +863,34 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection( void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg) { + const std::lock_guard lock(mutex_pd_); planner_data_->self_odometry = msg; } void BehaviorPathPlannerNode::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) { + const std::lock_guard lock(mutex_pd_); planner_data_->self_acceleration = msg; } void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg) { + const std::lock_guard lock(mutex_pd_); planner_data_->dynamic_object = msg; } void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg) { + const std::lock_guard lock(mutex_pd_); planner_data_->occupancy_grid = msg; } void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg) { + const 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) { + const std::lock_guard lock(mutex_pd_); auto getModuleName = [](PathChangeModuleId module) { if (module.type == PathChangeModuleId::FORCE_LANE_CHANGE) { return "ForceLaneChange";