Skip to content

Commit

Permalink
fix(goal_planner): use recursive mutex instead of transaction (#5379)
Browse files Browse the repository at this point in the history 
Revert "use transaction instead of recursive_mutex"

This reverts commit 654fa8c.
  • Loading branch information
@kosuke55
kosuke55 authored Oct 23, 2023
1 parent 3d2a7f6 commit 0938f5b
Show file tree
Hide file tree
Showing 2 changed files with 36 additions and 70 deletions.
57 changes: 17 additions & 40 deletions ...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::mutex> lock(mutex_); \
NAME##_ = value; \
} else { \
NAME##_ = value; \
} \
} \
\
TYPE get_##NAME() const \
{ \
if (!is_in_transaction_) { \
const std::lock_guard<std::mutex> lock(mutex_); \
return NAME##_; \
} \
return NAME##_; \
#define DEFINE_SETTER_GETTER(TYPE, NAME) \
public: \
void set_##NAME(const TYPE & value) \
{ \
const std::lock_guard<std::recursive_mutex> lock(mutex_); \
NAME##_ = value; \
} \
\
TYPE get_##NAME() const \
{ \
const std::lock_guard<std::recursive_mutex> 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<std::mutex> lock(mutex_);
std::lock_guard<std::recursive_mutex> lock(mutex_);
pull_over_path_ = nullptr;
lane_parking_pull_over_path_ = nullptr;
current_path_idx_ = 0;
Expand All @@ -136,9 +118,6 @@ class PullOverStatus
is_ready_ = false;
}

// lock all data members
Transaction startTransaction() { return Transaction(*this); }

DEFINE_SETTER_GETTER(std::shared_ptr<PullOverPath>, pull_over_path)
DEFINE_SETTER_GETTER(std::shared_ptr<PullOverPath>, lane_parking_pull_over_path)
DEFINE_SETTER_GETTER(size_t, current_path_idx)
Expand Down Expand Up @@ -200,9 +179,7 @@ class PullOverStatus
std::vector<PullOverPath> pull_over_path_candidates_;
std::optional<Pose> closest_start_pose_;

friend class Transaction;
mutable std::mutex mutex_;
bool is_in_transaction_{false};
std::recursive_mutex & mutex_;
};

#undef DEFINE_SETTER_GETTER
Expand Down Expand Up @@ -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.
Expand Down
49 changes: 19 additions & 30 deletions planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<GoalPlannerParameters> & parameters,
const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & 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_);
Expand Down Expand Up @@ -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<std::recursive_mutex> lock(mutex_);
status_.set_pull_over_path_candidates(path_candidates);
status_.set_closest_start_pose(closest_start_pose);
}
}

void GoalPlannerModule::onFreespaceParkingTimer()
Expand Down Expand Up @@ -447,15 +438,15 @@ bool GoalPlannerModule::planFreespacePath()
status_.set_goal_candidates(goal_candidates);

{
const std::lock_guard<std::mutex> lock(mutex_);
const std::lock_guard<std::recursive_mutex> lock(mutex_);
debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size();
debug_data_.freespace_planner.is_planning = true;
}

for (size_t i = 0; i < goal_candidates.size(); i++) {
const auto goal_candidate = goal_candidates.at(i);
{
const std::lock_guard<std::mutex> lock(mutex_);
const std::lock_guard<std::recursive_mutex> lock(mutex_);
debug_data_.freespace_planner.current_goal_idx = i;
}

Expand All @@ -470,20 +461,19 @@ bool GoalPlannerModule::planFreespacePath()
}

{
const auto transaction = status_.startTransaction();
const std::lock_guard<std::recursive_mutex> lock(mutex_);
status_.set_pull_over_path(std::make_shared<PullOverPath>(*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<rclcpp::Time>(clock_->now()));
const std::lock_guard<std::mutex> lock(mutex_);
debug_data_.freespace_planner.is_planning = false;
}

return true;
}

const std::lock_guard<std::mutex> lock(mutex_);
const std::lock_guard<std::recursive_mutex> lock(mutex_);
debug_data_.freespace_planner.is_planning = false;
return false;
}
Expand Down Expand Up @@ -513,7 +503,7 @@ void GoalPlannerModule::returnToLaneParking()
}

{
const auto transaction = status_.startTransaction();
const std::lock_guard<std::recursive_mutex> 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());
Expand Down Expand Up @@ -609,7 +599,7 @@ void GoalPlannerModule::selectSafePullOverPath()
std::vector<PullOverPath> pull_over_path_candidates{};
GoalCandidates goal_candidates{};
{
const auto transaction = status_.startTransaction();
const std::lock_guard<std::recursive_mutex> lock(mutex_);
goal_searcher_->setPlannerData(planner_data_);
goal_candidates = status_.get_goal_candidates();
goal_searcher_->update(goal_candidates);
Expand Down Expand Up @@ -640,7 +630,7 @@ void GoalPlannerModule::selectSafePullOverPath()

// found safe pull over path
{
const auto transaction = status_.startTransaction();
const std::lock_guard<std::recursive_mutex> lock(mutex_);
status_.set_is_safe_static_objects(true);
status_.set_pull_over_path(std::make_shared<PullOverPath>(pull_over_path));
status_.set_current_path_idx(0);
Expand Down Expand Up @@ -681,7 +671,7 @@ void GoalPlannerModule::selectSafePullOverPath()

void GoalPlannerModule::setLanes()
{
const auto transaction = status_.startTransaction();
const std::lock_guard<std::recursive_mutex> lock(mutex_);
status_.set_current_lanes(utils::getExtendedCurrentLanes(
planner_data_, parameters_->backward_goal_search_length,
parameters_->forward_goal_search_length,
Expand Down Expand Up @@ -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<std::recursive_mutex> 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);
Expand All @@ -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<PathWithLaneId>(generateStopPath());
const auto transaction = status_.startTransaction();
const std::lock_guard<std::recursive_mutex> lock(mutex_);
status_.set_prev_stop_path(output.path);
// set stop path as pull over path
auto stop_pull_over_path = std::make_shared<PullOverPath>();
Expand Down Expand Up @@ -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<std::recursive_mutex> lock(mutex_);
status_.set_last_approved_time(std::make_shared<rclcpp::Time>(clock_->now()));
status_.set_last_approved_pose(
std::make_shared<Pose>(planner_data_->self_odometry->pose.pose));
Expand Down Expand Up @@ -1226,7 +1216,7 @@ bool GoalPlannerModule::isStopped(

bool GoalPlannerModule::isStopped()
{
const std::lock_guard<std::mutex> lock(mutex_);
const std::lock_guard<std::recursive_mutex> lock(mutex_);
return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time);
}

Expand All @@ -1236,7 +1226,6 @@ bool GoalPlannerModule::isStuck()
return false;
}

const std::lock_guard<std::mutex> lock(mutex_);
constexpr double stuck_time = 5.0;
if (!isStopped(odometry_buffer_stuck_, stuck_time)) {
return false;
Expand Down

0 comments on commit 0938f5b

Please sign in to comment.