feat(goal_planner): divide Planners to isolated threads #9514

soblin · 2024-11-28T08:45:35Z

Description

Currently thread_safe_data_ member variable is accessed from many member functions. This structure is not healthy because there are other possible not thread-safe member variables, and numerous number of call to lock() is very insufficient.

After this PR, there are 3 agents

goal_planner_module
LaneParkingPlannerThread
FreespaceParkingThread

goal_planner_module

Lock the lane_parking_mutex_, and

prepare the LaneParkingRequest member
copy the LaneParkingResponse to local scope

Lock the freespace_parking_mutex_, and

prepare the FreespaceParkingRequest member
copy the FreespaceParkingResponse to local scope

LaneParkingThread

Lock the lane_parking_mutex_ which is shared with goal_planner_module via reference, and

copy LaneParkingResponse to local scope

After computation,
Lock the lane_parking_mutex_, and

set LaneParkingResponse

This will be used by goal_planner_module

FreespaceParkingThread

Lock the freespace_parking_mutex_ which is shared with goal_planner_module via reference, and

copy FreespaceParkingResponse to local scope

After computation,
Lock the freespace_parking_mutex_, and

set FreespaceParkingResponse

This will be used by goal_planner_module

Overall, thread safe data has been divided to above 4 classes, and the response value is copied to module side PullOverContextData member, so repetitive access to thread_safe_data with lock have been purged.

How was this PR tested?

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

github-actions · 2024-11-28T08:45:57Z

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

You've checked our contribution guidelines.
Your PR follows our pull request guidelines.
All required CI checks pass before marking the PR ready for review.

Signed-off-by: Mamoru Sobue <[email protected]>

kosuke55 · 2024-12-13T05:17:35Z

...behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

+  auto lane_parking_planner = std::make_unique<LaneParkingPlanner>(
+    node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_,
+    is_lane_parking_cb_running_, getLogger(), *parameters_);
  lane_parking_timer_ = rclcpp::create_timer(
-    &node, clock_, lane_parking_period_ns, std::bind(&GoalPlannerModule::onTimer, this),
+    &node, clock_, lane_parking_period_ns,
+    [lane_parking_planner_bind = std::move(lane_parking_planner)]() {
+      lane_parking_planner_bind->onTimer();
+    },


is it necessary to create lane_parking_planner temporaly variable?
creating planner in lamda is ok?

const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); lane_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); lane_parking_timer_ = rclcpp::create_timer( &node, clock_, lane_parking_period_ns, [lane_parkng_planner = std::make_unique<LaneParkingPlanner>( node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_, is_lane_parking_cb_running_, getLogger(), *parameters_)]() { lane_parkng_planner->onTimer(); }, lane_parking_timer_cb_group_);

renamed to lane_parking_executor in d42fc0d

kosuke55 · 2024-12-13T05:18:35Z

...behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

+    auto freespace_planner = std::make_shared<FreespacePullOver>(node, *parameters, vehicle_info);
    const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period();
    freespace_parking_timer_cb_group_ =
      node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    auto freespace_parking_planner = std::make_unique<FreespaceParkingPlanner>(
+      freespace_parking_mutex_, freespace_parking_request_, freespace_parking_response_,
+      is_freespace_parking_cb_running_, getLogger(), clock_, freespace_planner);
    freespace_parking_timer_ = rclcpp::create_timer(
      &node, clock_, freespace_parking_period_ns,
-      std::bind(&GoalPlannerModule::onFreespaceParkingTimer, this),
+      [freespace_parking_planner_bind = std::move(freespace_parking_planner)]() {
+        freespace_parking_planner_bind->onTimer();
+      },


same to lane_parking

is it necessary to create lane_parking_planner temporaly variable?
creating planner in lamda is ok?

renamed to freespace_parking_executor in d42fc0d

kosuke55 · 2024-12-13T05:24:18Z

...behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

+    auto freespace_planner = std::make_shared<FreespacePullOver>(node, *parameters, vehicle_info);
    const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period();
    freespace_parking_timer_cb_group_ =
      node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    auto freespace_parking_planner = std::make_unique<FreespaceParkingPlanner>(
+      freespace_parking_mutex_, freespace_parking_request_, freespace_parking_response_,
+      is_freespace_parking_cb_running_, getLogger(), clock_, freespace_planner);
    freespace_parking_timer_ = rclcpp::create_timer(
      &node, clock_, freespace_parking_period_ns,
-      std::bind(&GoalPlannerModule::onFreespaceParkingTimer, this),
+      [freespace_parking_planner_bind = std::move(freespace_parking_planner)]() {
+        freespace_parking_planner_bind->onTimer();
+      },


nits:
freespace_planner and freespace_parking_planner are little confusing.
FreespaceParkingPlanner is more like a thread or something.

kosuke55 · 2024-12-13T07:06:15Z

...al_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

-  std::optional<GoalPlannerData> gp_planner_data_{std::nullopt};
-  std::mutex gp_planner_data_mutex_;


gp_planner_data is deleted but still remain in comments. please modify or remove comments if they are not necessary.

delete in d42fc0d

kosuke55 · 2024-12-13T07:09:07Z

...behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

    }
-    auto & gp_planner_data = gp_planner_data_.value();
+    auto & lane_parking_request = lane_parking_request_.value();


question:
why creating refrence?
we can not use update() of lane_parking_request_ directly?

fixed in d42fc0d

kosuke55 · 2024-12-13T07:20:35Z

...behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

    // GoalPlannerModule::occupancy_grid_map_ and gp_planner_data.occupancy_grid_map share the
    // ownership, and gp_planner_data.occupancy_grid_map maybe also shared by the local
    // planner_data on onFreespaceParkingTimer thread local memory space. So following operation
    // is thread-safe because gp_planner_data.occupancy_grid_map is only re-pointed here and its
    // prior resource is still owned by the onFreespaceParkingTimer thread locally.
-    occupancy_grid_map_ = gp_planner_data.occupancy_grid_map;
+    occupancy_grid_map_ = freespace_parking_request.get_occupancy_grid_map();


occupancy_grid_map is not only for freepace_parking, it is also used for lane_parking.

for example,
checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { is in selectPullOverPath

occupancy_grid_map is come from planner_data

occupancy_grid_map->setMap(*(planner_data.occupancy_grid));

not from freespace parking.

remove ogm-based-collision-detector from FreespaceExecutor in the future

kosuke55 · 2024-12-13T07:24:22Z

...al_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

+  std::atomic<bool> & is_lane_parking_cb_running_;
+  rclcpp::Logger logger_;
+
+  std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;


nits:
(I know this is just using previous code. but we can use unique_ptr or just vector. I want refactor in the future)

kosuke55 · 2024-12-13T07:29:27Z

...behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

+  if (pull_over_planners_.empty()) {
+    RCLCPP_ERROR(logger_, "Not found enabled planner");
+  }


Suggested change

if (pull_over_planners_.empty()) {

RCLCPP_ERROR(logger_, "Not found enabled planner");

}

if (pull_over_planners_.empty()) {

RCLCPP_WARN(

getLogger(),

"No enabled planner found. The vehicle will stop in the road lane without pull over. Please "

"check the parameters if this is the intended behavior.");

}

currently I changed some log messages in #9562

there may be similar parts, so please check

Signed-off-by: Mamoru Sobue <[email protected]>

kosuke55 · 2024-12-13T07:39:46Z

...behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

-    auto path_and_goal_opt =
+    const auto & pull_over_path_candidates =
+      context_data.lane_parking_response.pull_over_path_candidates;
+    auto lane_pull_over_path_opt =


Suggested change

auto lane_pull_over_path_opt =

const auto lane_pull_over_path_opt =

?

I can build with const

fixed in 181415e

...behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

Signed-off-by: Mamoru Sobue <[email protected]>

codecov · 2024-12-13T11:18:41Z

Codecov Report

Attention: Patch coverage is 0% with 195 lines in your changes missing coverage. Please review.

Project coverage is 29.85%. Comparing base (9ff1c38) to head (83e3f8f).

Files with missing lines	Patch %	Lines
...th_goal_planner_module/src/goal_planner_module.cpp	0.00%	146 Missing ⚠️
...avior_path_goal_planner_module/src/thread_data.cpp	0.00%	24 Missing ⚠️
.../behavior_path_goal_planner_module/thread_data.hpp	0.00%	13 Missing ⚠️
...r_path_goal_planner_module/goal_planner_module.hpp	0.00%	12 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #9514      +/-   ##
==========================================
- Coverage   29.90%   29.85%   -0.05%     
==========================================
  Files        1441     1446       +5     
  Lines      108349   108409      +60     
  Branches    42492    42496       +4     
==========================================
- Hits        32403    32370      -33     
- Misses      72763    72852      +89     
- Partials     3183     3187       +4

Flag	Coverage Δ		*Carryforward flag
differential	`0.52% <0.00%> (?)`
total	`29.93% <ø> (+0.02%)`	⬆️	Carriedforward from f0f215c

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

Signed-off-by: Mamoru Sobue <[email protected]>

github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Nov 28, 2024

soblin force-pushed the feat/goal-planner/divide-thread branch 5 times, most recently from 6161574 to 919e4b9 Compare November 28, 2024 12:05

This was referenced Nov 29, 2024

refactor(goal_planner): move unnecessary member functions #9522

Merged

refactor(goal_planner): move PathDecisionController implementation to a different file #9523

Merged

soblin force-pushed the feat/goal-planner/divide-thread branch 8 times, most recently from e183b1e to b172a68 Compare December 2, 2024 06:35

soblin force-pushed the feat/goal-planner/divide-thread branch 12 times, most recently from 7410599 to 4ce8b96 Compare December 12, 2024 08:51

feat(goal_planner): disable freespace feature temporarily

4a98ca9

Signed-off-by: Mamoru Sobue <[email protected]>

soblin added 2 commits December 12, 2024 23:00

does not do pullover

362d70a

Signed-off-by: Mamoru Sobue <[email protected]>

seems working

4edb7a1

Signed-off-by: Mamoru Sobue <[email protected]>

soblin force-pushed the feat/goal-planner/divide-thread branch from 4ce8b96 to 4edb7a1 Compare December 12, 2024 14:01

soblin marked this pull request as ready for review December 12, 2024 15:13

soblin requested review from danielsanchezaran, kosuke55, kyoichi-sugahara, satoshi-ota, shmpwk, TomohitoAndo and tkimura4 as code owners December 12, 2024 15:13

fix

b4c7d79

Signed-off-by: Mamoru Sobue <[email protected]>

kosuke55 reviewed Dec 13, 2024

View reviewed changes

reflect comment

d42fc0d

Signed-off-by: Mamoru Sobue <[email protected]>

kosuke55 reviewed Dec 13, 2024

View reviewed changes

...behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp Outdated Show resolved Hide resolved

remove ogm-based-collision-detecor from Freespace in the future

bc1eb5f

Signed-off-by: Mamoru Sobue <[email protected]>

soblin mentioned this pull request Dec 13, 2024

perf(goal_planner): remove unnecessary call to setMap on freespace planning #9644

Merged

reflect comment2

181415e

Signed-off-by: Mamoru Sobue <[email protected]>

kosuke55 approved these changes Dec 13, 2024

View reviewed changes

soblin added the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Dec 13, 2024

soblin and others added 2 commits December 15, 2024 14:33

Merge branch 'main' into feat/goal-planner/divide-thread

f0f215c

fix ci

83e3f8f

Signed-off-by: Mamoru Sobue <[email protected]>

soblin force-pushed the feat/goal-planner/divide-thread branch from 58c879a to 83e3f8f Compare December 15, 2024 06:55

soblin merged commit 657c894 into autowarefoundation:main Dec 15, 2024
31 of 33 checks passed

soblin deleted the feat/goal-planner/divide-thread branch December 15, 2024 07:40

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(goal_planner): divide Planners to isolated threads #9514

feat(goal_planner): divide Planners to isolated threads #9514

soblin commented Nov 28, 2024 •

edited

Loading

github-actions bot commented Nov 28, 2024 •

edited

Loading

kosuke55 Dec 13, 2024

soblin Dec 13, 2024

kosuke55 Dec 13, 2024

soblin Dec 13, 2024

kosuke55 Dec 13, 2024

kosuke55 Dec 13, 2024 •

edited

Loading

soblin Dec 13, 2024

kosuke55 Dec 13, 2024

soblin Dec 13, 2024

kosuke55 Dec 13, 2024

soblin Dec 13, 2024

kosuke55 Dec 13, 2024

kosuke55 Dec 13, 2024

kosuke55 Dec 13, 2024

soblin Dec 13, 2024

codecov bot commented Dec 13, 2024 •

edited

Loading

		std::optional<GoalPlannerData> gp_planner_data_{std::nullopt};
		std::mutex gp_planner_data_mutex_;

	auto lane_pull_over_path_opt =
	const auto lane_pull_over_path_opt =

feat(goal_planner): divide Planners to isolated threads #9514

feat(goal_planner): divide Planners to isolated threads #9514

Conversation

soblin commented Nov 28, 2024 • edited Loading

Description

goal_planner_module

LaneParkingThread

FreespaceParkingThread

Related links

How was this PR tested?

Notes for reviewers

Interface changes

Effects on system behavior

github-actions bot commented Nov 28, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kosuke55 Dec 13, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov bot commented Dec 13, 2024 • edited Loading

Codecov Report

soblin commented Nov 28, 2024 •

edited

Loading

github-actions bot commented Nov 28, 2024 •

edited

Loading

kosuke55 Dec 13, 2024 •

edited

Loading

codecov bot commented Dec 13, 2024 •

edited

Loading