Skip to content

Commit 35d39e7

Browse files
authored
feat(goal_planner): fix non-thread-safe access in goal_planner (revert of #6809 and minor fix) (#6965)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent c7c826d commit 35d39e7

File tree

9 files changed

+715
-364
lines changed

9 files changed

+715
-364
lines changed

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp

+1-5
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,6 @@
1616
#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_
1717

1818
#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp"
19-
#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
2019
#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp"
2120

2221
#include <lane_departure_checker/lane_departure_checker.hpp>
@@ -34,9 +33,7 @@ class GeometricPullOver : public PullOverPlannerBase
3433
public:
3534
GeometricPullOver(
3635
rclcpp::Node & node, const GoalPlannerParameters & parameters,
37-
const LaneDepartureChecker & lane_departure_checker,
38-
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
39-
const bool is_forward);
36+
const LaneDepartureChecker & lane_departure_checker, const bool is_forward);
4037

4138
PullOverPlannerType getPlannerType() const override
4239
{
@@ -61,7 +58,6 @@ class GeometricPullOver : public PullOverPlannerBase
6158
protected:
6259
ParallelParkingParameters parallel_parking_parameters_;
6360
LaneDepartureChecker lane_departure_checker_{};
64-
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_;
6561
bool is_forward_{true};
6662
bool left_side_parking_{true};
6763

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

+142-48
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,33 @@ public: \
9191
DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \
9292
DEFINE_GETTER_WITH_MUTEX(TYPE, NAME)
9393

94+
enum class DecidingPathStatus {
95+
NOT_DECIDED,
96+
DECIDING,
97+
DECIDED,
98+
};
99+
using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;
100+
101+
struct PreviousPullOverData
102+
{
103+
struct SafetyStatus
104+
{
105+
std::optional<rclcpp::Time> safe_start_time{};
106+
bool is_safe{false};
107+
};
108+
109+
void reset()
110+
{
111+
found_path = false;
112+
safety_status = SafetyStatus{};
113+
deciding_path_status = DecidingPathStatusWithStamp{};
114+
}
115+
116+
bool found_path{false};
117+
SafetyStatus safety_status{};
118+
DecidingPathStatusWithStamp deciding_path_status{};
119+
};
120+
94121
class ThreadSafeData
95122
{
96123
public:
@@ -145,6 +172,9 @@ class ThreadSafeData
145172
void set(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path(arg); }
146173
void set(const PullOverPath & arg) { set_pull_over_path(arg); }
147174
void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); }
175+
void set(const BehaviorModuleOutput & arg) { set_last_previous_module_output(arg); }
176+
void set(const PreviousPullOverData & arg) { set_prev_data(arg); }
177+
void set(const CollisionCheckDebugMap & arg) { set_collision_check(arg); }
148178

149179
void clearPullOverPath()
150180
{
@@ -182,6 +212,8 @@ class ThreadSafeData
182212
last_path_update_time_ = std::nullopt;
183213
last_path_idx_increment_time_ = std::nullopt;
184214
closest_start_pose_ = std::nullopt;
215+
last_previous_module_output_ = std::nullopt;
216+
prev_data_.reset();
185217
}
186218

187219
DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, pull_over_path)
@@ -193,6 +225,9 @@ class ThreadSafeData
193225
DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates)
194226
DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<GoalCandidate>, modified_goal_pose)
195227
DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<Pose>, closest_start_pose)
228+
DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<BehaviorModuleOutput>, last_previous_module_output)
229+
DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data)
230+
DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check)
196231

197232
private:
198233
std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
@@ -203,6 +238,9 @@ class ThreadSafeData
203238
std::optional<rclcpp::Time> last_path_update_time_;
204239
std::optional<rclcpp::Time> last_path_idx_increment_time_;
205240
std::optional<Pose> closest_start_pose_{};
241+
std::optional<BehaviorModuleOutput> last_previous_module_output_{};
242+
PreviousPullOverData prev_data_{};
243+
CollisionCheckDebugMap collision_check_{};
206244

207245
std::recursive_mutex & mutex_;
208246
rclcpp::Clock::SharedPtr clock_;
@@ -234,33 +272,6 @@ struct LastApprovalData
234272
Pose pose{};
235273
};
236274

237-
enum class DecidingPathStatus {
238-
NOT_DECIDED,
239-
DECIDING,
240-
DECIDED,
241-
};
242-
using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;
243-
244-
struct PreviousPullOverData
245-
{
246-
struct SafetyStatus
247-
{
248-
std::optional<rclcpp::Time> safe_start_time{};
249-
bool is_safe{false};
250-
};
251-
252-
void reset()
253-
{
254-
found_path = false;
255-
safety_status = SafetyStatus{};
256-
deciding_path_status = DecidingPathStatusWithStamp{};
257-
}
258-
259-
bool found_path{false};
260-
SafetyStatus safety_status{};
261-
DecidingPathStatusWithStamp deciding_path_status{};
262-
};
263-
264275
// store stop_pose_ pointer with reason string
265276
struct PoseWithString
266277
{
@@ -363,6 +374,50 @@ class GoalPlannerModule : public SceneModuleInterface
363374
CandidateOutput planCandidate() const override { return CandidateOutput{}; }
364375

365376
private:
377+
/**
378+
* @brief shared data for onTimer(onTimer/onFreespaceParkingTimer just read this)
379+
*/
380+
struct GoalPlannerData
381+
{
382+
GoalPlannerData(const PlannerData & planner_data, const GoalPlannerParameters & parameters)
383+
{
384+
initializeOccupancyGridMap(planner_data, parameters);
385+
};
386+
GoalPlannerParameters parameters;
387+
std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params;
388+
std::shared_ptr<ObjectsFilteringParams> objects_filtering_params;
389+
std::shared_ptr<SafetyCheckParams> safety_check_params;
390+
tier4_autoware_utils::LinearRing2d vehicle_footprint;
391+
392+
PlannerData planner_data;
393+
ModuleStatus current_status;
394+
BehaviorModuleOutput previous_module_output;
395+
// collision detector
396+
// need to be shared_ptr to be used in planner and goal searcher
397+
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map;
398+
std::shared_ptr<GoalSearcherBase> goal_searcher;
399+
400+
const BehaviorModuleOutput & getPreviousModuleOutput() const { return previous_module_output; }
401+
const ModuleStatus & getCurrentStatus() const { return current_status; }
402+
void updateOccupancyGrid();
403+
GoalPlannerData clone() const;
404+
void update(
405+
const GoalPlannerParameters & parameters,
406+
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params_,
407+
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params_,
408+
const std::shared_ptr<SafetyCheckParams> & safety_check_params_,
409+
const PlannerData & planner_data, const ModuleStatus & current_status,
410+
const BehaviorModuleOutput & previous_module_output,
411+
const std::shared_ptr<GoalSearcherBase> goal_searcher_,
412+
const tier4_autoware_utils::LinearRing2d & vehicle_footprint);
413+
414+
private:
415+
void initializeOccupancyGridMap(
416+
const PlannerData & planner_data, const GoalPlannerParameters & parameters);
417+
};
418+
std::optional<GoalPlannerData> gp_planner_data_{std::nullopt};
419+
std::mutex gp_planner_data_mutex_;
420+
366421
// Flag class for managing whether a certain callback is running in multi-threading
367422
class ScopedFlag
368423
{
@@ -420,9 +475,10 @@ class GoalPlannerModule : public SceneModuleInterface
420475
// goal searcher
421476
std::shared_ptr<GoalSearcherBase> goal_searcher_;
422477

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

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

433489
std::recursive_mutex mutex_;
434-
ThreadSafeData thread_safe_data_;
490+
// TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable
491+
mutable ThreadSafeData thread_safe_data_;
435492

436493
std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};
437-
PreviousPullOverData prev_data_{};
438494

439495
// approximate distance from the start point to the end point of pull_over.
440496
// this is used as an assumed value to decelerate, etc., before generating the actual path.
@@ -460,11 +516,12 @@ class GoalPlannerModule : public SceneModuleInterface
460516
mutable PoseWithString debug_stop_pose_with_info_;
461517

462518
// collision check
463-
void initializeOccupancyGridMap();
464-
void updateOccupancyGrid();
465-
bool checkOccupancyGridCollision(const PathWithLaneId & path) const;
519+
bool checkOccupancyGridCollision(
520+
const PathWithLaneId & path,
521+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;
466522
bool checkObjectsCollision(
467-
const PathWithLaneId & path, const double collision_check_margin,
523+
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
524+
const GoalPlannerParameters & parameters, const double collision_check_margin,
468525
const bool extract_static_objects, const bool update_debug_data = false) const;
469526

470527
// goal seach
@@ -487,13 +544,39 @@ class GoalPlannerModule : public SceneModuleInterface
487544
bool isStopped(
488545
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
489546
bool hasFinishedCurrentPath();
490-
bool isOnModifiedGoal() const;
547+
bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const;
491548
double calcModuleRequestLength() const;
492-
bool needPathUpdate(const double path_update_duration) const;
493-
bool isStuck();
494-
bool hasDecidedPath() const;
495-
bool hasNotDecidedPath() const;
496-
DecidingPathStatusWithStamp checkDecidingPathStatus() const;
549+
bool needPathUpdate(
550+
const Pose & current_pose, const double path_update_duration,
551+
const GoalPlannerParameters & parameters) const;
552+
bool isStuck(
553+
const std::shared_ptr<const PlannerData> planner_data,
554+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
555+
const GoalPlannerParameters & parameters);
556+
bool hasDecidedPath(
557+
const std::shared_ptr<const PlannerData> planner_data,
558+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
559+
const GoalPlannerParameters & parameters,
560+
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
561+
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
562+
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
563+
const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
564+
bool hasNotDecidedPath(
565+
const std::shared_ptr<const PlannerData> planner_data,
566+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
567+
const GoalPlannerParameters & parameters,
568+
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
569+
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
570+
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
571+
const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
572+
DecidingPathStatusWithStamp checkDecidingPathStatus(
573+
const std::shared_ptr<const PlannerData> planner_data,
574+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
575+
const GoalPlannerParameters & parameters,
576+
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
577+
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
578+
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
579+
const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
497580
void decideVelocity();
498581
bool foundPullOverPath() const;
499582
void updateStatus(const BehaviorModuleOutput & output);
@@ -508,7 +591,10 @@ class GoalPlannerModule : public SceneModuleInterface
508591
bool hasEnoughTimePassedSincePathUpdate(const double duration) const;
509592

510593
// freespace parking
511-
bool planFreespacePath();
594+
bool planFreespacePath(
595+
std::shared_ptr<const PlannerData> planner_data,
596+
const std::shared_ptr<GoalSearcherBase> goal_searcher,
597+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map);
512598
bool canReturnToLaneParking();
513599

514600
// plan pull over path
@@ -537,10 +623,12 @@ class GoalPlannerModule : public SceneModuleInterface
537623
TurnSignalInfo calcTurnSignalInfo();
538624
std::optional<lanelet::Id> ignore_signal_{std::nullopt};
539625

540-
std::optional<BehaviorModuleOutput> last_previous_module_output_{};
541-
bool hasPreviousModulePathShapeChanged() const;
542-
bool hasDeviatedFromLastPreviousModulePath() const;
543-
bool hasDeviatedFromCurrentPreviousModulePath() const;
626+
bool hasPreviousModulePathShapeChanged(const BehaviorModuleOutput & previous_module_output) const;
627+
bool hasDeviatedFromLastPreviousModulePath(
628+
const std::shared_ptr<const PlannerData> planner_data) const;
629+
bool hasDeviatedFromCurrentPreviousModulePath(
630+
const std::shared_ptr<const PlannerData> planner_data,
631+
const BehaviorModuleOutput & previous_module_output) const;
544632

545633
// timer for generating pull over path candidates in a separate thread
546634
void onTimer();
@@ -556,16 +644,22 @@ class GoalPlannerModule : public SceneModuleInterface
556644
// safety check
557645
void initializeSafetyCheckParameters();
558646
SafetyCheckParams createSafetyCheckParams() const;
647+
/*
559648
void updateSafetyCheckTargetObjectsData(
560649
const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
561650
const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
651+
*/
562652
/**
563653
* @brief Checks if the current path is safe.
564654
* @return std::pair<bool, bool>
565655
* first: If the path is safe for a certain period of time, true.
566656
* second: If the path is safe in the current state, true.
567657
*/
568-
std::pair<bool, bool> isSafePath() const;
658+
std::pair<bool, bool> isSafePath(
659+
const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
660+
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
661+
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
662+
const std::shared_ptr<SafetyCheckParams> & safety_check_params) const;
569663

570664
// debug
571665
void setDebugData();

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp

+24-12
Original file line numberDiff line numberDiff line change
@@ -29,33 +29,45 @@ using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
2929
class GoalSearcher : public GoalSearcherBase
3030
{
3131
public:
32-
GoalSearcher(
33-
const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint,
34-
const std::shared_ptr<OccupancyGridBasedCollisionDetector> & occupancy_grid_map);
32+
GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);
3533

36-
GoalCandidates search() override;
37-
void update(GoalCandidates & goal_candidates) const override;
34+
GoalCandidates search(
35+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
36+
const std::shared_ptr<const PlannerData> & planner_data) override;
37+
void update(
38+
GoalCandidates & goal_candidates,
39+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
40+
const std::shared_ptr<const PlannerData> & planner_data) const override;
3841

3942
// todo(kosuke55): Functions for this specific use should not be in the interface,
4043
// so it is better to consider interface design when we implement other goal searchers.
4144
GoalCandidate getClosetGoalCandidateAlongLanes(
42-
const GoalCandidates & goal_candidates) const override;
45+
const GoalCandidates & goal_candidates,
46+
const std::shared_ptr<const PlannerData> & planner_data) const override;
4347
bool isSafeGoalWithMarginScaleFactor(
44-
const GoalCandidate & goal_candidate, const double margin_scale_factor) const override;
48+
const GoalCandidate & goal_candidate, const double margin_scale_factor,
49+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
50+
const std::shared_ptr<const PlannerData> & planner_data) const override;
4551

4652
private:
4753
void countObjectsToAvoid(
48-
GoalCandidates & goal_candidates, const PredictedObjects & objects) const;
49-
void createAreaPolygons(std::vector<Pose> original_search_poses);
50-
bool checkCollision(const Pose & pose, const PredictedObjects & objects) const;
54+
GoalCandidates & goal_candidates, const PredictedObjects & objects,
55+
const std::shared_ptr<const PlannerData> & planner_data) const;
56+
void createAreaPolygons(
57+
std::vector<Pose> original_search_poses,
58+
const std::shared_ptr<const PlannerData> & planner_data);
59+
bool checkCollision(
60+
const Pose & pose, const PredictedObjects & objects,
61+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;
5162
bool checkCollisionWithLongitudinalDistance(
52-
const Pose & ego_pose, const PredictedObjects & objects) const;
63+
const Pose & ego_pose, const PredictedObjects & objects,
64+
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
65+
const std::shared_ptr<const PlannerData> & planner_data) const;
5366
BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
5467
BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
5568
bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const;
5669

5770
LinearRing2d vehicle_footprint_{};
58-
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{};
5971
bool left_side_parking_{true};
6072
};
6173
} // namespace behavior_path_planner

0 commit comments

Comments
 (0)