Skip to content

Commit c897b3d

Browse files
authored
fix(start/goal_planner): fix multi thread memory crash (#6322)
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent d408e15 commit c897b3d

File tree

4 files changed

+84
-1
lines changed

4 files changed

+84
-1
lines changed

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

+44
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
3838
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
3939

40+
#include <atomic>
4041
#include <deque>
4142
#include <limits>
4243
#include <memory>
@@ -290,6 +291,35 @@ class GoalPlannerModule : public SceneModuleInterface
290291
std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
291292
objects_of_interest_marker_interface_ptr_map);
292293

294+
~GoalPlannerModule()
295+
{
296+
if (lane_parking_timer_) {
297+
lane_parking_timer_->cancel();
298+
}
299+
if (freespace_parking_timer_) {
300+
freespace_parking_timer_->cancel();
301+
}
302+
303+
while (is_lane_parking_cb_running_.load() || is_freespace_parking_cb_running_.load()) {
304+
const std::string running_callbacks = std::invoke([&]() {
305+
if (is_lane_parking_cb_running_ && is_freespace_parking_cb_running_) {
306+
return "lane parking and freespace parking";
307+
}
308+
if (is_lane_parking_cb_running_) {
309+
return "lane parking";
310+
}
311+
return "freespace parking";
312+
});
313+
RCLCPP_INFO_THROTTLE(
314+
getLogger(), *clock_, 1000, "Waiting for %s callback to finish...",
315+
running_callbacks.c_str());
316+
std::this_thread::sleep_for(std::chrono::milliseconds(10));
317+
}
318+
319+
RCLCPP_INFO_THROTTLE(
320+
getLogger(), *clock_, 1000, "lane parking and freespace parking callbacks finished");
321+
}
322+
293323
void updateModuleParams(const std::any & parameters) override
294324
{
295325
parameters_ = std::any_cast<std::shared_ptr<GoalPlannerParameters>>(parameters);
@@ -330,6 +360,18 @@ class GoalPlannerModule : public SceneModuleInterface
330360
CandidateOutput planCandidate() const override { return CandidateOutput{}; }
331361

332362
private:
363+
// Flag class for managing whether a certain callback is running in multi-threading
364+
class ScopedFlag
365+
{
366+
public:
367+
explicit ScopedFlag(std::atomic<bool> & flag) : flag_(flag) { flag_.store(true); }
368+
369+
~ScopedFlag() { flag_.store(false); }
370+
371+
private:
372+
std::atomic<bool> & flag_;
373+
};
374+
333375
/*
334376
* state transitions and plan function used in each state
335377
*
@@ -403,10 +445,12 @@ class GoalPlannerModule : public SceneModuleInterface
403445
// pre-generate lane parking paths in a separate thread
404446
rclcpp::TimerBase::SharedPtr lane_parking_timer_;
405447
rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
448+
std::atomic<bool> is_lane_parking_cb_running_;
406449

407450
// generate freespace parking paths in a separate thread
408451
rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
409452
rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
453+
std::atomic<bool> is_freespace_parking_cb_running_;
410454

411455
// debug
412456
mutable GoalPlannerDebugData debug_data_;

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,8 @@ GoalPlannerModule::GoalPlannerModule(
6262
parameters_{parameters},
6363
vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
6464
thread_safe_data_{mutex_, clock_},
65+
is_lane_parking_cb_running_{false},
66+
is_freespace_parking_cb_running_{false},
6567
debug_stop_pose_with_info_{&stop_pose_}
6668
{
6769
LaneDepartureChecker lane_departure_checker{};
@@ -171,6 +173,8 @@ bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const
171173
// generate pull over candidate paths
172174
void GoalPlannerModule::onTimer()
173175
{
176+
const ScopedFlag flag(is_lane_parking_cb_running_);
177+
174178
if (getCurrentStatus() == ModuleStatus::IDLE) {
175179
return;
176180
}
@@ -287,6 +291,8 @@ void GoalPlannerModule::onTimer()
287291

288292
void GoalPlannerModule::onFreespaceParkingTimer()
289293
{
294+
const ScopedFlag flag(is_freespace_parking_cb_running_);
295+
290296
if (getCurrentStatus() == ModuleStatus::IDLE) {
291297
return;
292298
}

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

+30
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@
3535

3636
#include <tf2/utils.h>
3737

38+
#include <atomic>
3839
#include <deque>
3940
#include <memory>
4041
#include <string>
@@ -83,6 +84,21 @@ class StartPlannerModule : public SceneModuleInterface
8384
std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
8485
objects_of_interest_marker_interface_ptr_map);
8586

87+
~StartPlannerModule()
88+
{
89+
if (freespace_planner_timer_) {
90+
freespace_planner_timer_->cancel();
91+
}
92+
93+
while (is_freespace_planner_cb_running_.load()) {
94+
RCLCPP_INFO_THROTTLE(
95+
getLogger(), *clock_, 1000, "Waiting for freespace planner callback to finish...");
96+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
97+
}
98+
99+
RCLCPP_INFO_THROTTLE(getLogger(), *clock_, 1000, "freespace planner callback finished");
100+
}
101+
86102
void updateModuleParams(const std::any & parameters) override
87103
{
88104
parameters_ = std::any_cast<std::shared_ptr<StartPlannerParameters>>(parameters);
@@ -135,6 +151,18 @@ class StartPlannerModule : public SceneModuleInterface
135151
bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; }
136152

137153
private:
154+
// Flag class for managing whether a certain callback is running in multi-threading
155+
class ScopedFlag
156+
{
157+
public:
158+
explicit ScopedFlag(std::atomic<bool> & flag) : flag_(flag) { flag_.store(true); }
159+
160+
~ScopedFlag() { flag_.store(false); }
161+
162+
private:
163+
std::atomic<bool> & flag_;
164+
};
165+
138166
bool canTransitSuccessState() override;
139167

140168
bool canTransitFailureState() override { return false; }
@@ -202,6 +230,8 @@ class StartPlannerModule : public SceneModuleInterface
202230
std::unique_ptr<PullOutPlannerBase> freespace_planner_;
203231
rclcpp::TimerBase::SharedPtr freespace_planner_timer_;
204232
rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_;
233+
std::atomic<bool> is_freespace_planner_cb_running_;
234+
205235
// TODO(kosuke55)
206236
// Currently, we only do lock when updating a member of status_.
207237
// However, we need to ensure that the value does not change when referring to it.

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,8 @@ StartPlannerModule::StartPlannerModule(
5757
objects_of_interest_marker_interface_ptr_map)
5858
: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT
5959
parameters_{parameters},
60-
vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}
60+
vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
61+
is_freespace_planner_cb_running_{false}
6162
{
6263
lane_departure_checker_ = std::make_shared<LaneDepartureChecker>();
6364
lane_departure_checker_->setVehicleInfo(vehicle_info_);
@@ -88,6 +89,8 @@ StartPlannerModule::StartPlannerModule(
8889

8990
void StartPlannerModule::onFreespacePlannerTimer()
9091
{
92+
const ScopedFlag flag(is_freespace_planner_cb_running_);
93+
9194
if (getCurrentStatus() == ModuleStatus::IDLE) {
9295
return;
9396
}

0 commit comments

Comments
 (0)