Skip to content

Commit

Permalink
fix(goal_planner): mutex lock for all getter and setter of status (au…
Browse files Browse the repository at this point in the history 
…towarefoundation#5202)

* fix(goal_planner): mutex lock for all getter and setter of status

Signed-off-by: kosuke55 <[email protected]>

* use transaction instead of recursive_mutex

Signed-off-by: kosuke55 <[email protected]>

fix

Signed-off-by: kosuke55 <[email protected]>

* fix increment

Signed-off-by: kosuke55 <[email protected]>

---------

Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
@kosuke55
kosuke55 authored Oct 19, 2023
1 parent 2ad1d81 commit b019643
Show file tree
Hide file tree
Showing 4 changed files with 396 additions and 259 deletions.
175 changes: 138 additions & 37 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 @@ -74,28 +74,143 @@ enum class PathType {
FREESPACE,
};

struct PullOverStatus
class PullOverStatus; // Forward declaration for Transaction
// class that locks the PullOverStatus when multiple values are being updated from
// an external source.
class Transaction
{
std::shared_ptr<PullOverPath> pull_over_path{};
std::shared_ptr<PullOverPath> lane_parking_pull_over_path{};
size_t current_path_idx{0};
bool require_increment_{true}; // if false, keep current path idx.
std::shared_ptr<PathWithLaneId> prev_stop_path{nullptr};
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr};
// stop path after approval, stop path is not updated until safety is confirmed
lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain
lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain
std::vector<DrivableLanes> lanes{}; // current + pull_over
bool has_decided_path{false}; // if true, the path has is decided and safe against static objects
bool is_safe_static_objects{false}; // current path is safe against *static* objects
bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects
bool prev_is_safe{false};
bool prev_is_safe_dynamic_objects{false};
bool has_decided_velocity{false};
bool has_requested_approval{false};
bool is_ready{false};
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##_; \
}

class PullOverStatus
{
public:
// Reset all data members to their initial states
void reset()
{
const std::lock_guard<std::mutex> lock(mutex_);
pull_over_path_ = nullptr;
lane_parking_pull_over_path_ = nullptr;
current_path_idx_ = 0;
require_increment_ = true;
prev_stop_path_ = nullptr;
prev_stop_path_after_approval_ = nullptr;
current_lanes_.clear();
pull_over_lanes_.clear();
lanes_.clear();
has_decided_path_ = false;
is_safe_static_objects_ = false;
is_safe_dynamic_objects_ = false;
prev_is_safe_ = false;
prev_is_safe_dynamic_objects_ = false;
has_decided_velocity_ = false;
has_requested_approval_ = false;
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)
DEFINE_SETTER_GETTER(bool, require_increment)
DEFINE_SETTER_GETTER(std::shared_ptr<PathWithLaneId>, prev_stop_path)
DEFINE_SETTER_GETTER(std::shared_ptr<PathWithLaneId>, prev_stop_path_after_approval)
DEFINE_SETTER_GETTER(lanelet::ConstLanelets, current_lanes)
DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes)
DEFINE_SETTER_GETTER(std::vector<DrivableLanes>, lanes)
DEFINE_SETTER_GETTER(bool, has_decided_path)
DEFINE_SETTER_GETTER(bool, is_safe_static_objects)
DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects)
DEFINE_SETTER_GETTER(bool, prev_is_safe)
DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects)
DEFINE_SETTER_GETTER(bool, has_decided_velocity)
DEFINE_SETTER_GETTER(bool, has_requested_approval)
DEFINE_SETTER_GETTER(bool, is_ready)
DEFINE_SETTER_GETTER(std::shared_ptr<rclcpp::Time>, last_approved_time)
DEFINE_SETTER_GETTER(std::shared_ptr<rclcpp::Time>, last_increment_time)
DEFINE_SETTER_GETTER(std::shared_ptr<rclcpp::Time>, last_path_update_time)
DEFINE_SETTER_GETTER(std::shared_ptr<Pose>, last_approved_pose)
DEFINE_SETTER_GETTER(std::optional<GoalCandidate>, modified_goal_pose)
DEFINE_SETTER_GETTER(Pose, refined_goal_pose)
DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates)
DEFINE_SETTER_GETTER(std::vector<PullOverPath>, pull_over_path_candidates)
DEFINE_SETTER_GETTER(std::optional<Pose>, closest_start_pose)

void push_goal_candidate(const GoalCandidate & goal_candidate)
{
std::lock_guard<std::mutex> lock(mutex_);
goal_candidates_.push_back(goal_candidate);
}

private:
std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
std::shared_ptr<PullOverPath> lane_parking_pull_over_path_{nullptr};
size_t current_path_idx_{0};
bool require_increment_{true};
std::shared_ptr<PathWithLaneId> prev_stop_path_{nullptr};
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval_{nullptr};
lanelet::ConstLanelets current_lanes_{};
lanelet::ConstLanelets pull_over_lanes_{};
std::vector<DrivableLanes> lanes_{};
bool has_decided_path_{false};
bool is_safe_static_objects_{false};
bool is_safe_dynamic_objects_{false};
bool prev_is_safe_{false};
bool prev_is_safe_dynamic_objects_{false};
bool has_decided_velocity_{false};
bool has_requested_approval_{false};
bool is_ready_{false};

// save last time and pose
std::shared_ptr<rclcpp::Time> last_approved_time_;
std::shared_ptr<rclcpp::Time> last_increment_time_;
std::shared_ptr<rclcpp::Time> last_path_update_time_;
std::shared_ptr<Pose> last_approved_pose_;

// goal modification
std::optional<GoalCandidate> modified_goal_pose_;
Pose refined_goal_pose_{};
GoalCandidates goal_candidates_{};

// pull over path
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};
};

#undef DEFINE_SETTER_GETTER

struct FreespacePlannerDebugData
{
bool is_planning{false};
Expand Down Expand Up @@ -155,8 +270,6 @@ class GoalPlannerModule : public SceneModuleInterface

bool canTransitIdleToRunningState() override { return false; }

PullOverStatus status_;

mutable StartGoalPlannerData goal_planner_data_;

std::shared_ptr<GoalPlannerParameters> parameters_;
Expand All @@ -174,29 +287,19 @@ class GoalPlannerModule : public SceneModuleInterface

// goal searcher
std::shared_ptr<GoalSearcherBase> goal_searcher_;
std::optional<GoalCandidate> modified_goal_pose_;
Pose refined_goal_pose_{};
GoalCandidates goal_candidates_{};

// collision detector
// need to be shared_ptr to be used in planner and goal searcher
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_;

// pull over path
std::vector<PullOverPath> pull_over_path_candidates_;
std::optional<Pose> closest_start_pose_;

// check stopped and stuck state
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stopped_;
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stuck_;

tier4_autoware_utils::LinearRing2d vehicle_footprint_;

// save last time and pose
std::unique_ptr<rclcpp::Time> last_approved_time_;
std::unique_ptr<rclcpp::Time> last_increment_time_;
std::unique_ptr<rclcpp::Time> last_path_update_time_;
std::unique_ptr<Pose> last_approved_pose_;
std::mutex mutex_;
PullOverStatus status_;

// approximate distance from the start point to the end point of pull_over.
// this is used as an assumed value to decelerate, etc., before generating the actual path.
Expand All @@ -210,7 +313,6 @@ class GoalPlannerModule : public SceneModuleInterface
// pre-generate lane parking paths in a separate thread
rclcpp::TimerBase::SharedPtr lane_parking_timer_;
rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
std::mutex mutex_;

// generate freespace parking paths in a separate thread
rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
Expand Down Expand Up @@ -245,7 +347,6 @@ class GoalPlannerModule : public SceneModuleInterface
bool hasFinishedCurrentPath();
bool isOnModifiedGoal() const;
double calcModuleRequestLength() const;
void resetStatus();
bool needPathUpdate(const double path_update_duration) const;
bool isStuck();
bool hasDecidedPath() const;
Expand All @@ -268,8 +369,8 @@ class GoalPlannerModule : public SceneModuleInterface
BehaviorModuleOutput planWithGoalModification();
BehaviorModuleOutput planWaitingApprovalWithGoalModification();
void selectSafePullOverPath();
void sortPullOverPathCandidatesByGoalPriority(
std::vector<PullOverPath> & pull_over_path_candidates,
std::vector<PullOverPath> sortPullOverPathCandidatesByGoalPriority(
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const;

// deal with pull over partial paths
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ MarkerArray createPosesMarkerArray(
MarkerArray createTextsMarkerArray(
const std::vector<Pose> & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color);
MarkerArray createGoalCandidatesMarkerArray(
GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color);
const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color);
MarkerArray createNumObjectsToAvoidTextsMarkerArray(
const GoalCandidates & goal_candidates, std::string && ns,
const std_msgs::msg::ColorRGBA & color);
Expand Down
Loading

0 comments on commit b019643

Please sign in to comment.