Skip to content

Commit

Permalink
feat(goal_planner): fix non-thread-safe access in goal_planner (rever…
Browse files Browse the repository at this point in the history 
…t of autowarefoundation#6809 and minor fix) (autowarefoundation#6965)

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed May 16, 2024
1 parent ad9ace8 commit e527bca
Show file tree
Hide file tree
Showing 9 changed files with 715 additions and 364 deletions.
6 changes: 1 addition & 5 deletions ...ath_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_

#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp"
#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp"

#include <lane_departure_checker/lane_departure_checker.hpp>
Expand All @@ -34,9 +33,7 @@ class GeometricPullOver : public PullOverPlannerBase
public:
GeometricPullOver(
rclcpp::Node & node, const GoalPlannerParameters & parameters,
const LaneDepartureChecker & lane_departure_checker,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const bool is_forward);
const LaneDepartureChecker & lane_departure_checker, const bool is_forward);

PullOverPlannerType getPlannerType() const override
{
Expand All @@ -61,7 +58,6 @@ class GeometricPullOver : public PullOverPlannerBase
protected:
ParallelParkingParameters parallel_parking_parameters_;
LaneDepartureChecker lane_departure_checker_{};
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_;
bool is_forward_{true};
bool left_side_parking_{true};

Expand Down
190 changes: 142 additions & 48 deletions ...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,33 @@ public: \
DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \
DEFINE_GETTER_WITH_MUTEX(TYPE, NAME)

enum class DecidingPathStatus {
NOT_DECIDED,
DECIDING,
DECIDED,
};
using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;

struct PreviousPullOverData
{
struct SafetyStatus
{
std::optional<rclcpp::Time> safe_start_time{};
bool is_safe{false};
};

void reset()
{
found_path = false;
safety_status = SafetyStatus{};
deciding_path_status = DecidingPathStatusWithStamp{};
}

bool found_path{false};
SafetyStatus safety_status{};
DecidingPathStatusWithStamp deciding_path_status{};
};

class ThreadSafeData
{
public:
Expand Down Expand Up @@ -145,6 +172,9 @@ class ThreadSafeData
void set(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path(arg); }
void set(const PullOverPath & arg) { set_pull_over_path(arg); }
void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); }
void set(const BehaviorModuleOutput & arg) { set_last_previous_module_output(arg); }
void set(const PreviousPullOverData & arg) { set_prev_data(arg); }
void set(const CollisionCheckDebugMap & arg) { set_collision_check(arg); }

void clearPullOverPath()
{
Expand Down Expand Up @@ -182,6 +212,8 @@ class ThreadSafeData
last_path_update_time_ = std::nullopt;
last_path_idx_increment_time_ = std::nullopt;
closest_start_pose_ = std::nullopt;
last_previous_module_output_ = std::nullopt;
prev_data_.reset();
}

DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, pull_over_path)
Expand All @@ -193,6 +225,9 @@ class ThreadSafeData
DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates)
DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<GoalCandidate>, modified_goal_pose)
DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<Pose>, closest_start_pose)
DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<BehaviorModuleOutput>, last_previous_module_output)
DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data)
DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check)

private:
std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
Expand All @@ -203,6 +238,9 @@ class ThreadSafeData
std::optional<rclcpp::Time> last_path_update_time_;
std::optional<rclcpp::Time> last_path_idx_increment_time_;
std::optional<Pose> closest_start_pose_{};
std::optional<BehaviorModuleOutput> last_previous_module_output_{};
PreviousPullOverData prev_data_{};
CollisionCheckDebugMap collision_check_{};

std::recursive_mutex & mutex_;
rclcpp::Clock::SharedPtr clock_;
Expand Down Expand Up @@ -234,33 +272,6 @@ struct LastApprovalData
Pose pose{};
};

enum class DecidingPathStatus {
NOT_DECIDED,
DECIDING,
DECIDED,
};
using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;

struct PreviousPullOverData
{
struct SafetyStatus
{
std::optional<rclcpp::Time> safe_start_time{};
bool is_safe{false};
};

void reset()
{
found_path = false;
safety_status = SafetyStatus{};
deciding_path_status = DecidingPathStatusWithStamp{};
}

bool found_path{false};
SafetyStatus safety_status{};
DecidingPathStatusWithStamp deciding_path_status{};
};

// store stop_pose_ pointer with reason string
struct PoseWithString
{
Expand Down Expand Up @@ -363,6 +374,50 @@ class GoalPlannerModule : public SceneModuleInterface
CandidateOutput planCandidate() const override { return CandidateOutput{}; }

private:
/**
* @brief shared data for onTimer(onTimer/onFreespaceParkingTimer just read this)
*/
struct GoalPlannerData
{
GoalPlannerData(const PlannerData & planner_data, const GoalPlannerParameters & parameters)
{
initializeOccupancyGridMap(planner_data, parameters);
};
GoalPlannerParameters parameters;
std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params;
std::shared_ptr<ObjectsFilteringParams> objects_filtering_params;
std::shared_ptr<SafetyCheckParams> safety_check_params;
tier4_autoware_utils::LinearRing2d vehicle_footprint;

PlannerData planner_data;
ModuleStatus current_status;
BehaviorModuleOutput previous_module_output;
// collision detector
// need to be shared_ptr to be used in planner and goal searcher
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map;
std::shared_ptr<GoalSearcherBase> goal_searcher;

const BehaviorModuleOutput & getPreviousModuleOutput() const { return previous_module_output; }
const ModuleStatus & getCurrentStatus() const { return current_status; }
void updateOccupancyGrid();
GoalPlannerData clone() const;
void update(
const GoalPlannerParameters & parameters,
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params_,
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params_,
const std::shared_ptr<SafetyCheckParams> & safety_check_params_,
const PlannerData & planner_data, const ModuleStatus & current_status,
const BehaviorModuleOutput & previous_module_output,
const std::shared_ptr<GoalSearcherBase> goal_searcher_,
const tier4_autoware_utils::LinearRing2d & vehicle_footprint);

private:
void initializeOccupancyGridMap(
const PlannerData & planner_data, const GoalPlannerParameters & parameters);
};
std::optional<GoalPlannerData> gp_planner_data_{std::nullopt};
std::mutex gp_planner_data_mutex_;

// Flag class for managing whether a certain callback is running in multi-threading
class ScopedFlag
{
Expand Down Expand Up @@ -420,9 +475,10 @@ class GoalPlannerModule : public SceneModuleInterface
// goal searcher
std::shared_ptr<GoalSearcherBase> goal_searcher_;

// collision detector
// need to be shared_ptr to be used in planner and goal searcher
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_;
// NOTE: this is latest occupancy_grid_map pointer which the local planner_data on
// onFreespaceParkingTimer thread storage may point to while calculation.
// onTimer/onFreespaceParkingTimer and their callees MUST NOT use this
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{nullptr};

// check stopped and stuck state
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stopped_;
Expand All @@ -431,10 +487,10 @@ class GoalPlannerModule : public SceneModuleInterface
tier4_autoware_utils::LinearRing2d vehicle_footprint_;

std::recursive_mutex mutex_;
ThreadSafeData thread_safe_data_;
// TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable
mutable ThreadSafeData thread_safe_data_;

std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};
PreviousPullOverData prev_data_{};

// 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 @@ -460,11 +516,12 @@ class GoalPlannerModule : public SceneModuleInterface
mutable PoseWithString debug_stop_pose_with_info_;

// collision check
void initializeOccupancyGridMap();
void updateOccupancyGrid();
bool checkOccupancyGridCollision(const PathWithLaneId & path) const;
bool checkOccupancyGridCollision(
const PathWithLaneId & path,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;
bool checkObjectsCollision(
const PathWithLaneId & path, const double collision_check_margin,
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
const GoalPlannerParameters & parameters, const double collision_check_margin,
const bool extract_static_objects, const bool update_debug_data = false) const;

// goal seach
Expand All @@ -487,13 +544,39 @@ class GoalPlannerModule : public SceneModuleInterface
bool isStopped(
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
bool hasFinishedCurrentPath();
bool isOnModifiedGoal() const;
bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const;
double calcModuleRequestLength() const;
bool needPathUpdate(const double path_update_duration) const;
bool isStuck();
bool hasDecidedPath() const;
bool hasNotDecidedPath() const;
DecidingPathStatusWithStamp checkDecidingPathStatus() const;
bool needPathUpdate(
const Pose & current_pose, const double path_update_duration,
const GoalPlannerParameters & parameters) const;
bool isStuck(
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const GoalPlannerParameters & parameters);
bool hasDecidedPath(
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const GoalPlannerParameters & parameters,
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
bool hasNotDecidedPath(
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const GoalPlannerParameters & parameters,
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
DecidingPathStatusWithStamp checkDecidingPathStatus(
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const GoalPlannerParameters & parameters,
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
void decideVelocity();
bool foundPullOverPath() const;
void updateStatus(const BehaviorModuleOutput & output);
Expand All @@ -509,7 +592,10 @@ class GoalPlannerModule : public SceneModuleInterface
bool hasEnoughTimePassedSincePathUpdate(const double duration) const;

// freespace parking
bool planFreespacePath();
bool planFreespacePath(
std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<GoalSearcherBase> goal_searcher,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map);
bool canReturnToLaneParking();

// plan pull over path
Expand Down Expand Up @@ -538,10 +624,12 @@ class GoalPlannerModule : public SceneModuleInterface
TurnSignalInfo calcTurnSignalInfo();
std::optional<lanelet::Id> ignore_signal_{std::nullopt};

std::optional<BehaviorModuleOutput> last_previous_module_output_{};
bool hasPreviousModulePathShapeChanged() const;
bool hasDeviatedFromLastPreviousModulePath() const;
bool hasDeviatedFromCurrentPreviousModulePath() const;
bool hasPreviousModulePathShapeChanged(const BehaviorModuleOutput & previous_module_output) const;
bool hasDeviatedFromLastPreviousModulePath(
const std::shared_ptr<const PlannerData> planner_data) const;
bool hasDeviatedFromCurrentPreviousModulePath(
const std::shared_ptr<const PlannerData> planner_data,
const BehaviorModuleOutput & previous_module_output) const;

// timer for generating pull over path candidates in a separate thread
void onTimer();
Expand All @@ -557,16 +645,22 @@ class GoalPlannerModule : public SceneModuleInterface
// safety check
void initializeSafetyCheckParameters();
SafetyCheckParams createSafetyCheckParams() const;
/*
void updateSafetyCheckTargetObjectsData(
const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
*/
/**
* @brief Checks if the current path is safe.
* @return std::pair<bool, bool>
* first: If the path is safe for a certain period of time, true.
* second: If the path is safe in the current state, true.
*/
std::pair<bool, bool> isSafePath() const;
std::pair<bool, bool> isSafePath(
const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<SafetyCheckParams> & safety_check_params) const;

// debug
void setDebugData();
Expand Down
36 changes: 24 additions & 12 deletions ...vior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,33 +29,45 @@ using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
class GoalSearcher : public GoalSearcherBase
{
public:
GoalSearcher(
const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> & occupancy_grid_map);
GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);

GoalCandidates search() override;
void update(GoalCandidates & goal_candidates) const override;
GoalCandidates search(
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) override;
void update(
GoalCandidates & goal_candidates,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) const override;

// todo(kosuke55): Functions for this specific use should not be in the interface,
// so it is better to consider interface design when we implement other goal searchers.
GoalCandidate getClosetGoalCandidateAlongLanes(
const GoalCandidates & goal_candidates) const override;
const GoalCandidates & goal_candidates,
const std::shared_ptr<const PlannerData> & planner_data) const override;
bool isSafeGoalWithMarginScaleFactor(
const GoalCandidate & goal_candidate, const double margin_scale_factor) const override;
const GoalCandidate & goal_candidate, const double margin_scale_factor,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) const override;

private:
void countObjectsToAvoid(
GoalCandidates & goal_candidates, const PredictedObjects & objects) const;
void createAreaPolygons(std::vector<Pose> original_search_poses);
bool checkCollision(const Pose & pose, const PredictedObjects & objects) const;
GoalCandidates & goal_candidates, const PredictedObjects & objects,
const std::shared_ptr<const PlannerData> & planner_data) const;
void createAreaPolygons(
std::vector<Pose> original_search_poses,
const std::shared_ptr<const PlannerData> & planner_data);
bool checkCollision(
const Pose & pose, const PredictedObjects & objects,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;
bool checkCollisionWithLongitudinalDistance(
const Pose & ego_pose, const PredictedObjects & objects) const;
const Pose & ego_pose, const PredictedObjects & objects,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) const;
BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const;

LinearRing2d vehicle_footprint_{};
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{};
bool left_side_parking_{true};
};
} // namespace behavior_path_planner
Expand Down
Loading

0 comments on commit e527bca

Please sign in to comment.