Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(goal_planner): use recursive mutex instead of transaction #5379

Merged
merged 2 commits into from
Oct 23, 2023
Merged

fix(goal_planner): use recursive mutex instead of transaction #5379

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
386b627
Revert "use transaction instead of recursive_mutex"
kosuke55 Oct 23, 2023
e37bd12
Merge branch 'main' into fix/goal_planner_recursive_mutex
kosuke55 Oct 23, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
59 changes: 18 additions & 41 deletions ...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,47 +76,29 @@
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 @@
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 @@ -168,7 +147,7 @@

void push_goal_candidate(const GoalCandidate & goal_candidate)
{
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard<std::recursive_mutex> lock(mutex_);

Check warning on line 150 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L150 

Added line #L150 was not covered by tests
goal_candidates_.push_back(goal_candidate);
}

Expand Down Expand Up @@ -206,9 +185,7 @@
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 @@ -300,7 +277,7 @@

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
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Lines of Code in a Single File 

The lines of code decreases from 1375 to 1365, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 in planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity 

The mean cyclomatic complexity increases from 4.22 to 4.33, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -50,25 +50,14 @@

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 @@ -198,9 +187,11 @@
}

// 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 @@ -461,15 +452,15 @@
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 @@ -484,20 +475,19 @@
}

{
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 @@ -527,7 +517,7 @@
}

{
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 @@ -621,7 +611,7 @@
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 @@ -652,7 +642,7 @@

// 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 @@ -693,7 +683,7 @@

void GoalPlannerModule::setLanes()
{
const auto transaction = status_.startTransaction();
const std::lock_guard<std::recursive_mutex> lock(mutex_);

Check warning on line 686 in planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp#L686 

Added line #L686 was not covered by tests
status_.set_current_lanes(utils::getExtendedCurrentLanes(
planner_data_, parameters_->backward_goal_search_length,
parameters_->forward_goal_search_length,
Expand Down Expand Up @@ -743,7 +733,7 @@

// 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_);

Check warning on line 736 in planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp#L736 

Added line #L736 was not covered by tests
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 @@ -754,7 +744,7 @@
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_);

Check warning on line 747 in planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp#L747 

Added line #L747 was not covered by tests
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 @@ -922,7 +912,7 @@
if (status_.get_has_decided_path()) {
if (isActivated() && isWaitingApproval()) {
{
const auto transaction = status_.startTransaction();
const std::lock_guard<std::recursive_mutex> lock(mutex_);

Check warning on line 915 in planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp#L915 

Added line #L915 was not covered by tests
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 @@ -1235,7 +1225,7 @@

bool GoalPlannerModule::isStopped()
{
const std::lock_guard<std::mutex> lock(mutex_);
const std::lock_guard<std::recursive_mutex> lock(mutex_);

Check warning on line 1228 in planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp#L1228 

Added line #L1228 was not covered by tests
return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time);
}

Expand All @@ -1245,7 +1235,6 @@
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
Loading