diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 34ffb61dcd8d5..21372ed6e2c2f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -76,47 +76,29 @@ enum class PathType { FREESPACE, }; -class PullOverStatus; // Forward declaration for Transaction -// class that locks the PullOverStatus when multiple values are being updated from -// an external source. -class Transaction -{ -public: - explicit Transaction(PullOverStatus & status); - ~Transaction(); - -private: - PullOverStatus & status_; -}; - -#define DEFINE_SETTER_GETTER(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - if (!is_in_transaction_) { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } else { \ - NAME##_ = value; \ - } \ - } \ - \ - TYPE get_##NAME() const \ - { \ - if (!is_in_transaction_) { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ - } \ - return NAME##_; \ +#define DEFINE_SETTER_GETTER(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } \ + \ + TYPE get_##NAME() const \ + { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ } class PullOverStatus { public: + explicit PullOverStatus(std::recursive_mutex & mutex) : mutex_(mutex) {} + // Reset all data members to their initial states void reset() { - const std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); pull_over_path_ = nullptr; lane_parking_pull_over_path_ = nullptr; current_path_idx_ = 0; @@ -136,9 +118,6 @@ class PullOverStatus is_ready_ = false; } - // lock all data members - Transaction startTransaction() { return Transaction(*this); } - DEFINE_SETTER_GETTER(std::shared_ptr, pull_over_path) DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) DEFINE_SETTER_GETTER(size_t, current_path_idx) @@ -200,9 +179,7 @@ class PullOverStatus std::vector pull_over_path_candidates_; std::optional closest_start_pose_; - friend class Transaction; - mutable std::mutex mutex_; - bool is_in_transaction_{false}; + std::recursive_mutex & mutex_; }; #undef DEFINE_SETTER_GETTER @@ -291,7 +268,7 @@ class GoalPlannerModule : public SceneModuleInterface tier4_autoware_utils::LinearRing2d vehicle_footprint_; - std::mutex mutex_; + std::recursive_mutex mutex_; PullOverStatus status_; // approximate distance from the start point to the end point of pull_over. diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index e71b1ac7936be..95ac47494d016 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -50,25 +50,14 @@ using tier4_autoware_utils::inverseTransformPose; namespace behavior_path_planner { -Transaction::Transaction(PullOverStatus & status) : status_(status) -{ - status_.mutex_.lock(); - status_.is_in_transaction_ = true; -} - -Transaction::~Transaction() -{ - status_.mutex_.unlock(); - status_.is_in_transaction_ = false; -} - GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + status_{mutex_} { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); @@ -205,9 +194,11 @@ void GoalPlannerModule::onTimer() } // set member variables - const auto transaction = status_.startTransaction(); - status_.set_pull_over_path_candidates(path_candidates); - status_.set_closest_start_pose(closest_start_pose); + { + const std::lock_guard lock(mutex_); + status_.set_pull_over_path_candidates(path_candidates); + status_.set_closest_start_pose(closest_start_pose); + } } void GoalPlannerModule::onFreespaceParkingTimer() @@ -447,7 +438,7 @@ bool GoalPlannerModule::planFreespacePath() status_.set_goal_candidates(goal_candidates); { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); debug_data_.freespace_planner.is_planning = true; } @@ -455,7 +446,7 @@ bool GoalPlannerModule::planFreespacePath() for (size_t i = 0; i < goal_candidates.size(); i++) { const auto goal_candidate = goal_candidates.at(i); { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); debug_data_.freespace_planner.current_goal_idx = i; } @@ -470,20 +461,19 @@ bool GoalPlannerModule::planFreespacePath() } { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_pull_over_path(std::make_shared(*freespace_path)); status_.set_current_path_idx(0); status_.set_is_safe_static_objects(true); status_.set_modified_goal_pose(goal_candidate); status_.set_last_path_update_time(std::make_shared(clock_->now())); - const std::lock_guard lock(mutex_); debug_data_.freespace_planner.is_planning = false; } return true; } - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); debug_data_.freespace_planner.is_planning = false; return false; } @@ -513,7 +503,7 @@ void GoalPlannerModule::returnToLaneParking() } { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_is_safe_static_objects(true); status_.set_has_decided_path(false); status_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); @@ -609,7 +599,7 @@ void GoalPlannerModule::selectSafePullOverPath() std::vector pull_over_path_candidates{}; GoalCandidates goal_candidates{}; { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); goal_searcher_->setPlannerData(planner_data_); goal_candidates = status_.get_goal_candidates(); goal_searcher_->update(goal_candidates); @@ -640,7 +630,7 @@ void GoalPlannerModule::selectSafePullOverPath() // found safe pull over path { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_is_safe_static_objects(true); status_.set_pull_over_path(std::make_shared(pull_over_path)); status_.set_current_path_idx(0); @@ -681,7 +671,7 @@ void GoalPlannerModule::selectSafePullOverPath() void GoalPlannerModule::setLanes() { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_current_lanes(utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, @@ -731,7 +721,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_prev_is_safe(status_.get_is_safe_static_objects()); status_.set_prev_is_safe_dynamic_objects( parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); @@ -742,7 +732,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) if (status_.get_prev_is_safe() || !status_.get_prev_stop_path()) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_prev_stop_path(output.path); // set stop path as pull over path auto stop_pull_over_path = std::make_shared(); @@ -916,7 +906,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() if (status_.get_has_decided_path()) { if (isActivated() && isWaitingApproval()) { { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_last_approved_time(std::make_shared(clock_->now())); status_.set_last_approved_pose( std::make_shared(planner_data_->self_odometry->pose.pose)); @@ -1226,7 +1216,7 @@ bool GoalPlannerModule::isStopped( bool GoalPlannerModule::isStopped() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); } @@ -1236,7 +1226,6 @@ bool GoalPlannerModule::isStuck() return false; } - const std::lock_guard lock(mutex_); constexpr double stuck_time = 5.0; if (!isStopped(odometry_buffer_stuck_, stuck_time)) { return false;