From 7135f721738e44aea002e733d7cbe3e6c41035ad Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 1 Aug 2024 09:21:03 +0900 Subject: [PATCH 001/126] feat(behavior_path _planner): divide planner manager modules into dependent slots (#8117) --- .../manager.hpp | 2 - .../src/manager.cpp | 4 - .../manager.hpp | 2 - .../src/manager.cpp | 19 - .../config/scene_module_manager.param.yaml | 44 +- .../behavior_path_planner_manager_design.md | 102 +- ...modules.svg => failure_modules.drawio.svg} | 145 +-- ...move.svg => lane_change_remove.drawio.svg} | 31 +- ...e_skip.svg => lane_change_skip.drawio.svg} | 19 +- ...le_select.svg => module_select.drawio.svg} | 11 +- ...ep5.svg => multiple_candidates.drawio.svg} | 15 +- ...ut_module.svg => output_module.drawio.svg} | 13 +- .../image/manager/process_step1.drawio.svg | 554 ++++++++ .../image/manager/process_step1.svg | 322 ----- ...ess_step2.svg => process_step2.drawio.svg} | 145 ++- ...ess_step4.svg => process_step4.drawio.svg} | 15 +- ...ess_step5.svg => process_step5.drawio.svg} | 27 +- ...ess_step6.svg => process_step6.drawio.svg} | 21 +- ...est_step1.svg => request_step1.drawio.svg} | 83 +- ...step2-2.svg => request_step2-2.drawio.svg} | 3 +- ...step2-3.svg => request_step2-3.drawio.svg} | 3 +- ...est_step2.svg => request_step2.drawio.svg} | 3 +- ...est_step3.svg => request_step3.drawio.svg} | 7 +- ...step4-2.svg => request_step4-2.drawio.svg} | 152 ++- ...step4-3.svg => request_step4-3.drawio.svg} | 11 +- ...est_step4.svg => request_step4.drawio.svg} | 7 +- ...andidates.svg => request_step5.drawio.svg} | 144 +-- ....svg => success_modules_remove.drawio.svg} | 23 +- ...ip.svg => success_modules_skip.drawio.svg} | 165 +-- ...approve.svg => waiting_approve.drawio.svg} | 27 +- .../behavior_path_planner/planner_manager.hpp | 552 ++++---- .../src/behavior_path_planner_node.cpp | 20 +- .../src/planner_manager.cpp | 1146 ++++++++--------- .../scene_module_manager_interface.hpp | 25 - .../parameters.hpp | 3 - 35 files changed, 2097 insertions(+), 1768 deletions(-) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{failure_modules.svg => failure_modules.drawio.svg} (73%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{lane_change_remove.svg => lane_change_remove.drawio.svg} (89%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{lane_change_skip.svg => lane_change_skip.drawio.svg} (92%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{module_select.svg => module_select.drawio.svg} (84%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{request_step5.svg => multiple_candidates.drawio.svg} (82%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{output_module.svg => output_module.drawio.svg} (85%) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.drawio.svg delete mode 100644 planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.svg rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{process_step2.svg => process_step2.drawio.svg} (75%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{process_step4.svg => process_step4.drawio.svg} (90%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{process_step5.svg => process_step5.drawio.svg} (90%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{process_step6.svg => process_step6.drawio.svg} (90%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{request_step1.svg => request_step1.drawio.svg} (78%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{request_step2-2.svg => request_step2-2.drawio.svg} (88%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{request_step2-3.svg => request_step2-3.drawio.svg} (89%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{request_step2.svg => request_step2.drawio.svg} (89%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{request_step3.svg => request_step3.drawio.svg} (84%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{request_step4-2.svg => request_step4-2.drawio.svg} (58%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{request_step4-3.svg => request_step4-3.drawio.svg} (88%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{request_step4.svg => request_step4.drawio.svg} (88%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{multiple_candidates.svg => request_step5.drawio.svg} (51%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{success_modules_remove.svg => success_modules_remove.drawio.svg} (85%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{success_modules_skip.svg => success_modules_skip.drawio.svg} (77%) rename planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/{waiting_approve.svg => waiting_approve.drawio.svg} (86%) diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index 9c85a463067de..f09196b2cc8e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -47,8 +47,6 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; - bool isAlwaysExecutableModule() const override; - private: std::shared_ptr parameters_; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp index eccf2b2a1bfde..cdf878fb62c63 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp @@ -287,10 +287,6 @@ void DynamicObstacleAvoidanceModuleManager::updateModuleParams( }); } -bool DynamicObstacleAvoidanceModuleManager::isAlwaysExecutableModule() const -{ - return true; -} } // namespace autoware::behavior_path_planner #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp index 32d816d9a37e8..5408dce9dcdfc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp @@ -43,8 +43,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; - bool isAlwaysExecutableModule() const override; - bool isSimultaneousExecutableAsApprovedModule() const override; bool isSimultaneousExecutableAsCandidateModule() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 8a4f3ab202a67..00b27d43087f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -827,23 +827,8 @@ void GoalPlannerModuleManager::updateModuleParams( }); } -bool GoalPlannerModuleManager::isAlwaysExecutableModule() const -{ - // enable AlwaysExecutable whenever goal modification is not allowed - // because only minor path refinements are made for fixed goals - if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { - return true; - } - - return false; -} - bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const { - if (isAlwaysExecutableModule()) { - return true; - } - // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { @@ -855,10 +840,6 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const { - if (isAlwaysExecutableModule()) { - return true; - } - // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml index ef6cb82d1412a..f94657c1418e4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml @@ -3,80 +3,80 @@ # NOTE: The smaller the priority number is, the higher the module priority is. /**: ros__parameters: + # NOTE: modules which are not set true in the preset is ignored in the slot configuration + slots: + # NOTE: array of array is not supported + - slot1 + - slot2 + - slot3 + - slot4 + slot1: + - "start_planner" + slot2: + - "side_shift" + - "avoidance_by_lane_change" + - "static_obstacle_avoidance" + - "lane_change_left" + - "lane_change_right" + - "external_request_lane_change_left" + - "external_request_lane_change_right" + slot3: + - "goal_planner" + slot4: + - "dynamic_obstacle_avoidance" + external_request_lane_change_left: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true - keep_last: false - priority: 6 external_request_lane_change_right: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true - keep_last: false - priority: 6 lane_change_left: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true - keep_last: false - priority: 5 lane_change_right: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true - keep_last: false - priority: 5 start_planner: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false - keep_last: false - priority: 0 side_shift: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false - keep_last: false - priority: 2 goal_planner: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false - keep_last: true - priority: 1 static_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false - keep_last: false - priority: 4 avoidance_by_lane_change: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false - keep_last: false - priority: 3 dynamic_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true - keep_last: false - priority: 7 sampling_planner: enable_module: true enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false - keep_last: true - priority: 16 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md index 09627d2d5d91e..9cf7abfd322ed 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -46,6 +46,31 @@ Additionally, the manager generates root reference path, and if any other module ![manager_overview](../image/manager/manager_overview.svg) +### Slot + +The manager owns several containers of sub-managers, namely _slots_, that holds/runs several sub-managers and send the output to the next slot. Given the initial reference path, each slot processes the input path and the ouptut path is processed by the next slot. The final slot output is utilized as the output of the manager. The slot passes following information + +```cpp +struct SlotOutput +{ + BehaviorModuleOutput valid_output; + + // if candidate module is running, valid_output contains the planning by candidate module. In + // that case, downstream slots will just run aproved modules and do not try to launch new + // modules + bool is_upstream_candidate_exclusive{false}; + + // if this slot failed, downstream slots need to refresh approved/candidate modules and just + // forward valid_output of this slot output + bool is_upstream_failed_approved{false}; + + // if the approved module in this slot returned to isWaitingApproval, downstream slots need to + // refresh candidate once + bool is_upstream_waiting_approved{false}; +}; + +``` + ### Sub-managers The sub-manager's main task is @@ -136,13 +161,13 @@ There are 6 steps in one process: At first, the manager set latest planner data, and run all approved modules and get output path. At this time, the manager checks module status and removes expired modules from approved modules stack. -![process_step1](../image/manager/process_step1.svg) +![process_step1](../image/manager/process_step1.drawio.svg) ### Step2 Input approved modules output and necessary data to all registered modules, and the modules judge the necessity of path modification based on it. The manager checks which module makes execution request. -![process_step2](../image/manager/process_step2.svg) +![process_step2](../image/manager/process_step2.drawio.svg) ### Step3 @@ -152,19 +177,19 @@ Check request module existence. The manager decides which module to execute as candidate modules from the modules that requested to execute path modification. -![process_step4](../image/manager/process_step4.svg) +![process_step4](../image/manager/process_step4.drawio.svg) ### Step5 Decides the priority order of execution among candidate modules. And, run all candidate modules. Each modules outputs reference path and RTC cooperate status. -![process_step5](../image/manager/process_step5.svg) +![process_step5](../image/manager/process_step5.drawio.svg) ### Step6 Move approved module to approved modules stack from candidate modules stack. -![process_step6](../image/manager/process_step6.svg) +![process_step6](../image/manager/process_step6.drawio.svg) --- @@ -377,20 +402,20 @@ If a module that doesn't support simultaneous execution exists in approved modul For example, if approved module's setting of `enable_simultaneous_execution_as_approved_module` is **ENABLE**, then only modules whose the setting is **ENABLE** proceed to the next step. -![request_step2](../image/manager/request_step2.svg) +![request_step2](../image/manager/request_step2.drawio.svg) Other examples: -| Process | Description | -| :------------------------------------------------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ![request_step2-2](../image/manager/request_step2-2.svg) | If approved modules stack is empty, then all request modules proceed to the next step, regardless of the setting of `enable_simultaneous_execution_as_approved_module`. | -| ![request_step2-3](../image/manager/request_step2-3.svg) | If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded. | +| Process | Description | +| :-------------------------------------------------------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ![request_step2-2](../image/manager/request_step2-2.drawio.svg) | If approved modules stack is empty, then all request modules proceed to the next step, regardless of the setting of `enable_simultaneous_execution_as_approved_module`. | +| ![request_step2-3](../image/manager/request_step2-3.drawio.svg) | If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded. | ### Step3 Sort `request_modules` by priority. -![request_step3](../image/manager/request_step3.svg) +![request_step3](../image/manager/request_step3.drawio.svg) ### Step4 @@ -444,26 +469,26 @@ Executable or not: For example, if the highest priority module's setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE**, then all modules after the second priority are discarded. -![request_step4](../image/manager/request_step4.svg) +![request_step4](../image/manager/request_step4.drawio.svg) Other examples: -| Process | Description | -| :------------------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ![request_step4-2](../image/manager/request_step4-2.svg) | If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. | -| ![request_step4-3](../image/manager/request_step4-3.svg) | If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step. | +| Process | Description | +| :-------------------------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ![request_step4-2](../image/manager/request_step4-2.drawio.svg) | If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. | +| ![request_step4-3](../image/manager/request_step4-3.drawio.svg) | If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step. | ### Step5 Run all candidate modules. -![request_step5](../image/manager/request_step5.svg) +![request_step5](../image/manager/request_step5.drawio.svg) ## How to decide which module's output to use? Sometimes, multiple candidate modules are running simultaneously. -![multiple_candidates](../image/manager/multiple_candidates.svg) +![multiple_candidates](../image/manager/multiple_candidates.drawio.svg) In this case, the manager selects a candidate modules which output path is used as `behavior_path_planner` output by approval condition in the following rules. @@ -482,11 +507,11 @@ In this case, the manager selects a candidate modules which output path is used The smaller the number is, the higher the priority is.
- ![module_select](../image/manager/module_select.svg){width=1000} + ![module_select](../image/manager/module_select.drawio.svg){width=1000}
module priority
-![output_module](../image/manager/output_module.svg) +![output_module](../image/manager/output_module.drawio.svg) Additionally, the manager moves the highest priority module to approved modules stack if it is already approved. @@ -502,7 +527,9 @@ This is because module C is planning output path with the output of module B as As a result, the module A's output is used as approved modules stack. -![waiting_approve](../image/manager/waiting_approve.svg) +![waiting_approve](../image/manager/waiting_approve.drawio.svg) + +If this case happened in the slot, `is_upstream_waiting_approved` is set to true. ### Failure modules @@ -510,24 +537,45 @@ The failure modules return the status `ModuleStatus::FAILURE`. The manager remov As a result, the module A's output is used as approved modules stack. -![failure_modules](../image/manager/failure_modules.svg) +![failure_modules](../image/manager/failure_modules.drawio.svg) + +If this case happened in the slot, `is_upstream_failed_approved` is set to true. ### Succeeded modules The succeeded modules return the status `ModuleStatus::SUCCESS`. The manager removes those modules based on **Last In First Out** policy. In other words, if a module added later to approved modules stack is still running (is in `ModuleStatus::RUNNING`), the manager doesn't remove the succeeded module. The reason for this is the same as in removal for waiting approval modules, and is to prevent sudden changes of the running module's output. -![success_modules_remove](../image/manager/success_modules_remove.svg) +![success_modules_remove](../image/manager/success_modules_remove.drawio.svg) -![success_modules_skip](../image/manager/success_modules_skip.svg) +![success_modules_skip](../image/manager/success_modules_skip.drawio.svg) As an exception, if **Lane Change** module returns status `ModuleStatus::SUCCESS`, the manager doesn't remove any modules until all modules is in status `ModuleStatus::SUCCESS`. This is because when the manager removes the **Lane Change** (normal LC, external LC, avoidance by LC) module as succeeded module, the manager updates the information of the lane Ego is currently driving in, so root reference path (= module A's input path) changes significantly at that moment. -![lane_change_remove](../image/manager/lane_change_remove.svg) +![lane_change_remove](../image/manager/lane_change_remove.drawio.svg) -![lane_change_skip](../image/manager/lane_change_skip.svg) +![lane_change_skip](../image/manager/lane_change_skip.drawio.svg) When the manager removes succeeded modules, the last added module's output is used as approved modules stack. +## Slot output propagation + +As the initial solution, following _SlotOutput_ is passed to the first slot. + +```cpp + SlotOutput result_output = SlotOutput{ + getReferencePath(data), + false, + false, + false, + }; +``` + +If a slot turned out to be `is_upstream_failed_approved`, then all the subsequent slots are refreshed and have all of their approved_modules and candidate_modules cleared. The valid_output is just transferred to the end without any modification. + +If a slot turned out to be `is_upstream_waiting_approved`, then all the subsequent slots clear their candidate_modules once and apply their approved_modules to obtain the slot output. + +If a slot turned out to be `is_upstream_candidate_exclusive`, it means that a not `simultaneously_executable_as_candidate` module is running in that slot. Then all the subsequent modules just apply their approved_modules without trying to launch new candidate_modules. + ## Reference path generation The reference path is generated from the centerline of the **lanelet sequence** obtained from the **current route lanelet**, and it is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running. @@ -545,7 +593,7 @@ The **current route lanelet** can be reset to the closest lanelet within the rou ![current_route_lanelet](../image/manager/current_route_lanelet.svg) -The manager needs to know the ego behavior and then generate a root reference path from the lanes that Ego should follow. +The manager needs to know the ego behavior and then generate a root reference path from the lanes tghat Ego should follow. For example, during autonomous driving, even if Ego moves into the next lane in order to avoid a parked vehicle, the target lanes that Ego should follow will **NOT** change because Ego will return to the original lane after the avoidance maneuver. Therefore, the manager does **NOT** reset the **current route lanelet**, even if the avoidance maneuver is finished. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.drawio.svg similarity index 73% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.drawio.svg index 78653e8f9170b..afe1842e46595 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/failure_modules.drawio.svg @@ -1,25 +1,26 @@ + - - - + + +
- approved modules stack + approved modules stack - +
- candidate modules stack + candidate modules stack - - - - + + + + -
+
- previous module output + previous module output - +
- scene module A + scene module A - - + + -
+
- previous module output + previous module output - +
- scene module B + scene module B - +
- scene module D + scene module D - - - + + +
- scene module C + scene module C - +
- ModuleStatus::FAILURE + ModuleStatus::FAILURE - +
- approved modules stack + approved modules stack - +
- candidate modules stack + candidate modules stack - - - + + +
- scene module B + scene module B - +
- scene module D + scene module D - - - - + + + +
- scene module A + scene module A - +
- scene module C + scene module C - - + + - +
- Remove. + Remove. - +
- Remove. + Remove. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.drawio.svg similarity index 89% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.drawio.svg index 26dd05637b5da..32cc053064913 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_remove.drawio.svg @@ -1,13 +1,14 @@ + @@ -53,8 +54,8 @@ candidate modules stack - - + + @@ -98,7 +99,7 @@ -
+
- previous module output + previous module output @@ -151,8 +152,8 @@ scene module D - - + + @@ -349,7 +350,7 @@ -
+
- driving lane data + driving lane data @@ -399,7 +400,7 @@
- driving lane data + driving lane data @@ -453,8 +454,8 @@ ModuleStatus::SUCCESS - - + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.drawio.svg similarity index 92% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.drawio.svg index c3741351798c1..f35efebea0695 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/lane_change_skip.drawio.svg @@ -1,4 +1,5 @@ + @@ -98,7 +99,7 @@ -
+
- previous module output + previous module output @@ -269,7 +270,7 @@ -
+
- driving lane data + driving lane data @@ -359,7 +360,7 @@
- driving lane data + driving lane data @@ -443,7 +444,7 @@
- previous module output + previous module output diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.drawio.svg similarity index 84% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.drawio.svg index 5033a54e480e6..88e9365128a1a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/module_select.drawio.svg @@ -1,6 +1,15 @@ + - + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.drawio.svg similarity index 82% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.drawio.svg index 54b5b7840995b..6f666aed8f1e4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.drawio.svg @@ -1,13 +1,14 @@ + @@ -93,12 +94,12 @@ - - + + -
+
- approved module output + approved module output diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.drawio.svg similarity index 85% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.drawio.svg index ec3079114f4ea..f3bf8866d7f00 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/output_module.drawio.svg @@ -1,13 +1,14 @@ + @@ -135,8 +136,8 @@ - - + + @@ -156,7 +157,7 @@
- approved module output + approved module output diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.drawio.svg new file mode 100644 index 0000000000000..4e72c4a87faea --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.drawio.svg @@ -0,0 +1,554 @@ + + + + + + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + +
+
+
+ Slots +
+
+
+ + Slots + + + + + + +
+
+
+ Slot 1 +
+
+
+ + Slot 1 + + + + + + +
+
+
+ Slot 2 +
+
+
+ + Slot 2 + + + + + + +
+
+
+ Slot 3 +
+
+
+ + Slot 3 + + + + + + +
+
+
+ Slot 4 +
+
+
+ + Slot 4 + + + + + + +
+
+
+ Output +
+ path +
+
+
+ + Output... + + + + + + +
+
+
+ The manager passes lateest +
+ planner data and initial path +
+ to the slots +
+
+
+ + The manager passes lateest... + + + + + + +
+
+
+ The manager receives the output path as the final solution +
+
+
+ + The manager receives the output path a... + + + + + + +
+
+
+ Slot +
+
+
+ + Slot + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + +
+
+
+ previous module output +
+
+
+ + previous module output + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + + +
+
+
+ previous slot +
+ output +
+
+
+ + previous slot... + + + + + + +
+
+
+ The manager run all approved modules. +
+
+
+ + The manager run all approved modules. + + + + + + +
+
+
+ Get approved modules' output. +
+
+
+ + Get approved modules... + + + + + + +
+
+
+ Initial +
+ path +
+
+
+ + Initial... + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.svg deleted file mode 100644 index c8bc12a070d0d..0000000000000 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step1.svg +++ /dev/null @@ -1,322 +0,0 @@ - - - - - - - - - -
-
-
- manager -
-
-
- - manager - - - - - - -
-
-
- sub-managers -
-
-
- - sub-managers - - - - - - -
-
-
- sub-manager A -
-
-
- - sub-manager A - - - - - - -
-
-
- sub-manager B -
-
-
- - sub-manager B - - - - - - -
-
-
- approved modules stack -
-
-
- - approved modules stack - - - - - - -
-
-
- sub-manager C -
-
-
- - sub-manager C - - - - - - -
-
-
- sub-manager D -
-
-
- - sub-manager D - - - - - - -
-
-
- candidate modules stack -
-
-
- - candidate modules stack - - - - - - - - -
-
-
- scene module A -
-
-
- - scene module A - - - - - - - -
-
-
- previous module output -
-
-
- - previous module output - - - - - - -
-
-
- scene module B -
-
-
- - scene module B - - - - - - -
-
-
- scene module D -
-
-
- - scene module D - - - - - - - - - - -
-
-
- generate root reference path -
-
-
- - generate root refere... - - - - - - -
-
-
- The manager run all approved modules. -
-
-
- - The manager run all approved modules. - - - - - - -
-
-
- Get approved modules' output. -
-
-
- - Get approved modules... - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.drawio.svg similarity index 75% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.drawio.svg index b387c73e3165f..b21b6a68f3fae 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step2.drawio.svg @@ -1,41 +1,42 @@ + - +
- manager + Slot
- manager + Slot - +
@@ -44,16 +45,16 @@
- sub-managers + sub-managers - +
- sub-manager A + sub-manager A - +
- sub-manager B + sub-manager B - +
- approved modules stack + approved modules stack - +
- sub-manager C + sub-manager C - +
- sub-manager D + sub-manager D - + -
+
planner @@ -161,16 +162,16 @@
- planner... + planner... - +
- candidate modules stack + candidate modules stack - - - + + +
- scene module A + scene module A - - + + -
+
- previous module output + previous module output - +
- scene module B + scene module B - +
- scene module D + scene module D -
+
module @@ -285,41 +286,43 @@
- module... + module... - - - - - + + + + +
- generate root reference path + previous slot +
+ output
- generate root refere... + previous slot... - - + +
- The manager pass latest plan... + The manager pass latest plan... - +
- The manager receives module'... + The manager receives module'... - +
- Execution request!!! + Execution request!!! - +
- Execution request!!! + Execution request!!! diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.drawio.svg similarity index 90% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.drawio.svg index 06536181e6334..880bb4449ec77 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step4.drawio.svg @@ -1,4 +1,5 @@ + @@ -21,12 +22,12 @@ >
- manager + Slot
- manager + Slot @@ -310,7 +311,7 @@ - + @@ -322,12 +323,14 @@
- generate root reference path + previous slot +
+ output
- generate root refere... + previous slot... diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.drawio.svg similarity index 90% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.drawio.svg index 1d886e47d9ad4..997814acd9fcc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step5.drawio.svg @@ -1,13 +1,14 @@ + @@ -21,12 +22,12 @@ >
- manager + Slot
- manager + Slot @@ -195,7 +196,7 @@ -
+
- previous module output + previous module output @@ -301,7 +302,7 @@ -
+
- approved module output + approved module output - + @@ -328,12 +329,14 @@
- generate root reference path + previous module +
+ output
- generate root refere... + previous module... diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.drawio.svg similarity index 90% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.drawio.svg index cf0001737acd1..ad0fe7b843342 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/process_step6.drawio.svg @@ -1,13 +1,14 @@ + @@ -21,12 +22,12 @@ >
- manager + Slot
- manager + Slot @@ -310,7 +311,7 @@ - + @@ -322,12 +323,14 @@
- generate root reference path + previous module +
+ output
- generate root refere... + previous module... diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.drawio.svg similarity index 78% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.drawio.svg index a3e3489f0cb7f..5243deb6375cb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step1.drawio.svg @@ -1,38 +1,39 @@ + - + - +
- manager + Slot
- manager + Slot - + - +
- + - +
- + - +
- + - +
- + - +
- + - +
- + - +
- + - +
- + - +
- + - +
- + - +
- + - +
- + - +
@@ -297,9 +298,9 @@ stroke-miterlimit="10" pointer-events="all" /> - + - +
@@ -319,9 +320,9 @@ stroke-miterlimit="10" pointer-events="all" /> - + - +
- + - +
- + - +
+ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.drawio.svg similarity index 89% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.drawio.svg index 38ef0df67f3f2..82d2d400f0ddf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2-3.drawio.svg @@ -1,4 +1,5 @@ + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.drawio.svg similarity index 89% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.drawio.svg index b51d1a62cd35d..9f4097d048c22 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step2.drawio.svg @@ -1,4 +1,5 @@ + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.drawio.svg similarity index 84% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.drawio.svg index 631cf38245b17..092fce6099d95 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step3.drawio.svg @@ -1,13 +1,14 @@ + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.drawio.svg similarity index 58% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.drawio.svg index d35c8c60ec78f..4aaacf92c0f49 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-2.drawio.svg @@ -1,6 +1,15 @@ + - + @@ -8,13 +17,13 @@ - - + + - +
- request modules + request modules - - + + - +
- scene module Bsimultaneous execution as... + scene module Bsimultaneous execution as... - - + + - +
- scene module D... + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A - - + + - +
- Push back... + Push back... - - + + - +
- Discard + Discard - - + + - -
+ +
- priority: HIGH + priority: HIGH - - + + - +
- priority: LOW + priority: LOW - - + + - +
- scene module Csimultaneous execution as... + scene module Csimultaneous execution as... - - + + - +
- Push back... + Push back... diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.drawio.svg similarity index 88% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.drawio.svg index 78240c9f5a307..02fc096b54fa4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4-3.drawio.svg @@ -1,6 +1,15 @@ + - + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.drawio.svg similarity index 88% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.drawio.svg index c4b1d8d385122..0dd387d460cd9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step4.drawio.svg @@ -1,13 +1,14 @@ + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.drawio.svg similarity index 51% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.drawio.svg index 650b0b76548d1..29fb69f77be40 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/multiple_candidates.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/request_step5.drawio.svg @@ -1,23 +1,24 @@ + - +
- approved modules stack + approved modules stack - +
- candidate modules stack + candidate modules stack - - - +
- scene module A + scene module A - +
- scene module B + scene module B - + + + + -
-
-
- scene module C -
-
-
- - scene module C - - - - - - -
-
-
- scene module D -
-
-
- - scene module D - - - - - - - - - - - - - - - -
+
- approved module output + approved module output - - - + -
+
- generate root refere... - - - - - - - -
-
-
- output -
-
-
- - output + generate root refere... - - + +
- output + output - - + + + + +
-
+
- output + executing
- output + executing diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.drawio.svg similarity index 85% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.drawio.svg index 7b85f38c21e92..eefd36856e39a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/success_modules_remove.drawio.svg @@ -1,4 +1,5 @@ + @@ -53,8 +54,8 @@ candidate modules stack - - + + @@ -98,7 +99,7 @@ -
+
- previous module output + previous module output @@ -151,8 +152,8 @@ scene module D - - + + @@ -233,8 +234,8 @@ candidate modules stack - - + + @@ -296,8 +297,8 @@ scene module D - - + + + - - - + + +
- approved modules stack + approved modules stack - +
- candidate modules stack + candidate modules stack - - - - + + + + -
+
- previous module output + previous module output - +
- scene module A + scene module A - - + + -
+
- previous module output + previous module output - +
- scene module B + scene module B - +
- scene module D + scene module D - - - + + +
- scene module C + scene module C - +
- ModuleStatus::RUNNING + ModuleStatus::RUNNING - +
- candidate modules stack + candidate modules stack - +
- scene module D + scene module D - - + +
- ModuleStatus::SUCCESS + ModuleStatus::SUCCESS - +
- approved modules stack + approved modules stack - - - - + + + +
- previous module output + previous module output - +
- scene module A + scene module A - - + +
- previous module output + previous module output - +
- scene module B + scene module B - - - + + +
- scene module C + scene module C
- ModuleStatus::RUNNING + ModuleStatus::RUNNING
- ModuleStatus::SUCCESS + ModuleStatus::SUCCESS - +
- Do nothing. + Do nothing. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.svg b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.drawio.svg similarity index 86% rename from planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.svg rename to planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.drawio.svg index 41345df4f3fb1..5177ba100b2a3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/image/manager/waiting_approve.drawio.svg @@ -1,13 +1,14 @@ + @@ -53,8 +54,8 @@ candidate modules stack - - + + @@ -98,7 +99,7 @@ -
+
- previous module output + previous module output @@ -151,8 +152,8 @@ scene module D - - + + @@ -233,8 +234,8 @@ candidate modules stack - - + + @@ -295,8 +296,8 @@ scene module D - - + + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 0876d44b477a9..e46dad0210095 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -49,15 +49,14 @@ using DebugPublisher = autoware::universe_utils::DebugPublisher; using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; -enum Action { - ADD = 0, - DELETE, - MOVE, -}; - -struct ModuleUpdateInfo +struct SceneModuleUpdateInfo { - explicit ModuleUpdateInfo( + enum Action { + ADD = 0, + DELETE, + MOVE, + }; + explicit SceneModuleUpdateInfo( const SceneModulePtr & module_ptr, const Action & action, const std::string & description) : status(module_ptr->getCurrentStatus()), action(action), @@ -66,7 +65,7 @@ struct ModuleUpdateInfo { } - explicit ModuleUpdateInfo( + explicit SceneModuleUpdateInfo( const std::string & name, const Action & action, const ModuleStatus & status, const std::string & description) : status(status), action(action), module_name(name), description(description) @@ -82,6 +81,19 @@ struct ModuleUpdateInfo std::string description; }; +enum SlotStatus { + NORMAL = 0, + UPSTREAM_APPROVED_FAILED, + UPSTREAM_WAITING_APPROVED, + UPSTREAM_EXCLUSIVE_CANDIDATE +}; + +struct ModuleUpdateInfo +{ + std::vector scene_status; + std::vector slot_status; +}; + struct SceneModuleStatus { explicit SceneModuleStatus(const std::string & n) : module_name(n) {} @@ -94,6 +106,264 @@ struct SceneModuleStatus ModuleStatus status{ModuleStatus::SUCCESS}; }; +struct SlotOutput +{ + BehaviorModuleOutput valid_output; + + // if candidate module is running, valid_output contains the planning by candidate module. In + // that case, downstream slots will just run aproved modules and do not try to launch new + // modules + bool is_upstream_candidate_exclusive{false}; + + // if this slot failed, downstream slots need to refresh approved/candidate modules and just + // forward valid_output of this slot output + bool is_upstream_failed_approved{false}; + + // if the approved module in this slot returned to isWaitingApproval, downstream slots need to + // refresh candidate once + bool is_upstream_waiting_approved{false}; +}; + +class SubPlannerManager +{ +public: + explicit SubPlannerManager( + std::shared_ptr> lanelet, + std::unordered_map & processing_time, ModuleUpdateInfo & debug_info) + : current_route_lanelet_(lanelet), + processing_time_(std::ref(processing_time)), + debug_info_(std::ref(debug_info)) + { + } + + void addSceneModuleManager(const SceneModuleManagerPtr module_ptr) + { + manager_ptrs_.push_back(module_ptr); + // NOTE(Mamoru Sobue): SceneModuleManagerPtr->name() == SceneModulePtr->name() + module_priorities_[module_ptr->name()] = module_priorities_.size(); + } + + const std::vector & getSceneModuleManager() const { return manager_ptrs_; } + + /** + * @pre previous_slot_output.is_upstream_candidate_exclusive is false + * @pre previous_slot_output.is_upstream_failed_approved is false + */ + SlotOutput propagateFull( + const std::shared_ptr & planner_data, const SlotOutput & previous_slot_output); + + /** + * @brief run approved modules without launching new moules(candidates) + * @pre previous_slot_output.is_upstream_candidate_exclusive is true + * @post is_upstream_candidate_exclusive of output is true + * @post candidate_module does not increase + */ + SlotOutput propagateWithExclusiveCandidate( + const std::shared_ptr & planner_data, const SlotOutput & previous_slot_output); + + /** + * @brief delete approved/candidate modules and just forward upstream output + * @post candidate_module and approved_module get empty + */ + void propagateWithFailedApproved(); + + /** + * @pre previous_slot_output.is_upstream_waiting_approved is true + * @post is_upstream_waiting_approved of output is true + */ + SlotOutput propagateWithWaitingApproved( + const std::shared_ptr & planner_data, const SlotOutput & previous_slot_output); + + template + bool isAnyApprovedPred(F && pred) const + { + return std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [&](const auto & m) { return pred(m); }); + } + + template + bool isAnyCandidatePred(F && pred) const + { + return std::any_of( + candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), + [&](const auto & m) { return pred(m); }); + } + + const std::vector & approved_modules() const { return approved_module_ptrs_; } + + const std::vector & candidate_modules() const { return candidate_module_ptrs_; } + + /** + * @brief reset the current route lanelet to be the closest lanelet within the route + * @param planner data. + */ + void resetCurrentRouteLanelet(const std::shared_ptr & data) + { + lanelet::ConstLanelet ret{}; + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + *current_route_lanelet_ = ret; + } + + void reset() + { + approved_module_ptrs_.clear(); + candidate_module_ptrs_.clear(); + } + +private: + /** + * @brief get all modules that make execution request. + * @param decided (=approved) path. + * @return request modules. + */ + std::vector getRequestModules( + const BehaviorModuleOutput & previous_module_output, + const std::vector & deleted_modules) const; + + /** + * @brief get manager pointer from module name. + * @param module pointer. + * @return manager pointer. + */ + SceneModuleManagerPtr getManager(const SceneModulePtr & module_ptr) const + { + const auto itr = std::find_if( + manager_ptrs_.begin(), manager_ptrs_.end(), + [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); }); + + if (itr == manager_ptrs_.end()) { + throw std::domain_error("unknown manager name."); + } + + return *itr; + } + + /** + * @brief select a module that should be execute at first. + * @param modules that make execution request. + * @return the highest priority module. + */ + SceneModulePtr selectHighestPriorityModule(std::vector & request_modules) const; + + /** + * @brief swap the modules order based on it's priority. + * @param modules. + * @details for now, the priority is decided in config file and doesn't change runtime. + */ + void sortByPriority(std::vector & modules) const + { + // NOTE(Mamoru Sobue): SceneModuleManagerPtr->name() == SceneModulePtr->name() + std::sort(modules.begin(), modules.end(), [this](auto a, auto b) { + const auto a_it = module_priorities_.find(a->name()); + const auto b_it = module_priorities_.find(b->name()); + if (a_it == module_priorities_.end() || b_it == module_priorities_.end()) { + throw std::domain_error("unknown module name"); + } + return a_it->second < b_it->second; + }); + } + + /** + * @brief stop and unregister the module from manager. + * @param module. + */ + void deleteExpiredModules(SceneModulePtr & module_ptr) const + { + module_ptr->onExit(); + module_ptr->publishObjectsOfInterestMarker(); + module_ptr.reset(); + } + + /** + * @brief stop and remove all modules in candidate_module_ptrs_. + */ + void clearCandidateModules() + { + std::for_each(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) { + deleteExpiredModules(m); + }); + candidate_module_ptrs_.clear(); + } + + /** + * @brief stop and remove all modules in approved_module_ptrs_. + */ + void clearApprovedModules(); + + /** + * @brief update candidate_module_ptrs_ based on latest request modules. + * @param the highest priority module in latest request modules. + */ + void updateCandidateModules( + const std::vector & request_modules, + const SceneModulePtr & highest_priority_module); + + /** + * @brief run the module and publish RTC status. + * @param module. + * @param planner data. + * @return planning result. + */ + BehaviorModuleOutput run( + const SceneModulePtr & module_ptr, const std::shared_ptr & planner_data, + const BehaviorModuleOutput & previous_module_output) const; + + /** + * @brief checks whether a path of trajectory has forward driving direction + * @param modules that make execution request. + * @param planner data. + * @param decided (=approved) path. + * @return the highest priority module in request modules, and it's planning result. + */ + std::pair runRequestModules( + const std::vector & request_modules, const std::shared_ptr & data, + const BehaviorModuleOutput & previous_module_output); + + /** + * @brief run all modules in approved_module_ptrs_ and get a planning result as + * approved_modules_output. + * @param planner data. + * @return valid planning result. + * @details in this function, expired modules (ModuleStatus::FAILURE or ModuleStatus::SUCCESS) are + * removed from approved_module_ptrs_. + */ + SlotOutput runApprovedModules( + const std::shared_ptr & data, const BehaviorModuleOutput & upstream_slot_output); + + /** + * @brief push back to approved_module_ptrs_. + * @param approved module pointer. + */ + void addApprovedModule(const SceneModulePtr & module_ptr) + { + approved_module_ptrs_.push_back(module_ptr); + } + + bool isAnyCandidateExclusive() const + { + return std::any_of( + candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), + [&](const auto & m) { return !getManager(m)->isSimultaneousExecutableAsCandidateModule(); }); + } + + std::vector manager_ptrs_; + + std::unordered_map module_priorities_; + + // all the slots share the current_route_lanelet of PlannerManager, so if one of the slot changed + // it, it is reflected to later calculations as well + std::shared_ptr> current_route_lanelet_{nullptr}; + + std::unordered_map & processing_time_; + + std::vector approved_module_ptrs_; + + std::vector candidate_module_ptrs_; + + ModuleUpdateInfo & debug_info_; +}; + class PlannerManager { public: @@ -112,23 +382,28 @@ class PlannerManager */ void launchScenePlugin(rclcpp::Node & node, const std::string & name); - /** - * @brief calculate max iteration numbers. - * Let N be the number of scene modules. The maximum number of iterations executed in a loop is N, - * but after that, if there are any modules that have succeeded or failed, the approve_modules of - * all modules are cleared, and the loop is executed for N-1 modules. As this process repeats, it - * becomes N + (N-1) + (N-2) + … + 1, therefore the maximum number of iterations is N(N+1)/2. - * @param number of scene module - * - */ - void calculateMaxIterationNum(const size_t scene_module_num); + void configureModuleSlot(const std::vector> & slot_configuration); - /** - * @brief unregister managers. - * @param node. - * @param plugin name. - */ - void removeScenePlugin(rclcpp::Node & node, const std::string & name); + std::vector approved_modules() const + { + std::vector modules; + for (const auto & planner_manager_slot : planner_manager_slots_) { + const auto & sub_modules = planner_manager_slot.approved_modules(); + std::copy(sub_modules.begin(), sub_modules.end(), std::back_inserter(modules)); + } + + return modules; + } + + std::vector candidate_modules() const + { + std::vector modules; + for (const auto & planner_manager_slot : planner_manager_slots_) { + const auto & sub_modules = planner_manager_slot.candidate_modules(); + std::copy(sub_modules.begin(), sub_modules.end(), std::back_inserter(modules)); + } + return modules; + } /** * @brief update module parameters. used for dynamic reconfigure. @@ -147,10 +422,11 @@ class PlannerManager void reset() { std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); }); - approved_module_ptrs_.clear(); - candidate_module_ptrs_.clear(); - current_route_lanelet_ = std::nullopt; + *current_route_lanelet_ = std::nullopt; resetProcessingTime(); + for (auto & planner_manager_slot : planner_manager_slots_) { + planner_manager_slot.reset(); + } } /** @@ -185,18 +461,20 @@ class PlannerManager { std::vector> ret; - const auto size = approved_module_ptrs_.size() + candidate_module_ptrs_.size(); + const auto approved_module_ptrs = approved_modules(); + const auto candidate_module_ptrs = candidate_modules(); + const auto size = approved_module_ptrs.size() + candidate_module_ptrs.size(); ret.reserve(size); - for (const auto & m : approved_module_ptrs_) { + for (const auto & m : approved_module_ptrs) { auto s = std::make_shared(m->name()); s->is_waiting_approval = m->isWaitingApproval(); s->status = m->getCurrentStatus(); ret.push_back(s); } - for (const auto & m : candidate_module_ptrs_) { + for (const auto & m : candidate_module_ptrs) { auto s = std::make_shared(m->name()); s->is_waiting_approval = m->isWaitingApproval(); s->status = m->getCurrentStatus(); @@ -216,40 +494,31 @@ class PlannerManager stop_reason_array.header.frame_id = "map"; stop_reason_array.header.stamp = clock_.now(); - std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { + const auto approved_module_ptrs = approved_modules(); + const auto candidate_module_ptrs = candidate_modules(); + + std::for_each(approved_module_ptrs.begin(), approved_module_ptrs.end(), [&](const auto & m) { const auto reason = m->getStopReason(); if (reason.reason != "") { stop_reason_array.stop_reasons.push_back(m->getStopReason()); } }); - std::for_each( - candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [&](const auto & m) { - const auto reason = m->getStopReason(); - if (reason.reason != "") { - stop_reason_array.stop_reasons.push_back(m->getStopReason()); - } - }); + std::for_each(candidate_module_ptrs.begin(), candidate_module_ptrs.end(), [&](const auto & m) { + const auto reason = m->getStopReason(); + if (reason.reason != "") { + stop_reason_array.stop_reasons.push_back(m->getStopReason()); + } + }); return stop_reason_array; } /** - * @brief check if there are approved modules. - */ - bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); } - - bool hasNonAlwaysExecutableApprovedModules() const - { - return std::any_of( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); - } - - /** - * @brief check if there are candidate modules. + * @brief check if reroutable approved module is running(namely except for fixed_goal_planner and + * dynamic_avoidance) */ - bool hasCandidateModules() const { return !candidate_module_ptrs_.empty(); } + bool hasPossibleRerouteApprovedModules(const std::shared_ptr & data) const; /** * @brief show planner manager internal condition. @@ -275,43 +544,10 @@ class PlannerManager lanelet::ConstLanelet ret{}; data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); RCLCPP_DEBUG(logger_, "update current route lanelet. id:%ld", ret.id()); - current_route_lanelet_ = ret; + *current_route_lanelet_ = ret; } private: - /** - * @brief run the module and publish RTC status. - * @param module. - * @param planner data. - * @return planning result. - */ - BehaviorModuleOutput run( - const SceneModulePtr & module_ptr, const std::shared_ptr & planner_data, - const BehaviorModuleOutput & previous_module_output) const - { - universe_utils::ScopedTimeTrack st(module_ptr->name() + "=>run", *module_ptr->getTimeKeeper()); - stop_watch_.tic(module_ptr->name()); - - module_ptr->setData(planner_data); - module_ptr->setPreviousModuleOutput(previous_module_output); - - module_ptr->lockRTCCommand(); - const auto result = module_ptr->run(); - module_ptr->unlockRTCCommand(); - - module_ptr->postProcess(); - - module_ptr->updateCurrentState(); - - module_ptr->publishSteeringFactor(); - - module_ptr->publishObjectsOfInterestMarker(); - - processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true); - - return result; - } - /** * @brief find and set the closest lanelet within the route to current route lanelet * @param planner data. @@ -333,39 +569,6 @@ class PlannerManager */ void publishDebugRootReferencePath(const BehaviorModuleOutput & reference_path) const; - /** - * @brief stop and unregister the module from manager. - * @param module. - */ - void deleteExpiredModules(SceneModulePtr & module_ptr) const - { - module_ptr->onExit(); - module_ptr->publishObjectsOfInterestMarker(); - module_ptr.reset(); - } - - /** - * @brief swap the modules order based on it's priority. - * @param modules. - * @details for now, the priority is decided in config file and doesn't change runtime. - */ - void sortByPriority(std::vector & modules) const - { - // TODO(someone) enhance this priority decision method. - std::sort(modules.begin(), modules.end(), [this](auto a, auto b) { - return getManager(a)->getPriority() < getManager(b)->getPriority(); - }); - } - - /** - * @brief push back to approved_module_ptrs_. - * @param approved module pointer. - */ - void addApprovedModule(const SceneModulePtr & module_ptr) - { - approved_module_ptrs_.push_back(module_ptr); - } - /** * @brief reset processing time. */ @@ -376,121 +579,30 @@ class PlannerManager } } - /** - * @brief stop and remove all modules in candidate_module_ptrs_. - */ - void clearCandidateModules() - { - std::for_each(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) { - deleteExpiredModules(m); - }); - candidate_module_ptrs_.clear(); - } - - /** - * @brief get manager pointer from module name. - * @param module pointer. - * @return manager pointer. - */ - SceneModuleManagerPtr getManager(const SceneModulePtr & module_ptr) const - { - const auto itr = std::find_if( - manager_ptrs_.begin(), manager_ptrs_.end(), - [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); }); - - if (itr == manager_ptrs_.end()) { - throw std::domain_error("unknown manager name."); - } - - return *itr; - } - - /** - * @brief run all modules in approved_module_ptrs_ and get a planning result as - * approved_modules_output. - * @param planner data. - * @param deleted modules. - * @return valid planning result. - * @details in this function, expired modules (ModuleStatus::FAILURE or ModuleStatus::SUCCESS) are - * removed from approved_module_ptrs_ and added to deleted_modules. - */ - BehaviorModuleOutput runApprovedModules( - const std::shared_ptr & data, std::vector & deleted_modules); - - /** - * @brief select a module that should be execute at first. - * @param modules that make execution request. - * @return the highest priority module. - */ - SceneModulePtr selectHighestPriorityModule(std::vector & request_modules) const; - - /** - * @brief update candidate_module_ptrs_ based on latest request modules. - * @param the highest priority module in latest request modules. - */ - void updateCandidateModules( - const std::vector & request_modules, - const SceneModulePtr & highest_priority_module); + // NOTE: current_route_lanelet_ is shared with SubPlannerManagers + std::shared_ptr> current_route_lanelet_; - /** - * @brief get all modules that make execution request. - * @param decided (=approved) path. - * @param deleted modules. - * @return request modules. - */ - std::vector getRequestModules( - const BehaviorModuleOutput & previous_module_output, - const std::vector & deleted_modules) const; - - /** - * @brief checks whether a path of trajectory has forward driving direction - * @param modules that make execution request. - * @param planner data. - * @param decided (=approved) path. - * @return the highest priority module in request modules, and it's planning result. - */ - std::pair runRequestModules( - const std::vector & request_modules, const std::shared_ptr & data, - const BehaviorModuleOutput & previous_module_output); - - /** - * @brief run keep last approved modules - * @param planner data. - * @param previous module output. - * @return planning result. - */ - BehaviorModuleOutput runKeepLastModules( - const std::shared_ptr & data, const BehaviorModuleOutput & previous_output) const; - - static std::string getNames(const std::vector & modules); - - std::optional current_route_lanelet_{std::nullopt}; + // NOTE: SubPlannerManager::manager_ptrs_ and manager_ptrs_ share the same SceneModuleManager + // instance as shared_ptr + std::vector planner_manager_slots_; std::vector manager_ptrs_; - std::vector approved_module_ptrs_; - - std::vector candidate_module_ptrs_; - std::unique_ptr debug_publisher_ptr_; std::unique_ptr state_publisher_ptr_; pluginlib::ClassLoader plugin_loader_; - mutable rclcpp::Logger logger_; + rclcpp::Logger logger_; mutable rclcpp::Clock clock_; - mutable StopWatch stop_watch_; - - mutable std::unordered_map processing_time_; - - mutable std::vector debug_info_; + std::unordered_map processing_time_; - mutable std::shared_ptr debug_msg_ptr_; + ModuleUpdateInfo debug_info_; - size_t max_iteration_num_{100}; + std::shared_ptr debug_msg_ptr_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index bf323e76912d4..135e420d113f3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -68,18 +68,28 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const std::lock_guard lock(mutex_manager_); // for planner_manager_ + const auto slots = declare_parameter>("slots"); + std::vector> slot_configuration{slots.size()}; + for (size_t i = 0; i < slots.size(); ++i) { + const auto & slot = slots.at(i); + const auto modules = declare_parameter>(slot); + for (const auto & module_name : modules) { + slot_configuration.at(i).push_back(module_name); + } + } + planner_manager_ = std::make_shared(*this); - size_t scene_module_num = 0; for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. if (name == "") { break; } planner_manager_->launchScenePlugin(*this, name); - scene_module_num++; } - planner_manager_->calculateMaxIterationNum(scene_module_num); + + // NOTE: this needs to be after launchScenePlugin() + planner_manager_->configureModuleSlot(slot_configuration); for (const auto & manager : planner_manager_->getSceneModuleManagers()) { path_candidate_publishers_.emplace( @@ -419,7 +429,7 @@ void BehaviorPathPlannerNode::run() planner_data_->operation_mode->is_autoware_control_enabled; if ( !controlled_by_autoware_autonomously && - !planner_manager_->hasNonAlwaysExecutableApprovedModules()) + !planner_manager_->hasPossibleRerouteApprovedModules(planner_data_)) planner_manager_->resetCurrentRouteLanelet(planner_data_); // run behavior planner @@ -548,7 +558,7 @@ void BehaviorPathPlannerNode::publish_reroute_availability() const // always-executable module is approved and running, rerouting will not be possible. RerouteAvailability is_reroute_available; is_reroute_available.stamp = this->now(); - if (planner_manager_->hasNonAlwaysExecutableApprovedModules()) { + if (planner_manager_->hasPossibleRerouteApprovedModules(planner_data_)) { is_reroute_available.availability = false; } else { is_reroute_available.availability = true; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 5278ea8e76981..dc6f7552c5a61 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -37,6 +37,7 @@ PlannerManager::PlannerManager(rclcpp::Node & node) logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()) { + current_route_lanelet_ = std::make_shared>(std::nullopt); processing_time_.emplace("total_time", 0.0); debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); state_publisher_ptr_ = std::make_unique(&node, "~/debug"); @@ -65,151 +66,123 @@ void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & } } -void PlannerManager::calculateMaxIterationNum(const size_t scene_module_num) +void PlannerManager::configureModuleSlot( + const std::vector> & slot_configuration) { - max_iteration_num_ = scene_module_num * (scene_module_num + 1) / 2; -} - -void PlannerManager::removeScenePlugin(rclcpp::Node & node, const std::string & name) -{ - auto it = std::remove_if(manager_ptrs_.begin(), manager_ptrs_.end(), [&](const auto plugin) { - return plugin->name() == name; - }); + std::unordered_map registered_modules; + for (const auto & manager_ptr : manager_ptrs_) { + registered_modules[manager_ptr->name()] = manager_ptr; + } - if (it == manager_ptrs_.end()) { - RCLCPP_WARN_STREAM( - node.get_logger(), - "The scene plugin '" << name << "' is not found in the registered modules."); - } else { - manager_ptrs_.erase(it, manager_ptrs_.end()); - processing_time_.erase(name); - RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded."); + for (const auto & slot : slot_configuration) { + SubPlannerManager sub_manager(current_route_lanelet_, processing_time_, debug_info_); + for (const auto & module_name : slot) { + if (const auto it = registered_modules.find(module_name); it != registered_modules.end()) { + sub_manager.addSceneModuleManager(it->second); + } else { + // TODO(Mamoru Sobue): use LOG + std::cout << module_name << " registered in slot_configuration is not registered, skipping" + << std::endl; + } + } + if (sub_manager.getSceneModuleManager().size() != 0) { + planner_manager_slots_.push_back(sub_manager); + // TODO(Mamoru Sobue): use LOG + std::cout << "added a slot with " << sub_manager.getSceneModuleManager().size() << " modules" + << std::endl; + } } } BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & data) { resetProcessingTime(); - stop_watch_.tic("total_time"); - debug_info_.clear(); + StopWatch stop_watch; + stop_watch.tic("total_time"); + BOOST_SCOPE_EXIT((&processing_time_)(&stop_watch)) + { + processing_time_.at("total_time") += stop_watch.toc("total_time", true); + } + BOOST_SCOPE_EXIT_END; - if (!current_route_lanelet_) resetCurrentRouteLanelet(data); + debug_info_.scene_status.clear(); + debug_info_.slot_status.clear(); + + if (!current_route_lanelet_->has_value()) resetCurrentRouteLanelet(data); std::for_each( manager_ptrs_.begin(), manager_ptrs_.end(), [&data](const auto & m) { m->setData(data); }); - auto result_output = [&]() { - const bool is_any_approved_module_running = - std::any_of(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) { - return m->getCurrentStatus() == ModuleStatus::RUNNING || - m->getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + const bool is_any_approved_module_running = std::any_of( + planner_manager_slots_.begin(), planner_manager_slots_.end(), [&](const auto & slot) { + return slot.isAnyApprovedPred([](const auto & m) { + const auto status = m->getCurrentStatus(); + return status == ModuleStatus::RUNNING || status == ModuleStatus::WAITING_APPROVAL; }); + }); - // IDLE is a state in which an execution has been requested but not yet approved. - // once approved, it basically turns to running. - const bool is_any_candidate_module_running_or_idle = - std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](const auto & m) { - return m->getCurrentStatus() == ModuleStatus::RUNNING || - m->getCurrentStatus() == ModuleStatus::WAITING_APPROVAL || - m->getCurrentStatus() == ModuleStatus::IDLE; + // IDLE is a state in which an execution has been requested but not yet approved. + // once approved, it basically turns to running. + const bool is_any_candidate_module_running_or_idle = std::any_of( + planner_manager_slots_.begin(), planner_manager_slots_.end(), [](const auto & slot) { + return slot.isAnyCandidatePred([](const auto & m) { + const auto status = m->getCurrentStatus(); + return status == ModuleStatus::RUNNING || status == ModuleStatus::WAITING_APPROVAL || + status == ModuleStatus::IDLE; }); + }); - const bool is_any_module_running = - is_any_approved_module_running || is_any_candidate_module_running_or_idle; - - updateCurrentRouteLanelet(data); - - const bool is_out_of_route = utils::isEgoOutOfRoute( - data->self_odometry->pose.pose, current_route_lanelet_.value(), data->prev_modified_goal, - data->route_handler); + const bool is_any_module_running = + is_any_approved_module_running || is_any_candidate_module_running_or_idle; - if (!is_any_module_running && is_out_of_route) { - BehaviorModuleOutput output = utils::createGoalAroundPath(data); - generateCombinedDrivableArea(output, data); - RCLCPP_WARN_THROTTLE( - logger_, clock_, 5000, - "Ego is out of route, no module is running. Skip running scene modules."); - return output; - } - std::vector - deleted_modules; // store the scene modules deleted from approved modules - - for (size_t itr_num = 1;; ++itr_num) { - /** - * STEP1: get approved modules' output - */ - auto approved_modules_output = runApprovedModules(data, deleted_modules); - - /** - * STEP2: check modules that need to be launched - */ - const auto request_modules = getRequestModules(approved_modules_output, deleted_modules); - - /** - * STEP3: if there is no module that need to be launched, return approved modules' output - */ - if (request_modules.empty()) { - const auto output = runKeepLastModules(data, approved_modules_output); - processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return output; - } + updateCurrentRouteLanelet(data); - /** - * STEP4: if there is module that should be launched, execute the module - */ - const auto [highest_priority_module, candidate_modules_output] = - runRequestModules(request_modules, data, approved_modules_output); - - /** - * STEP5: run keep last approved modules after running candidate modules. - * NOTE: if no candidate module is launched, approved_modules_output used as input for keep - * last modules and return the result immediately. - */ - const auto output = runKeepLastModules( - data, highest_priority_module ? candidate_modules_output : approved_modules_output); - if (!highest_priority_module) { - processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return output; - } + const bool is_out_of_route = utils::isEgoOutOfRoute( + data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal, + data->route_handler); - /** - * STEP6: if the candidate module's modification is NOT approved yet, return the result. - * NOTE: the result is output of the candidate module, but the output path don't contains path - * shape modification that needs approval. On the other hand, it could include velocity - * profile modification. - */ - if (highest_priority_module->isWaitingApproval()) { - processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return output; - } + if (!is_any_module_running && is_out_of_route) { + BehaviorModuleOutput result_output = utils::createGoalAroundPath(data); + RCLCPP_WARN_THROTTLE( + logger_, clock_, 5000, + "Ego is out of route, no module is running. Skip running scene modules."); + generateCombinedDrivableArea(result_output, data); + return result_output; + } + std::vector + deleted_modules; // store the scene modules deleted from approved modules + + SlotOutput result_output = SlotOutput{ + getReferencePath(data), + false, + false, + false, + }; - /** - * STEP7: if the candidate module is approved, push the module into approved_module_ptrs_ - */ - addApprovedModule(highest_priority_module); - clearCandidateModules(); - debug_info_.emplace_back(highest_priority_module, Action::ADD, "To Approval"); - - if (itr_num >= max_iteration_num_) { - RCLCPP_WARN_THROTTLE( - logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.", - max_iteration_num_); - processing_time_.at("total_time") = stop_watch_.toc("total_time", true); - return output; - } + for (auto & planner_manager_slot : planner_manager_slots_) { + if (result_output.is_upstream_failed_approved) { + // clear all candidate/approved modules of all subsequent slots, and keep result_output as is + planner_manager_slot.propagateWithFailedApproved(); + debug_info_.slot_status.push_back(SlotStatus::UPSTREAM_APPROVED_FAILED); + } else if (result_output.is_upstream_waiting_approved) { + result_output = planner_manager_slot.propagateWithWaitingApproved(data, result_output); + debug_info_.slot_status.push_back(SlotStatus::UPSTREAM_WAITING_APPROVED); + } else if (result_output.is_upstream_candidate_exclusive) { + result_output = planner_manager_slot.propagateWithExclusiveCandidate(data, result_output); + debug_info_.slot_status.push_back(SlotStatus::UPSTREAM_EXCLUSIVE_CANDIDATE); + } else { + result_output = planner_manager_slot.propagateFull(data, result_output); + debug_info_.slot_status.push_back(SlotStatus::NORMAL); } - - return BehaviorModuleOutput{}; // something wrong. - }(); + } std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); m->publishRTCStatus(); }); - generateCombinedDrivableArea(result_output, data); - - return result_output; + generateCombinedDrivableArea(result_output.valid_output, data); + return result_output.valid_output; } // NOTE: To deal with some policies about drivable area generation, currently DrivableAreaInfo is @@ -255,93 +228,227 @@ void PlannerManager::generateCombinedDrivableArea( utils::extractObstaclesFromDrivableArea(output.path, di.obstacles); } -std::vector PlannerManager::getRequestModules( - const BehaviorModuleOutput & previous_module_output, - const std::vector & deleted_modules) const +void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr & data) { - if (previous_module_output.path.points.empty()) { - RCLCPP_ERROR_STREAM( - logger_, "Current module output is null. Skip candidate module check." - << "\n - Approved module list: " << getNames(approved_module_ptrs_) - << "\n - Candidate module list: " << getNames(candidate_module_ptrs_)); - return {}; + const auto & route_handler = data->route_handler; + const auto & pose = data->self_odometry->pose.pose; + const auto p = data->parameters; + + constexpr double extra_margin = 10.0; + const auto backward_length = + std::max(p.backward_path_length, p.backward_path_length + extra_margin); + + lanelet::ConstLanelet closest_lane{}; + + if (route_handler->getClosestRouteLaneletFromLanelet( + pose, current_route_lanelet_->value(), &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold)) { + *current_route_lanelet_ = closest_lane; + return; } - std::vector request_modules{}; + const auto lanelet_sequence = route_handler->getLaneletSequence( + current_route_lanelet_->value(), pose, backward_length, p.forward_path_length); + + const auto could_calculate_closest_lanelet = + lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold) || + lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane); + + if (could_calculate_closest_lanelet) + *current_route_lanelet_ = closest_lane; + else + resetCurrentRouteLanelet(data); +} + +BehaviorModuleOutput PlannerManager::getReferencePath( + const std::shared_ptr & data) const +{ + const auto reference_path = utils::getReferencePath(current_route_lanelet_->value(), data); + publishDebugRootReferencePath(reference_path); + return reference_path; +} - const auto toc = [this](const auto & name) { - processing_time_.at(name) += stop_watch_.toc(name, true); +void PlannerManager::publishDebugRootReferencePath( + const BehaviorModuleOutput & reference_path) const +{ + using visualization_msgs::msg::Marker; + MarkerArray array; + Marker m = autoware::universe_utils::createDefaultMarker( + "map", clock_.now(), "root_reference_path", 0UL, Marker::LINE_STRIP, + autoware::universe_utils::createMarkerScale(1.0, 1.0, 1.0), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); + for (const auto & p : reference_path.path.points) m.points.push_back(p.point.pose.position); + array.markers.push_back(m); + m.points.clear(); + m.id = 1UL; + for (const auto & p : current_route_lanelet_->value().polygon3d().basicPolygon()) + m.points.emplace_back().set__x(p.x()).set__y(p.y()).set__z(p.z()); + array.markers.push_back(m); + debug_publisher_ptr_->publish("root_reference_path", array); +} + +bool PlannerManager::hasPossibleRerouteApprovedModules( + const std::shared_ptr & data) const +{ + const auto & approved_module = approved_modules(); + const auto not_possible_reroute_module = [&](const SceneModulePtr m) { + if (m->name() == "dynamic_avoidance") { + return false; + } + if (m->name() == "goal_planner" && !utils::isAllowedGoalModification(data->route_handler)) { + return false; + } + return true; }; + return std::any_of(approved_module.begin(), approved_module.end(), not_possible_reroute_module); +} + +void PlannerManager::print() const +{ + const auto approved_module_ptrs = approved_modules(); + const auto candidate_module_ptrs = candidate_modules(); + + const auto get_status = [](const auto & m) { + return magic_enum::enum_name(m->getCurrentStatus()); + }; + + size_t max_string_num = 0; + + std::ostringstream string_stream; + string_stream << "\n"; + string_stream << "***********************************************************\n"; + string_stream << " planner manager status\n"; + string_stream << "-----------------------------------------------------------\n"; + string_stream << "registered modules: "; + for (const auto & m : manager_ptrs_) { + string_stream << "[" << m->name() << "]"; + max_string_num = std::max(max_string_num, m->name().length()); + } + + string_stream << "\n"; + string_stream << "approved modules : "; + std::string delimiter = ""; + for (const auto & planner_manager_slot : planner_manager_slots_) { + string_stream << std::exchange(delimiter, " ==> ") << "[[ "; + std::string delimiter_sub = ""; + for (const auto & m : planner_manager_slot.approved_modules()) { + string_stream << std::exchange(delimiter_sub, "->") << "[" << m->name() << "(" + << get_status(m) << ")" + << "]"; + } + string_stream << " ]]"; + } + + string_stream << "\n"; + string_stream << "candidate modules : "; + delimiter = ""; + for (const auto & planner_manager_slot : planner_manager_slots_) { + string_stream << std::exchange(delimiter, " ==> ") << "[[ "; + std::string delimiter_sub = ""; + for (const auto & m : planner_manager_slot.candidate_modules()) { + string_stream << std::exchange(delimiter_sub, "->") << "[" << m->name() << "(" + << get_status(m) << ")" + << "]"; + } + string_stream << " ]]"; + } + + string_stream << "\n"; + string_stream << "slot_status : "; + delimiter = ""; + for (const auto & slot_status : debug_info_.slot_status) { + string_stream << std::exchange(delimiter, "->") << "[" << magic_enum::enum_name(slot_status) + << "]"; + } + + string_stream << "\n"; + string_stream << "update module info: "; + for (const auto & i : debug_info_.scene_status) { + string_stream << "[Module:" << i.module_name << " Status:" << magic_enum::enum_name(i.status) + << " Action:" << magic_enum::enum_name(i.action) + << " Description:" << i.description << "]\n" + << std::setw(28); + } + + string_stream << "\n" << std::fixed << std::setprecision(1); + string_stream << "processing time : "; + for (const auto & t : processing_time_) { + string_stream << std::right << "[" << std::setw(static_cast(max_string_num) + 1) + << std::left << t.first << ":" << std::setw(4) << std::right << t.second + << "ms]\n" + << std::setw(21); + } + + state_publisher_ptr_->publish("internal_state", string_stream.str()); + + RCLCPP_DEBUG_STREAM(logger_, string_stream.str()); +} + +void PlannerManager::publishProcessingTime() const +{ + for (const auto & t : processing_time_) { + std::string name = t.first + std::string("/processing_time_ms"); + debug_publisher_ptr_->publish(name, t.second); + } +} + +std::shared_ptr PlannerManager::getDebugMsg() +{ + debug_msg_ptr_ = std::make_shared(); + const auto approved_module_ptrs = approved_modules(); + const auto candidate_module_ptrs = candidate_modules(); + + for (const auto & approved_module : approved_module_ptrs) { + approved_module->acceptVisitor(debug_msg_ptr_); + } + + for (const auto & candidate_module : candidate_module_ptrs) { + candidate_module->acceptVisitor(debug_msg_ptr_); + } + return debug_msg_ptr_; +} + +std::vector SubPlannerManager::getRequestModules( + const BehaviorModuleOutput & previous_module_output, + const std::vector & deleted_modules) const +{ + std::vector request_modules{}; + StopWatch stop_watch; + for (const auto & manager_ptr : manager_ptrs_) { - stop_watch_.tic(manager_ptr->name()); - /** - * skip the module that is already deleted. - */ + stop_watch.tic(manager_ptr->name()); + BOOST_SCOPE_EXIT((&manager_ptr)(&processing_time_)(&stop_watch)) { - const auto name = manager_ptr->name(); - const auto find_deleted_module = [&name](const auto & m) { return m->name() == name; }; - const auto itr = - std::find_if(deleted_modules.begin(), deleted_modules.end(), find_deleted_module); - if (itr != deleted_modules.end()) { - continue; - } + processing_time_.at(manager_ptr->name()) += stop_watch.toc(manager_ptr->name(), true); } + BOOST_SCOPE_EXIT_END; - /** - * determine the execution capability of modules based on existing approved modules. - */ - // Condition 1: always executable module can be added regardless of the existence of other - // modules, so skip checking the existence of other modules. - // in other cases, need to check the existence of other modules and which module can be added. - if (!manager_ptr->isAlwaysExecutableModule() && hasNonAlwaysExecutableApprovedModules()) { - // pairs of find_block_module and is_executable - std::vector, std::function>> - conditions; - - // Condition 2: do not add modules that are neither always nor simultaneous executable - // if there exists at least one approved module that is simultaneous but not always - // executable. (only modules that are either always executable or simultaneous executable can - // be added) - conditions.emplace_back( - [&](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }, - [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }); - - // Condition 3: do not add modules that are not always executable if there exists - // at least one approved module that is neither always nor simultaneous executable. - // (only modules that are always executable can be added) - conditions.emplace_back( - [&](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - !getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }, - [&]() { return false; }); - - bool skip_module = false; - for (const auto & condition : conditions) { - const auto & find_block_module = condition.first; - const auto & is_executable = condition.second; - - const auto itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); - - if (itr != approved_module_ptrs_.end() && !is_executable()) { - toc(manager_ptr->name()); - skip_module = true; - continue; - } - } - if (skip_module) { - continue; - } + if (const auto deleted_it = std::find_if( + deleted_modules.begin(), deleted_modules.end(), + [&](const auto & m) { return m->name() == manager_ptr->name(); }); + deleted_it != deleted_modules.end()) { + continue; + } + + // Condition 1: + // the approved module queue is either + // - consists of multiple simultaneous_executable_as_approved modules only + // - consists of only 1 "not simultaneous_executable_as_approved" module + const bool exclusive_module_exist_in_approved_pool = std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const SceneModulePtr & m) { + return !getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }); + const bool is_this_exclusive = !manager_ptr->isSimultaneousExecutableAsApprovedModule(); + const bool approved_pool_is_not_empty = !approved_module_ptrs_.empty(); + + const bool is_this_not_joinable = + exclusive_module_exist_in_approved_pool || (is_this_exclusive && approved_pool_is_not_empty); + if (is_this_not_joinable) { + continue; } - // else{ - // Condition 4: if none of the above conditions are met, any module can be added. - // (when the approved modules are either empty or consist only of always executable modules.) - // } /** * launch new candidate module. @@ -359,8 +466,6 @@ std::vector PlannerManager::getRequestModules( request_modules.emplace_back(manager_ptr->getIdleModule()); } } - - toc(manager_ptr->name()); continue; } } @@ -380,7 +485,6 @@ std::vector PlannerManager::getRequestModules( if (itr != candidate_module_ptrs_.end()) { request_modules.clear(); request_modules.emplace_back(*itr); - toc(manager_ptr->name()); break; } } @@ -396,7 +500,6 @@ std::vector PlannerManager::getRequestModules( if (itr != candidate_module_ptrs_.end()) { request_modules.emplace_back(*itr); - toc(manager_ptr->name()); continue; } } @@ -406,112 +509,99 @@ std::vector PlannerManager::getRequestModules( */ { if (!manager_ptr->canLaunchNewModule()) { - toc(manager_ptr->name()); continue; } manager_ptr->updateIdleModuleInstance(); if (!manager_ptr->isExecutionRequested(previous_module_output)) { - toc(manager_ptr->name()); continue; } request_modules.emplace_back(manager_ptr->getIdleModule()); } - - toc(manager_ptr->name()); } return request_modules; } -BehaviorModuleOutput PlannerManager::runKeepLastModules( - const std::shared_ptr & data, const BehaviorModuleOutput & previous_output) const -{ - auto output = previous_output; - std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { - if (getManager(m)->isKeepLast()) { - output = run(m, data, output); - } - }); - - return output; -} - -void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr & data) +SceneModulePtr SubPlannerManager::selectHighestPriorityModule( + std::vector & request_modules) const { - const auto & route_handler = data->route_handler; - const auto & pose = data->self_odometry->pose.pose; - const auto p = data->parameters; - - constexpr double extra_margin = 10.0; - const auto backward_length = - std::max(p.backward_path_length, p.backward_path_length + extra_margin); - - lanelet::ConstLanelet closest_lane{}; - - if (route_handler->getClosestRouteLaneletFromLanelet( - pose, current_route_lanelet_.value(), &closest_lane, p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold)) { - current_route_lanelet_ = closest_lane; - return; + if (request_modules.empty()) { + return {}; } - const auto lanelet_sequence = route_handler->getLaneletSequence( - current_route_lanelet_.value(), pose, backward_length, p.forward_path_length); - - const auto could_calculate_closest_lanelet = - lanelet::utils::query::getClosestLaneletWithConstrains( - lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold) || - lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane); + sortByPriority(request_modules); - if (could_calculate_closest_lanelet) - current_route_lanelet_ = closest_lane; - else - resetCurrentRouteLanelet(data); + return request_modules.front(); } -BehaviorModuleOutput PlannerManager::getReferencePath( - const std::shared_ptr & data) const +void SubPlannerManager::clearApprovedModules() { - const auto reference_path = utils::getReferencePath(*current_route_lanelet_, data); - publishDebugRootReferencePath(reference_path); - return reference_path; + std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [this](auto & m) { + debug_info_.scene_status.emplace_back( + m, SceneModuleUpdateInfo::Action::DELETE, "From Approved"); + deleteExpiredModules(m); + }); + approved_module_ptrs_.clear(); } -void PlannerManager::publishDebugRootReferencePath( - const BehaviorModuleOutput & reference_path) const +void SubPlannerManager::updateCandidateModules( + const std::vector & request_modules, + const SceneModulePtr & highest_priority_module) { - using visualization_msgs::msg::Marker; - MarkerArray array; - Marker m = autoware::universe_utils::createDefaultMarker( - "map", clock_.now(), "root_reference_path", 0UL, Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(1.0, 1.0, 1.0), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0)); - for (const auto & p : reference_path.path.points) m.points.push_back(p.point.pose.position); - array.markers.push_back(m); - m.points.clear(); - m.id = 1UL; - for (const auto & p : current_route_lanelet_->polygon3d().basicPolygon()) - m.points.emplace_back().set__x(p.x()).set__y(p.y()).set__z(p.z()); - array.markers.push_back(m); - debug_publisher_ptr_->publish("root_reference_path", array); -} + const auto exist = [](const auto & module_ptr, const auto & module_ptrs) { + const auto itr = std::find_if( + module_ptrs.begin(), module_ptrs.end(), + [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); }); -SceneModulePtr PlannerManager::selectHighestPriorityModule( - std::vector & request_modules) const -{ - if (request_modules.empty()) { - return {}; + return itr != module_ptrs.end(); + }; + + /** + * unregister expired modules + */ + { + const auto candidate_to_remove = [&](auto & itr) { + if (!exist(itr, request_modules)) { + deleteExpiredModules(itr); + return true; + } + return itr->name() == highest_priority_module->name() && + !highest_priority_module->isWaitingApproval(); + }; + + candidate_module_ptrs_.erase( + std::remove_if( + candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), candidate_to_remove), + candidate_module_ptrs_.end()); + + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } - sortByPriority(request_modules); + /** + * register running candidate modules + */ + for (const auto & m : request_modules) { + if ( + m->name() == highest_priority_module->name() && + !highest_priority_module->isWaitingApproval()) { + continue; + } - return request_modules.front(); + if (!exist(m, candidate_module_ptrs_)) { + candidate_module_ptrs_.push_back(m); + } + } + + /** + * sort by priority. sorted_request_modules.front() is the highest priority module. + */ + sortByPriority(candidate_module_ptrs_); } -std::pair PlannerManager::runRequestModules( +std::pair SubPlannerManager::runRequestModules( const std::vector & request_modules, const std::shared_ptr & data, const BehaviorModuleOutput & previous_module_output) { @@ -533,62 +623,27 @@ std::pair PlannerManager::runRequestModule auto sorted_request_modules = request_modules; sortByPriority(sorted_request_modules); - /** - * remove non-executable modules. - */ + // the candidate module queue is either + // - consists of multiple simultaneous_executable_as_candidate modules only + // - consists of only 1 "not simultaneous_executable_as_candidate" module for (const auto & module_ptr : sorted_request_modules) { - // Condition 1: always executable module can be added regardless of the existence of other - // modules. - if (getManager(module_ptr)->isAlwaysExecutableModule()) { + // any module can join if executable_modules is empty + const bool is_executable_modules_empty = executable_modules.empty(); + if (is_executable_modules_empty) { executable_modules.push_back(module_ptr); continue; } - // Condition 4: If the executable modules are either empty or consist only of always executable - // modules, any module can be added. - const bool has_non_always_executable_module = std::any_of( - executable_modules.begin(), executable_modules.end(), - [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); - if (!has_non_always_executable_module) { - executable_modules.push_back(module_ptr); - continue; - } + // if executable_module is not empty, only SimultaneousExecutableAsCandidate is joinable + const bool is_this_cooperative = + getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); + const bool any_other_cooperative = std::any_of( + executable_modules.begin(), executable_modules.end(), [&](const SceneModulePtr & m) { + return getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }); - // pairs of find_block_module and is_executable - std::vector, std::function>> - conditions; - - // Condition 3: Only modules that are always executable can be added - // if there exists at least one executable module that is neither always nor simultaneous - // executable. - conditions.emplace_back( - [this](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - !getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }, - [&]() { return false; }); - - // Condition 2: Only modules that are either always executable or simultaneous executable can be - // added if there exists at least one executable module that is simultaneous but not always - // executable. - conditions.emplace_back( - [this](const SceneModulePtr & m) { - return !getManager(m)->isAlwaysExecutableModule() && - getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }, - [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }); - - for (const auto & condition : conditions) { - const auto & find_block_module = condition.first; - const auto & is_executable = condition.second; - - const auto itr = - std::find_if(executable_modules.begin(), executable_modules.end(), find_block_module); - - if (itr != executable_modules.end() && is_executable()) { - executable_modules.push_back(module_ptr); - break; - } + if (is_this_cooperative && any_other_cooperative) { + executable_modules.push_back(module_ptr); } } @@ -654,7 +709,7 @@ std::pair PlannerManager::runRequestModule /** * choice highest priority module. */ - const auto module_ptr = [&]() { + const auto module_ptr = [&]() -> SceneModulePtr { if (!already_approved_modules.empty()) { return selectHighestPriorityModule(already_approved_modules); } @@ -663,8 +718,11 @@ std::pair PlannerManager::runRequestModule return selectHighestPriorityModule(waiting_approved_modules); } - return SceneModulePtr(); + return nullptr; }(); + if (module_ptr == nullptr) { + return std::make_pair(nullptr, BehaviorModuleOutput{}); + } /** * register candidate modules. @@ -674,149 +732,116 @@ std::pair PlannerManager::runRequestModule return std::make_pair(module_ptr, results.at(module_ptr->name())); } -BehaviorModuleOutput PlannerManager::runApprovedModules( - const std::shared_ptr & data, std::vector & deleted_modules) +BehaviorModuleOutput SubPlannerManager::run( + const SceneModulePtr & module_ptr, const std::shared_ptr & planner_data, + const BehaviorModuleOutput & previous_module_output) const { - std::unordered_map results; - BehaviorModuleOutput output = getReferencePath(data); - results.emplace("root", output); + StopWatch stop_watch; + stop_watch.tic(module_ptr->name()); - if (approved_module_ptrs_.empty()) { - return output; - } + module_ptr->setData(planner_data); + module_ptr->setPreviousModuleOutput(previous_module_output); - // move modules whose keep last flag is true to end of the approved_module_ptrs_. - // if there are multiple keep last modules, sort by priority - { - const auto move_to_end = [](auto & modules, const auto & module_to_move) { - auto itr = std::find(modules.begin(), modules.end(), module_to_move); + module_ptr->lockRTCCommand(); + const auto result = module_ptr->run(); + module_ptr->unlockRTCCommand(); - if (itr != modules.end()) { - auto tmp = std::move(*itr); - modules.erase(itr); - modules.push_back(std::move(tmp)); - } - }; + module_ptr->postProcess(); - const auto get_sorted_keep_last_modules = [this](const auto & modules) { - std::vector keep_last_modules; + module_ptr->updateCurrentState(); - std::copy_if( - modules.begin(), modules.end(), std::back_inserter(keep_last_modules), - [this](const auto & m) { return getManager(m)->isKeepLast(); }); + module_ptr->publishSteeringFactor(); - // sort by priority (low -> high) - std::sort( - keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) { - return getManager(a)->getPriority() < getManager(b)->getPriority(); - }); + module_ptr->publishObjectsOfInterestMarker(); - return keep_last_modules; - }; + processing_time_.at(module_ptr->name()) += stop_watch.toc(module_ptr->name(), true); + return result; +} - for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) { - move_to_end(approved_module_ptrs_, module); - } +SlotOutput SubPlannerManager::runApprovedModules( + const std::shared_ptr & data, const BehaviorModuleOutput & upstream_slot_output) +{ + std::unordered_map results; + BehaviorModuleOutput output = upstream_slot_output; + results.emplace("root", output); + + const bool is_candidate_plan_applied = true /* NOTE: not used in this process */; + bool is_this_failed = false; + bool is_this_waiting_approval = false; + + if (approved_module_ptrs_.empty()) { + return SlotOutput{output, is_candidate_plan_applied, is_this_failed, is_this_waiting_approval}; } - // lock approved modules besides last one + // unlock only last approved module std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { m->lockOutputPath(); }); - - // unlock only last approved module except keep last module. - { - const auto not_keep_last_modules = std::find_if( - approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), - [this](const auto & m) { return !getManager(m)->isKeepLast(); }); - - if (not_keep_last_modules != approved_module_ptrs_.rend()) { - (*not_keep_last_modules)->unlockOutputPath(); - } - } + approved_module_ptrs_.back()->unlockOutputPath(); /** - * execute approved modules except keep last modules. + * bootstrap approved module output */ std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) { - if (!getManager(m)->isKeepLast()) { - output = run(m, data, output); - results.emplace(m->name(), output); - } + output = run(m, data, output); + results.emplace(m->name(), output); }); - /** - * pop waiting approve module. push it back candidate modules. if waiting approve module is found - * in iteration std::find_if, not only the module but also the next one are removed from - * approved_module_ptrs_ since the next module's input (= waiting approve module's output) maybe - * change significantly. And, only the waiting approve module is pushed back to - * candidate_module_ptrs_. - */ - { - const auto not_keep_last_module = std::find_if( - approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), - [this](const auto & m) { return !getManager(m)->isKeepLast(); }); - - // convert reverse iterator -> iterator - const auto begin_itr = not_keep_last_module != approved_module_ptrs_.rend() - ? std::next(not_keep_last_module).base() - : approved_module_ptrs_.begin(); - - const auto waiting_approval_modules_itr = std::find_if( - begin_itr, approved_module_ptrs_.end(), - [](const auto & m) { return m->isWaitingApproval(); }); + const auto waiting_approval_modules_itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->isWaitingApproval(); }); - if (waiting_approval_modules_itr != approved_module_ptrs_.end()) { - clearCandidateModules(); - candidate_module_ptrs_.push_back(*waiting_approval_modules_itr); + if (waiting_approval_modules_itr != approved_module_ptrs_.end()) { + is_this_waiting_approval = true; - debug_info_.emplace_back( - *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval"); - - std::for_each( - waiting_approval_modules_itr, approved_module_ptrs_.end(), - [&results](const auto & m) { results.erase(m->name()); }); + // only keep this module as candidate + clearCandidateModules(); + candidate_module_ptrs_.push_back(*waiting_approval_modules_itr); - approved_module_ptrs_.erase(waiting_approval_modules_itr); + // delete following result but keep the rest of the following modules + std::for_each( + waiting_approval_modules_itr, approved_module_ptrs_.end(), + [&results](const auto & m) { results.erase(m->name()); }); + debug_info_.scene_status.emplace_back( + *waiting_approval_modules_itr, SceneModuleUpdateInfo::Action::MOVE, + "Back To Waiting Approval"); + approved_module_ptrs_.erase(waiting_approval_modules_itr); - std::for_each( - manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); - } + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } /** * remove failure modules. these modules' outputs are discarded as invalid plan. */ - { - const auto itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::FAILURE; }); - if (itr != approved_module_ptrs_.end()) { - deleted_modules.push_back(*itr); - } + const auto failed_itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::FAILURE; }); + if (failed_itr != approved_module_ptrs_.end()) { + is_this_failed = true; + + // clear all candidates + clearCandidateModules(); - std::for_each(itr, approved_module_ptrs_.end(), [this](auto & m) { - debug_info_.emplace_back(m, Action::DELETE, "From Approved"); + // delete both subsequent result and modules + std::for_each(failed_itr, approved_module_ptrs_.end(), [&](auto & m) { + results.erase(m->name()); + debug_info_.scene_status.emplace_back( + m, SceneModuleUpdateInfo::Action::DELETE, "From Approved"); deleteExpiredModules(m); }); + approved_module_ptrs_.erase(failed_itr, approved_module_ptrs_.end()); std::for_each( manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); - - if (itr != approved_module_ptrs_.end()) { - clearCandidateModules(); - } - - approved_module_ptrs_.erase(itr, approved_module_ptrs_.end()); } if (approved_module_ptrs_.empty()) { - return results.at("root"); + return SlotOutput{ + results.at("root"), is_candidate_plan_applied, is_this_failed, is_this_waiting_approval}; } - /** - * use the last module's output as approved modules planning result. - */ + // use the last module's output as approved modules planning result. const auto approved_modules_output = [&results, this]() { const auto itr = std::find_if( approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), @@ -828,199 +853,124 @@ BehaviorModuleOutput PlannerManager::runApprovedModules( return results.at("root"); }(); - /** - * remove success module immediately. if lane change module has succeeded, update current route - * lanelet. - */ - { - const auto move_to_end = [](auto & modules, const auto & cond) { - auto itr = modules.begin(); - while (itr != modules.end()) { - const auto satisfied_exit_cond = - std::all_of(itr, modules.end(), [&cond](const auto & m) { return cond(m); }); - - if (satisfied_exit_cond) { - return; - } - - if (cond(*itr)) { - auto tmp = std::move(*itr); - itr = modules.erase(itr); - modules.insert(modules.end(), std::move(tmp)); - } else { - itr++; - } - } - }; - const auto success_module_cond = [](const auto & m) { - return m->getCurrentStatus() == ModuleStatus::SUCCESS; - }; - move_to_end(approved_module_ptrs_, success_module_cond); - - const auto itr = - std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), success_module_cond); - - if (std::any_of(itr, approved_module_ptrs_.end(), [](const auto & m) { - return m->isCurrentRouteLaneletToBeReset(); - })) - resetCurrentRouteLanelet(data); - - std::copy_if( - itr, approved_module_ptrs_.end(), std::back_inserter(deleted_modules), success_module_cond); - - std::for_each(itr, approved_module_ptrs_.end(), [&](auto & m) { - debug_info_.emplace_back(m, Action::DELETE, "From Approved"); - deleteExpiredModules(m); - }); - - approved_module_ptrs_.erase(itr, approved_module_ptrs_.end()); + // if lane change module has succeeded, update current route lanelet. + if (std::any_of(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) { + return m->getCurrentStatus() == ModuleStatus::SUCCESS && + m->isCurrentRouteLaneletToBeReset(); + })) + resetCurrentRouteLanelet(data); - std::for_each( - manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); + // remove success module immediately. + for (auto success_itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::SUCCESS; }); + success_itr != approved_module_ptrs_.end(); + /* success_itr++ */) { + if ((*success_itr)->getCurrentStatus() == ModuleStatus::SUCCESS) { + debug_info_.scene_status.emplace_back( + *success_itr, SceneModuleUpdateInfo::Action::DELETE, "From Approved"); + deleteExpiredModules(*success_itr); + success_itr = approved_module_ptrs_.erase(success_itr); + } else { + success_itr++; + } } - return approved_modules_output; + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); + + return SlotOutput{ + approved_modules_output, is_candidate_plan_applied, is_this_failed, is_this_waiting_approval}; } -void PlannerManager::updateCandidateModules( - const std::vector & request_modules, - const SceneModulePtr & highest_priority_module) +SlotOutput SubPlannerManager::propagateFull( + const std::shared_ptr & data, const SlotOutput & previous_slot_output) { - const auto exist = [](const auto & module_ptr, const auto & module_ptrs) { - const auto itr = std::find_if( - module_ptrs.begin(), module_ptrs.end(), - [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); }); - - return itr != module_ptrs.end(); - }; - - /** - * unregister expired modules - */ - { - const auto candidate_to_remove = [&](auto & itr) { - if (!exist(itr, request_modules)) { - deleteExpiredModules(itr); - return true; - } - return itr->name() == highest_priority_module->name() && - !highest_priority_module->isWaitingApproval(); - }; - - candidate_module_ptrs_.erase( - std::remove_if( - candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), candidate_to_remove), - candidate_module_ptrs_.end()); + const size_t module_size = manager_ptrs_.size(); + const size_t max_iteration_num = static_cast(module_size * (module_size + 1) / 2); + + bool is_waiting_approved_slot = previous_slot_output.is_upstream_waiting_approved; + bool is_failed_approved_slot = false; + auto output_path = previous_slot_output.valid_output; + + std::vector deleted_modules; + for (size_t itr_num = 0; itr_num < max_iteration_num; ++itr_num) { + const auto approved_module_result = runApprovedModules(data, previous_slot_output.valid_output); + const auto & approved_module_output = approved_module_result.valid_output; + + // these status needs to be propagated to downstream slots + // if any of the slots returned following statuses, keep it + is_waiting_approved_slot = + is_waiting_approved_slot || approved_module_result.is_upstream_waiting_approved; + is_failed_approved_slot = + is_failed_approved_slot || approved_module_result.is_upstream_failed_approved; + + const auto request_modules = getRequestModules(approved_module_output, deleted_modules); + if (request_modules.empty()) { + // there is no module that needs to be launched + return SlotOutput{ + approved_module_output, isAnyCandidateExclusive(), is_failed_approved_slot, + is_waiting_approved_slot}; + } - std::for_each( - manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); - } + const auto [highest_priority_module, candidate_module_output] = + runRequestModules(request_modules, data, approved_module_output); - /** - * register running candidate modules - */ - for (const auto & m : request_modules) { - if ( - m->name() == highest_priority_module->name() && - !highest_priority_module->isWaitingApproval()) { - continue; + if (!highest_priority_module) { + // there is no need to launch new module + return SlotOutput{ + approved_module_output, isAnyCandidateExclusive(), is_failed_approved_slot, + is_waiting_approved_slot}; } - if (!exist(m, candidate_module_ptrs_)) { - candidate_module_ptrs_.push_back(m); + if (highest_priority_module->isWaitingApproval()) { + // there is no need to launch new module + return SlotOutput{ + candidate_module_output, isAnyCandidateExclusive(), is_failed_approved_slot, + is_waiting_approved_slot}; } - } - - /** - * sort by priority. sorted_request_modules.front() is the highest priority module. - */ - sortByPriority(candidate_module_ptrs_); -} - -void PlannerManager::print() const -{ - const auto get_status = [](const auto & m) { - return magic_enum::enum_name(m->getCurrentStatus()); - }; - - size_t max_string_num = 0; - std::ostringstream string_stream; - string_stream << "\n"; - string_stream << "***********************************************************\n"; - string_stream << " planner manager status\n"; - string_stream << "-----------------------------------------------------------\n"; - string_stream << "registered modules: "; - for (const auto & m : manager_ptrs_) { - string_stream << "[" << m->name() << "]"; - max_string_num = std::max(max_string_num, m->name().length()); - } - - string_stream << "\n"; - string_stream << "approved modules : "; - for (const auto & m : approved_module_ptrs_) { - string_stream << "[" << m->name() << "(" << get_status(m) << ")" - << "]->"; - } - - string_stream << "\n"; - string_stream << "candidate module : "; - for (const auto & m : candidate_module_ptrs_) { - string_stream << "[" << m->name() << "(" << get_status(m) << ")" - << "]->"; - } - - string_stream << "\n"; - string_stream << "update module info: "; - for (const auto & i : debug_info_) { - string_stream << "[Module:" << i.module_name << " Status:" << magic_enum::enum_name(i.status) - << " Action:" << magic_enum::enum_name(i.action) - << " Description:" << i.description << "]\n" - << std::setw(28); - } - - string_stream << "\n" << std::fixed << std::setprecision(1); - string_stream << "processing time : "; - for (const auto & t : processing_time_) { - string_stream << std::right << "[" << std::setw(static_cast(max_string_num) + 1) - << std::left << t.first << ":" << std::setw(4) << std::right << t.second - << "ms]\n" - << std::setw(21); + output_path = candidate_module_output; + addApprovedModule(highest_priority_module); + clearCandidateModules(); } - state_publisher_ptr_->publish("internal_state", string_stream.str()); - - RCLCPP_DEBUG_STREAM(logger_, string_stream.str()); + return SlotOutput{ + output_path, isAnyCandidateExclusive(), is_failed_approved_slot, is_waiting_approved_slot}; } -void PlannerManager::publishProcessingTime() const +SlotOutput SubPlannerManager::propagateWithExclusiveCandidate( + const std::shared_ptr & data, const SlotOutput & previous_slot_output) { - for (const auto & t : processing_time_) { - std::string name = t.first + std::string("/processing_time_ms"); - debug_publisher_ptr_->publish(name, t.second); - } + const auto approved_module_result = runApprovedModules(data, previous_slot_output.valid_output); + const auto & approved_module_output = approved_module_result.valid_output; + + // these status needs to be propagated to downstream slots + // if any of the slots returned following statuses, keep it + const bool is_waiting_approved_slot = previous_slot_output.is_upstream_waiting_approved | + approved_module_result.is_upstream_waiting_approved; + const bool is_failed_approved_slot = previous_slot_output.is_upstream_failed_approved | + approved_module_result.is_upstream_failed_approved; + + // there is no module that needs to be launched + return SlotOutput{ + approved_module_output, true, is_failed_approved_slot, is_waiting_approved_slot}; } -std::shared_ptr PlannerManager::getDebugMsg() +void SubPlannerManager::propagateWithFailedApproved() { - debug_msg_ptr_ = std::make_shared(); - for (const auto & approved_module : approved_module_ptrs_) { - approved_module->acceptVisitor(debug_msg_ptr_); - } - - for (const auto & candidate_module : candidate_module_ptrs_) { - candidate_module->acceptVisitor(debug_msg_ptr_); - } - return debug_msg_ptr_; + clearCandidateModules(); + clearApprovedModules(); } -std::string PlannerManager::getNames(const std::vector & modules) +SlotOutput SubPlannerManager::propagateWithWaitingApproved( + const std::shared_ptr & data, const SlotOutput & previous_slot_output) { - std::stringstream ss; - for (const auto & m : modules) { - ss << "[" << m->name() << "], "; - } - return ss.str(); + clearCandidateModules(); + + return previous_slot_output.is_upstream_candidate_exclusive + ? propagateWithExclusiveCandidate(data, previous_slot_output) + : propagateFull(data, previous_slot_output); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index c9adf4d60c21f..9dfd30e15cfbd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -217,37 +217,16 @@ class SceneModuleManagerInterface */ bool canLaunchNewModule() const { return observers_.empty(); } - /** - * Determine if the module is always executable, regardless of the state of other modules. - * - * When this returns true: - * - The module can be executed even if other modules are not marked as 'simultaneously - * executable'. - * - Conversely, the presence of this module will not prevent other modules - * from executing, even if they are not marked as 'simultaneously executable'. - */ - virtual bool isAlwaysExecutableModule() const { return false; } - virtual bool isSimultaneousExecutableAsApprovedModule() const { - if (isAlwaysExecutableModule()) { - return true; - } - return config_.enable_simultaneous_execution_as_approved_module; } virtual bool isSimultaneousExecutableAsCandidateModule() const { - if (isAlwaysExecutableModule()) { - return true; - } - return config_.enable_simultaneous_execution_as_candidate_module; } - bool isKeepLast() const { return config_.keep_last; } - void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } void reset() @@ -268,8 +247,6 @@ class SceneModuleManagerInterface pub_debug_marker_->publish(MarkerArray{}); } - size_t getPriority() const { return config_.priority; } - std::string name() const { return name_; } std::vector getSceneModuleObservers() { return observers_; } @@ -298,8 +275,6 @@ class SceneModuleManagerInterface getOrDeclareParameter(*node, ns + "enable_simultaneous_execution_as_approved_module"); config_.enable_simultaneous_execution_as_candidate_module = getOrDeclareParameter( *node, ns + "enable_simultaneous_execution_as_candidate_module"); - config_.keep_last = getOrDeclareParameter(*node, ns + "keep_last"); - config_.priority = getOrDeclareParameter(*node, ns + "priority"); } // init rtc configuration diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp index a45e8366f45d8..ecbe9b5208347 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp @@ -23,13 +23,10 @@ struct ModuleConfigParameters bool enable_rtc{false}; bool enable_simultaneous_execution_as_approved_module{false}; bool enable_simultaneous_execution_as_candidate_module{false}; - bool keep_last{false}; - uint8_t priority{0}; }; struct BehaviorPathPlannerParameters { - size_t max_iteration_num{100}; double traffic_light_signal_timeout{1.0}; double backward_path_length; From 983c90c96b2d2ebc28021236fa0029d299ebda04 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Thu, 1 Aug 2024 10:04:54 +0900 Subject: [PATCH 002/126] fix(autoware_lidar_centerpoint): place device vector in CUDA device system (#8272) Signed-off-by: amadeuszsz --- .../postprocess/postprocess_kernel.hpp | 3 --- .../lib/postprocess/postprocess_kernel.cu | 17 ++++++++--------- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp index 4792fed3e9362..6ea9965f4fac0 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp @@ -19,7 +19,6 @@ #include "autoware/lidar_centerpoint/utils.hpp" #include "cuda.h" #include "cuda_runtime_api.h" -#include "thrust/device_vector.h" #include @@ -37,8 +36,6 @@ class PostProcessCUDA private: CenterPointConfig config_; - thrust::device_vector boxes3d_d_; - thrust::device_vector yaw_norm_thresholds_d_; }; } // namespace autoware::lidar_centerpoint diff --git a/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index e1af8f36eb082..81157e07991d1 100644 --- a/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -137,10 +137,6 @@ __global__ void generateBoxes3D_kernel( PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(config) { - const auto num_raw_boxes3d = config.down_grid_size_y_ * config.down_grid_size_x_; - boxes3d_d_ = thrust::device_vector(num_raw_boxes3d); - yaw_norm_thresholds_d_ = thrust::device_vector( - config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); } // cspell: ignore divup @@ -153,23 +149,26 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( divup(config_.down_grid_size_y_, THREADS_PER_BLOCK), divup(config_.down_grid_size_x_, THREADS_PER_BLOCK)); dim3 threads(THREADS_PER_BLOCK, THREADS_PER_BLOCK); + auto boxes3d_d = + thrust::device_vector(config_.down_grid_size_y_ * config_.down_grid_size_x_); + auto yaw_norm_thresholds_d = thrust::device_vector( + config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); generateBoxes3D_kernel<<>>( out_heatmap, out_offset, out_z, out_dim, out_rot, out_vel, config_.voxel_size_x_, config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_, config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, - config_.has_variance_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), - thrust::raw_pointer_cast(boxes3d_d_.data())); + config_.has_variance_, thrust::raw_pointer_cast(yaw_norm_thresholds_d.data()), + thrust::raw_pointer_cast(boxes3d_d.data())); // suppress by score const auto num_det_boxes3d = thrust::count_if( - thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), - is_score_greater(config_.score_threshold_)); + thrust::device, boxes3d_d.begin(), boxes3d_d.end(), is_score_greater(config_.score_threshold_)); if (num_det_boxes3d == 0) { return cudaGetLastError(); } thrust::device_vector det_boxes3d_d(num_det_boxes3d); thrust::copy_if( - thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), det_boxes3d_d.begin(), + thrust::device, boxes3d_d.begin(), boxes3d_d.end(), det_boxes3d_d.begin(), is_score_greater(config_.score_threshold_)); // sort by score From aa2de285f053946c57560728e87b196787687de4 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Thu, 1 Aug 2024 10:05:21 +0900 Subject: [PATCH 003/126] fix(autoware_lidar_transfusion): place device vector in CUDA device system (#8273) Signed-off-by: amadeuszsz --- .../postprocess/postprocess_kernel.hpp | 3 --- .../lib/postprocess/postprocess_kernel.cu | 16 ++++++++-------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp index ed677bc84b867..6228fec48a833 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp @@ -20,7 +20,6 @@ #include #include -#include #include @@ -41,8 +40,6 @@ class PostprocessCuda cudaStream_t stream_; cudaStream_t stream_event_; cudaEvent_t start_, stop_; - thrust::device_vector boxes3d_d_; - thrust::device_vector yaw_norm_thresholds_d_; }; } // namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu index fbdb89d59b3d1..905b4ea6f39f4 100644 --- a/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/postprocess/postprocess_kernel.cu @@ -93,9 +93,6 @@ __global__ void generateBoxes3D_kernel( PostprocessCuda::PostprocessCuda(const TransfusionConfig & config, cudaStream_t & stream) : config_(config), stream_(stream) { - boxes3d_d_ = thrust::device_vector(config_.num_proposals_); - yaw_norm_thresholds_d_ = thrust::device_vector( - config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); } // cspell: ignore divup @@ -106,22 +103,25 @@ cudaError_t PostprocessCuda::generateDetectedBoxes3D_launch( dim3 threads = {THREADS_PER_BLOCK}; dim3 blocks = {divup(config_.num_proposals_, threads.x)}; + auto boxes3d_d = thrust::device_vector(config_.num_proposals_); + auto yaw_norm_thresholds_d = thrust::device_vector( + config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); + generateBoxes3D_kernel<<>>( cls_output, box_output, dir_cls_output, config_.voxel_x_size_, config_.voxel_y_size_, config_.min_x_range_, config_.min_y_range_, config_.num_proposals_, config_.num_classes_, - config_.num_point_values_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), - thrust::raw_pointer_cast(boxes3d_d_.data())); + config_.num_point_values_, thrust::raw_pointer_cast(yaw_norm_thresholds_d.data()), + thrust::raw_pointer_cast(boxes3d_d.data())); // suppress by score const auto num_det_boxes3d = thrust::count_if( - thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), - is_score_greater(config_.score_threshold_)); + thrust::device, boxes3d_d.begin(), boxes3d_d.end(), is_score_greater(config_.score_threshold_)); if (num_det_boxes3d == 0) { return cudaGetLastError(); } thrust::device_vector det_boxes3d_d(num_det_boxes3d); thrust::copy_if( - thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), det_boxes3d_d.begin(), + thrust::device, boxes3d_d.begin(), boxes3d_d.end(), det_boxes3d_d.begin(), is_score_greater(config_.score_threshold_)); // sort by score From b840acff9f9e006e969e879d439d7d003f45eb78 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 1 Aug 2024 10:32:21 +0900 Subject: [PATCH 004/126] fix(autoware_behavior_velocity_intersection_module): fix funcArgNamesDifferent (#8023) * fix:funcArgNamesDifferent Signed-off-by: kobayu858 * fix:funcArgNamesDifferent Signed-off-by: kobayu858 * refactor:clang format Signed-off-by: kobayu858 * fix:funcArgNamesDifferent Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../src/object_manager.cpp | 36 +++++++++---------- .../src/object_manager.hpp | 18 +++++----- .../src/scene_intersection.hpp | 6 ++-- 3 files changed, 30 insertions(+), 30 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index b749207aa2393..d93fbf271aa1d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -74,12 +74,12 @@ ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_st void ObjectInfo::initialize( const autoware_perception_msgs::msg::PredictedObject & object, - std::optional attention_lanelet_opt_, - std::optional stopline_opt_) + std::optional attention_lanelet_opt, + std::optional stopline_opt) { predicted_object_ = object; - attention_lanelet_opt = attention_lanelet_opt_; - stopline_opt = stopline_opt_; + attention_lanelet_opt_ = attention_lanelet_opt; + stopline_opt_ = stopline_opt; unsafe_interval_ = std::nullopt; calc_dist_to_stopline(); } @@ -96,10 +96,10 @@ void ObjectInfo::update_safety( std::optional ObjectInfo::estimated_past_position( const double past_duration) const { - if (!attention_lanelet_opt) { + if (!attention_lanelet_opt_) { return std::nullopt; } - const auto attention_lanelet = attention_lanelet_opt.value(); + const auto attention_lanelet = attention_lanelet_opt_.value(); const auto current_arc_coords = lanelet::utils::getArcCoordinates( {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); const auto distance = current_arc_coords.distance; @@ -116,29 +116,29 @@ std::optional ObjectInfo::estimated_past_position( void ObjectInfo::calc_dist_to_stopline() { - if (!stopline_opt || !attention_lanelet_opt) { + if (!stopline_opt_ || !attention_lanelet_opt_) { return; } - const auto attention_lanelet = attention_lanelet_opt.value(); + const auto attention_lanelet = attention_lanelet_opt_.value(); const auto object_arc_coords = lanelet::utils::getArcCoordinates( {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); - const auto stopline = stopline_opt.value(); + const auto stopline = stopline_opt_.value(); geometry_msgs::msg::Pose stopline_center; stopline_center.position.x = (stopline.front().x() + stopline.back().x()) / 2.0; stopline_center.position.y = (stopline.front().y() + stopline.back().y()) / 2.0; stopline_center.position.z = (stopline.front().z() + stopline.back().z()) / 2.0; const auto stopline_arc_coords = lanelet::utils::getArcCoordinates({attention_lanelet}, stopline_center); - dist_to_stopline_opt = (stopline_arc_coords.length - object_arc_coords.length); + dist_to_stopline_opt_ = (stopline_arc_coords.length - object_arc_coords.length); } bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const { - if (!dist_to_stopline_opt) { + if (!dist_to_stopline_opt_) { return false; } const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; - const double dist_to_stopline = dist_to_stopline_opt.value(); + const double dist_to_stopline = dist_to_stopline_opt_.value(); const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration); return dist_to_stopline > braking_distance; } @@ -147,17 +147,17 @@ bool ObjectInfo::can_stop_before_ego_lane( const double brake_deceleration, const double tolerable_overshoot, lanelet::ConstLanelet ego_lane) const { - if (!dist_to_stopline_opt || !stopline_opt || !attention_lanelet_opt) { + if (!dist_to_stopline_opt_ || !stopline_opt_ || !attention_lanelet_opt_) { return false; } - const double dist_to_stopline = dist_to_stopline_opt.value(); + const double dist_to_stopline = dist_to_stopline_opt_.value(); const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration); if (dist_to_stopline > braking_distance) { return false; } - const auto attention_lanelet = attention_lanelet_opt.value(); - const auto stopline = stopline_opt.value(); + const auto attention_lanelet = attention_lanelet_opt_.value(); + const auto stopline = stopline_opt_.value(); const auto stopline_p1 = stopline.front(); const auto stopline_p2 = stopline.back(); const autoware::universe_utils::Point2d stopline_mid{ @@ -185,10 +185,10 @@ bool ObjectInfo::can_stop_before_ego_lane( bool ObjectInfo::before_stopline_by(const double margin) const { - if (!dist_to_stopline_opt) { + if (!dist_to_stopline_opt_) { return false; } - const double dist_to_stopline = dist_to_stopline_opt.value(); + const double dist_to_stopline = dist_to_stopline_opt_.value(); return dist_to_stopline < margin; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp index 0ba9947fdb318..d98f750dacf13 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp @@ -126,7 +126,7 @@ class ObjectInfo * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline */ void initialize( - const autoware_perception_msgs::msg::PredictedObject & predicted_object, + const autoware_perception_msgs::msg::PredictedObject & object, std::optional attention_lanelet_opt, std::optional stopline_opt); @@ -134,9 +134,8 @@ class ObjectInfo * @brief update unsafe_knowledge */ void update_safety( - const std::optional & unsafe_interval_opt, - const std::optional & safe_interval_opt, - const bool safe_under_traffic_control); + const std::optional & unsafe_interval, + const std::optional & safe_interval, const bool safe_under_traffic_control); /** * @brief find the estimated position of the object in the past @@ -196,13 +195,13 @@ class ObjectInfo autoware_perception_msgs::msg::PredictedObject predicted_object_; //! null if the object in intersection_area but not in attention_area - std::optional attention_lanelet_opt{std::nullopt}; + std::optional attention_lanelet_opt_{std::nullopt}; //! null if the object in intersection_area but not in attention_area - std::optional stopline_opt{std::nullopt}; + std::optional stopline_opt_{std::nullopt}; //! null if the object in intersection_area but not in attention_area - std::optional dist_to_stopline_opt{std::nullopt}; + std::optional dist_to_stopline_opt_{std::nullopt}; //! store the information if judged as UNSAFE std::optional unsafe_interval_{std::nullopt}; @@ -230,11 +229,12 @@ class ObjectInfoManager public: std::shared_ptr registerObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked); + const bool belong_intersection_area, const bool is_parked_vehicle); void registerExistingObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked, std::shared_ptr object); + const bool belong_intersection_area, const bool is_parked_vehicle, + std::shared_ptr object); void clearObjects(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index ea2e1a7ae16d7..03417a8410d20 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -590,7 +590,7 @@ class IntersectionModule : public SceneModuleInterface * @brief generate discretized detection lane linestring. */ std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, + lanelet::ConstLanelets detection_lanelets_all, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const; /** @} */ @@ -752,7 +752,7 @@ class IntersectionModule : public SceneModuleInterface void cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, - autoware_perception_msgs::msg::PredictedPath * predicted_path) const; + autoware_perception_msgs::msg::PredictedPath * path) const; /** * @brief check if there are any objects around the stoplines on the attention areas when ego @@ -809,7 +809,7 @@ class IntersectionModule : public SceneModuleInterface TimeDistanceArray calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const; + tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; /** @} */ mutable DebugData debug_data_; From 8ed7e55f3bc3db8de5e87ca404a955eb99056c1a Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 1 Aug 2024 10:34:56 +0900 Subject: [PATCH 005/126] fix(learning_based_vehicle_model): fix passedByValue (#8244) * fix:passedByValue Signed-off-by: kobayu858 * fix:clang format Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../model_connections_helpers.hpp | 5 +++-- .../src/model_connections_helpers.cpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp index 3f21595662fc5..0d46169600b2f 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp +++ b/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp @@ -19,9 +19,10 @@ #include std::vector fillVectorUsingMap( - std::vector vector1, std::vector vector2, std::vector map, bool inverse); + std::vector vector1, std::vector vector2, const std::vector & map, + bool inverse); std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2); + const std::vector & connection_names_1, const std::vector & connection_names_2); #endif // LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ diff --git a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp index b13f6497d2dea..c7944116d1758 100644 --- a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp +++ b/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp @@ -15,7 +15,8 @@ #include "learning_based_vehicle_model/model_connections_helpers.hpp" std::vector fillVectorUsingMap( - std::vector vector1, std::vector vector2, std::vector map, bool inverse) + std::vector vector1, std::vector vector2, const std::vector & map, + bool inverse) { // index in "map" is index in "vector1" and value in "map" is index in "vector2" // inverse = 0 from 1 -> 2; inverse = 1 from 2 -> 1 @@ -27,7 +28,7 @@ std::vector fillVectorUsingMap( } std::vector createConnectionsMap( - std::vector connection_names_1, std::vector connection_names_2) + const std::vector & connection_names_1, const std::vector & connection_names_2) { std::vector connection_map; // index in "connection_map" is index in "connection_names_2" and value in "connection_map" is From 3d1d83d36fb6f84f506a95f6a173ed618be629b9 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 1 Aug 2024 10:36:23 +0900 Subject: [PATCH 006/126] fix(autoware_ground_segmentation): fix functionConst (#8291) fix:functionConst Signed-off-by: kobayu858 --- .../src/scan_ground_filter/node.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index ad5d0ea4b6bc6..f9a888ad9dfcf 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -128,18 +128,18 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt float getAverageSlope() { return std::atan2(height_avg, radius_avg); } - float getAverageHeight() { return height_avg; } + float getAverageHeight() const { return height_avg; } - float getAverageRadius() { return radius_avg; } + float getAverageRadius() const { return radius_avg; } - float getMaxHeight() { return height_max; } + float getMaxHeight() const { return height_max; } - float getMinHeight() { return height_min; } + float getMinHeight() const { return height_min; } - uint16_t getGridId() { return grid_id; } + uint16_t getGridId() const { return grid_id; } - pcl::PointIndices getIndices() { return pcl_indices; } - std::vector getHeightList() { return height_list; } + pcl::PointIndices getIndices() const { return pcl_indices; } + std::vector getHeightList() const { return height_list; } pcl::PointIndices & getIndicesRef() { return pcl_indices; } std::vector & getHeightListRef() { return height_list; } From 37614bcbfd51597ec2d13f43cd1a016f62f21e0b Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 1 Aug 2024 10:38:34 +0900 Subject: [PATCH 007/126] fix(autoware_radar_fusion_to_detected_object): fix functionConst (#8287) fix:functionConst Signed-off-by: kobayu858 --- .../src/include/radar_fusion_to_detected_object.hpp | 4 ++-- .../src/radar_fusion_to_detected_object.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp b/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp index b296f52e2c139..3c95d8f068af1 100644 --- a/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/include/radar_fusion_to_detected_object.hpp @@ -99,9 +99,9 @@ class RadarFusionToDetectedObject // std::vector splitObject( // const DetectedObject & object, const std::shared_ptr> & radars); TwistWithCovariance estimateTwist( - const DetectedObject & object, const std::shared_ptr> & radars); + const DetectedObject & object, const std::shared_ptr> & radars) const; bool isQualified( - const DetectedObject & object, const std::shared_ptr> & radars); + const DetectedObject & object, const std::shared_ptr> & radars) const; TwistWithCovariance convertDopplerToTwist( const DetectedObject & object, const TwistWithCovariance & twist_with_covariance); static bool isYawCorrect( diff --git a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index bc96ac780fd0a..25e174a44c2ec 100644 --- a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -205,7 +205,7 @@ RadarFusionToDetectedObject::filterRadarWithinObject( // (Target value is amplitude if using radar pointcloud. Target value is probability if using radar // objects). TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( - const DetectedObject & object, const std::shared_ptr> & radars) + const DetectedObject & object, const std::shared_ptr> & radars) const { if (!radars || (*radars).empty()) { TwistWithCovariance output{}; @@ -298,7 +298,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( // Judge whether low confidence objects that do not have some radar points/objects or not. bool RadarFusionToDetectedObject::isQualified( - const DetectedObject & object, const std::shared_ptr> & radars) + const DetectedObject & object, const std::shared_ptr> & radars) const { if (object.existence_probability > param_.threshold_probability) { return true; From 2692d3cdeaa513d37a67f1bc3b8324515a2d74c2 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 1 Aug 2024 10:38:59 +0900 Subject: [PATCH 008/126] fix(autoware_freespace_planning_algorithms): fix functionConst (#8281) fix:functionConst Signed-off-by: kobayu858 --- .../autoware/freespace_planning_algorithms/astar_search.hpp | 2 +- .../autoware_freespace_planning_algorithms/src/astar_search.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp index 3f6c3935b8a42..f7434d912d5e0 100644 --- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp @@ -79,7 +79,7 @@ struct AstarNode struct NodeComparison { - bool operator()(const AstarNode * lhs, const AstarNode * rhs) { return lhs->fc > rhs->fc; } + bool operator()(const AstarNode * lhs, const AstarNode * rhs) const { return lhs->fc > rhs->fc; } }; struct NodeUpdate diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 4ead3cb5a7423..9233ad212bfa5 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -155,7 +155,7 @@ void AstarSearch::setCollisionFreeDistanceMap() using Entry = std::pair; struct CompareEntry { - bool operator()(const Entry & a, const Entry & b) { return a.second > b.second; } + bool operator()(const Entry & a, const Entry & b) const { return a.second > b.second; } }; std::priority_queue, CompareEntry> heap; std::vector closed(col_free_distance_map_.size(), false); From 24bfad3bb8f8848dbaa7fa90ac8044827509ce34 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 1 Aug 2024 10:39:25 +0900 Subject: [PATCH 009/126] fix(autoware_behavior_velocity_run_out_module): fix functionConst (#8284) fix:functionConst Signed-off-by: kobayu858 --- .../autoware_behavior_velocity_run_out_module/src/debug.cpp | 2 +- .../autoware_behavior_velocity_run_out_module/src/debug.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 826bdf5a8ed91..983a371234ccf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -202,7 +202,7 @@ autoware::motion_utils::VirtualWalls RunOutDebug::createVirtualWalls() } visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArrayFromDebugData( - const builtin_interfaces::msg::Time & current_time) + const builtin_interfaces::msg::Time & current_time) const { visualization_msgs::msg::MarkerArray msg; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp index b9ea77fc91e8c..aa96e5b018fad 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -127,7 +127,7 @@ class RunOutDebug private: visualization_msgs::msg::MarkerArray createVisualizationMarkerArrayFromDebugData( - const builtin_interfaces::msg::Time & current_time); + const builtin_interfaces::msg::Time & current_time) const; void clearDebugMarker(); rclcpp::Node & node_; From 5e1e22beb7ff988a4da81eb72f2dc12b941c41e4 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Thu, 1 Aug 2024 12:32:26 +0900 Subject: [PATCH 010/126] fix(behavior_path_planner, spellchecks): spell checks in behavior path planner (#8307) * fix spell checks in behavior path planner Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * try re-routable Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- .../docs/behavior_path_planner_manager_design.md | 4 ++-- .../autoware/behavior_path_planner/planner_manager.hpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md index 9cf7abfd322ed..68d81f4d257c1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -48,7 +48,7 @@ Additionally, the manager generates root reference path, and if any other module ### Slot -The manager owns several containers of sub-managers, namely _slots_, that holds/runs several sub-managers and send the output to the next slot. Given the initial reference path, each slot processes the input path and the ouptut path is processed by the next slot. The final slot output is utilized as the output of the manager. The slot passes following information +The manager owns several containers of sub-managers, namely _slots_, that holds/runs several sub-managers and send the output to the next slot. Given the initial reference path, each slot processes the input path and the output path is processed by the next slot. The final slot output is utilized as the output of the manager. The slot passes following information ```cpp struct SlotOutput @@ -593,7 +593,7 @@ The **current route lanelet** can be reset to the closest lanelet within the rou ![current_route_lanelet](../image/manager/current_route_lanelet.svg) -The manager needs to know the ego behavior and then generate a root reference path from the lanes tghat Ego should follow. +The manager needs to know the ego behavior and then generate a root reference path from the lanes that Ego should follow. For example, during autonomous driving, even if Ego moves into the next lane in order to avoid a parked vehicle, the target lanes that Ego should follow will **NOT** change because Ego will return to the original lane after the avoidance maneuver. Therefore, the manager does **NOT** reset the **current route lanelet**, even if the avoidance maneuver is finished. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index e46dad0210095..77f2ffc26f44d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -111,7 +111,7 @@ struct SlotOutput BehaviorModuleOutput valid_output; // if candidate module is running, valid_output contains the planning by candidate module. In - // that case, downstream slots will just run aproved modules and do not try to launch new + // that case, downstream slots will just run approved modules and do not try to launch new // modules bool is_upstream_candidate_exclusive{false}; @@ -153,7 +153,7 @@ class SubPlannerManager const std::shared_ptr & planner_data, const SlotOutput & previous_slot_output); /** - * @brief run approved modules without launching new moules(candidates) + * @brief run approved modules without launching new modules(candidates) * @pre previous_slot_output.is_upstream_candidate_exclusive is true * @post is_upstream_candidate_exclusive of output is true * @post candidate_module does not increase @@ -515,8 +515,8 @@ class PlannerManager } /** - * @brief check if reroutable approved module is running(namely except for fixed_goal_planner and - * dynamic_avoidance) + * @brief check if re-routable approved module is running(namely except for fixed_goal_planner + * and dynamic_avoidance) */ bool hasPossibleRerouteApprovedModules(const std::shared_ptr & data) const; From 22052b9d273ba75ffc7a862c25644c1c96f275f5 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Thu, 1 Aug 2024 13:09:12 +0900 Subject: [PATCH 011/126] fix(docs): autoware stop filter docs (#8302) fix autoware stop filter docs Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- localization/autoware_stop_filter/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/autoware_stop_filter/README.md b/localization/autoware_stop_filter/README.md index 261b8c9da392c..9904707a59996 100644 --- a/localization/autoware_stop_filter/README.md +++ b/localization/autoware_stop_filter/README.md @@ -25,4 +25,4 @@ This node aims to: ## Parameters -{{ json_to_markdown("localization/stop_filter/schema/stop_filter.schema.json") }} +{{ json_to_markdown("localization/autoware_stop_filter/schema/stop_filter.schema.json") }} From 41fca7e17cdca52030496f7d09748209e157d861 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Thu, 1 Aug 2024 13:15:48 +0900 Subject: [PATCH 012/126] fix(doc): landmark localizer (#8301) fix landmark localizer Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- .../autoware_ar_tag_based_localizer/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md index 14501c689dbe6..788f7bbc6b3a8 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md @@ -32,7 +32,7 @@ The positions and orientations of the AR-Tags are assumed to be written in the L ## Parameters -{{ json_to_markdown("localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }} +{{ json_to_markdown("localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json") }} ## How to launch From 13ee518d16dd016f509397e74aa88331b12c0222 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 1 Aug 2024 16:41:36 +0900 Subject: [PATCH 013/126] refactor(smart_mpc_trajectory_folower): modify pacakge structure and install without setup.py (#8268) * refactor(smart_mpc_trajectory_follower): modify pacakge structure and install without setup.py Signed-off-by: kosuke55 * import proxima calc Signed-off-by: kosuke55 * use ament_get_python_install_dir Signed-off-by: kosuke55 * add python3-torch Signed-off-by: kosuke55 * remove pip from readme Signed-off-by: kosuke55 * remove str conversion from open Signed-off-by: kosuke55 * sort in cmake Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../CMakeLists.txt | 21 +++++++- .../README.md | 14 ------ .../scripts/drive_NN.py | 2 +- .../scripts/drive_functions.py | 48 +++++-------------- .../{scripts => src}/proxima_calc.cpp | 0 .../package.xml | 2 + .../scripts/pympc_trajectory_follower.py | 0 .../setup.py | 42 ---------------- 8 files changed, 34 insertions(+), 95 deletions(-) rename control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/{scripts => src}/proxima_calc.cpp (100%) rename control/autoware_smart_mpc_trajectory_follower/{autoware_smart_mpc_trajectory_follower => }/scripts/pympc_trajectory_follower.py (100%) delete mode 100644 control/autoware_smart_mpc_trajectory_follower/setup.py diff --git a/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt index 57e7b596790c2..b4b21bded9c73 100644 --- a/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt +++ b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt @@ -2,12 +2,31 @@ cmake_minimum_required(VERSION 3.14) project(autoware_smart_mpc_trajectory_follower) find_package(autoware_cmake REQUIRED) +find_package(python_cmake_module REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development) +find_package(pybind11_vendor REQUIRED) +find_package(pybind11 REQUIRED) +find_package(Eigen3 REQUIRED) autoware_package() +execute_process(COMMAND bash -c "pip3 install numba==0.58.1 --force-reinstall") +execute_process(COMMAND bash -c "pip3 install GPy") + +pybind11_add_module(proxima_calc SHARED + ${PROJECT_NAME}/src/proxima_calc.cpp +) +ament_target_dependencies(proxima_calc Eigen3) +ament_get_python_install_dir(PYTHON_INSTALL_DIR) +install(TARGETS proxima_calc + LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}/${PROJECT_NAME} +) + +ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS - autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py + scripts/pympc_trajectory_follower.py DESTINATION lib/${PROJECT_NAME} ) ament_auto_package( INSTALL_TO_SHARE + autoware_smart_mpc_trajectory_follower/param ) diff --git a/control/autoware_smart_mpc_trajectory_follower/README.md b/control/autoware_smart_mpc_trajectory_follower/README.md index bb31a1be3a8b5..31270f701fce6 100644 --- a/control/autoware_smart_mpc_trajectory_follower/README.md +++ b/control/autoware_smart_mpc_trajectory_follower/README.md @@ -17,20 +17,6 @@ This technology makes it relatively easy to operate model predictive control, wh

-## Setup - -After building autoware, move to `control/autoware_smart_mpc_trajectory_follower` and run the following command: - -```bash -pip3 install . -``` - -If you have already installed and want to update the package, run the following command instead: - -```bash -pip3 install -U . -``` - ## Provided features This package provides smart MPC logic for path-following control as well as mechanisms for learning and evaluation. These features are described below. diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py index 84a0486b1e074..445897a4c580f 100644 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py @@ -13,8 +13,8 @@ # limitations under the License. +from autoware_smart_mpc_trajectory_follower import proxima_calc from autoware_smart_mpc_trajectory_follower.scripts import drive_functions -from autoware_smart_mpc_trajectory_follower.scripts import proxima_calc import numpy as np import torch from torch import nn diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py index 81ad4f8b38790..24773ed2d3e20 100644 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py @@ -17,23 +17,21 @@ """Define primary parameters and functions to be used elsewhere.""" from functools import partial -import json import os from pathlib import Path import sys from typing import Callable +from ament_index_python.packages import get_package_share_directory from numba import njit import numpy as np import yaml -package_path_json = str(Path(__file__).parent.parent) + "/package_path.json" -with open(package_path_json, "r") as file: - package_path = json.load(file) +PACKAGE_NAME = "autoware_smart_mpc_trajectory_follower" -mpc_param_path = ( - package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml" -) +param_path = Path(get_package_share_directory(PACKAGE_NAME)) / "param" + +mpc_param_path = param_path / "mpc_param.yaml" with open(mpc_param_path, "r") as yml: mpc_param = yaml.safe_load(yml) @@ -102,9 +100,7 @@ cap_pred_error = np.array(mpc_param["mpc_parameter"]["preprocessing"]["cap_pred_error"]) -nominal_param_path = ( - package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml" -) +nominal_param_path = param_path / "nominal_param.yaml" with open(nominal_param_path, "r") as yml: nominal_param = yaml.safe_load(yml) # Vehicle body information given by default. @@ -173,10 +169,7 @@ ) max_error_steer = float(mpc_param["mpc_parameter"]["compensation"]["max_error_steer"]) - -trained_model_param_path = ( - package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml" -) +trained_model_param_path = param_path / "trained_model_param.yaml" with open(trained_model_param_path, "r") as yml: trained_model_param = yaml.safe_load(yml) use_trained_model_diff = bool( @@ -370,31 +363,12 @@ ) # upper limit of input - -package_path_split = str(package_path["path"]).split("/") -control_dir_path = "" -for i in range(len(package_path_split)): - control_dir_path += package_path_split[i] - if package_path_split[i] == "control": - break - control_dir_path += "/" - read_limit_file = bool(mpc_param["mpc_parameter"]["limit"]["read_limit_file"]) if read_limit_file: - limit_yaml_path = None - for curDir, dirs, files in os.walk(control_dir_path): - for name in files: - if name == "vehicle_cmd_gate.param.yaml": - if curDir.split("/")[-2] == "autoware_vehicle_cmd_gate": - limit_yaml_path = curDir + "/" + name - break - - limit_params = None - if limit_yaml_path is not None: - with open(limit_yaml_path, "r") as yml: - limit_params = yaml.safe_load(yml) - else: - print("Error: limit_yaml_path is None") + limitter_package_path = Path(get_package_share_directory("autoware_vehicle_cmd_gate")) + limit_yaml_path = limitter_package_path / "config" / "vehicle_cmd_gate.param.yaml" + with open(limit_yaml_path, "r") as yml: + limit_params = yaml.safe_load(yml) if limit_params is not None: reference_speed_points = np.array( diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/src/proxima_calc.cpp similarity index 100% rename from control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/src/proxima_calc.cpp diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml index 46ce55058a000..3fbe1bedc106e 100644 --- a/control/autoware_smart_mpc_trajectory_follower/package.xml +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -25,9 +25,11 @@ autoware_planning_msgs autoware_system_msgs autoware_universe_utils + autoware_vehicle_cmd_gate autoware_vehicle_msgs pybind11_vendor python3-scipy + python3-torch rclcpp rclcpp_components diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py similarity index 100% rename from control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py rename to control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py diff --git a/control/autoware_smart_mpc_trajectory_follower/setup.py b/control/autoware_smart_mpc_trajectory_follower/setup.py deleted file mode 100644 index 22844b0f2d869..0000000000000 --- a/control/autoware_smart_mpc_trajectory_follower/setup.py +++ /dev/null @@ -1,42 +0,0 @@ -# cspell: ignore numba -import glob -import json -from pathlib import Path -import subprocess - -from setuptools import find_packages -from setuptools import setup - -SKIP_PRE_INSTALL_FLAG = False - -if not SKIP_PRE_INSTALL_FLAG: - subprocess.run("pip3 install numba==0.58.1 --force-reinstall", shell=True) - subprocess.run("pip3 install pybind11", shell=True) - subprocess.run("pip3 install GPy", shell=True) - subprocess.run("pip3 install torch", shell=True) - package_path = {} - package_path["path"] = str(Path(__file__).parent) - with open("autoware_smart_mpc_trajectory_follower/package_path.json", "w") as f: - json.dump(package_path, f) - build_cpp_command = ( - "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) " - ) - build_cpp_command += "autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp " - build_cpp_command += "-o autoware_smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) " - build_cpp_command += "-lrt -I/usr/include/eigen3" - subprocess.run(build_cpp_command, shell=True) - -so_path = ( - "scripts/" - + glob.glob("autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[ - -1 - ] -) -setup( - name="autoware_smart_mpc_trajectory_follower", - version="2.0.0", - packages=find_packages(), - package_data={ - "autoware_smart_mpc_trajectory_follower": ["package_path.json", so_path], - }, -) From 96dae8d3bcea24b0a63416f07845e79fef5fe6a9 Mon Sep 17 00:00:00 2001 From: Masato Saeki <78376491+MasatoSaeki@users.noreply.github.com> Date: Thu, 1 Aug 2024 17:32:04 +0900 Subject: [PATCH 014/126] fix(tier4_perception_launch): set `use_image_transport` in launch (#8315) set use_image_transport in launch Signed-off-by: MasatoSaeki --- .../traffic_light_node_container.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index 79c99398966da..74b89e9298da4 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -88,7 +88,7 @@ def create_parameter_dict(*args): package="autoware_traffic_light_visualization", plugin="autoware::traffic_light::TrafficLightRoiVisualizerNode", name="traffic_light_roi_visualizer", - parameters=[create_parameter_dict("enable_fine_detection")], + parameters=[create_parameter_dict("enable_fine_detection", "use_image_transport")], remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", LaunchConfiguration("output/rois")), @@ -172,6 +172,7 @@ def add_launch_arg(name: str, default_value=None, description=None): classifier_share_dir = get_package_share_directory("autoware_traffic_light_classifier") add_launch_arg("enable_image_decompressor", "True") add_launch_arg("enable_fine_detection", "True") + add_launch_arg("use_image_transport", "True") add_launch_arg("input/image", "/sensing/camera/traffic_light/image_raw") add_launch_arg("output/rois", "/perception/traffic_light_recognition/rois") add_launch_arg( From 50bf7b216a931c998c81c28d0e3d1d04d8c9283f Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 2 Aug 2024 10:03:11 +0900 Subject: [PATCH 015/126] fix(autoware_velocity_smoother): fix passedByValue (#8207) * fix:passedByValue Signed-off-by: kobayu858 * fix:clang format Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../velocity_planning_utils.hpp | 4 ++-- .../velocity_planning_utils.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 541ba62e95d74..0640bc950f411 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -45,8 +45,8 @@ bool calcStopVelocityWithConstantJerkAccLimit( const std::vector & times, const size_t start_index, TrajectoryPoints & output_trajectory); void updateStopVelocityStatus( - double v0, double a0, double jerk_acc, double jerk_dec, int type, std::vector times, - double t, double & x, double & v, double & a, double & j); + double v0, double a0, double jerk_acc, double jerk_dec, int type, + const std::vector & times, double t, double & x, double & v, double & a, double & j); double integ_x(double x0, double v0, double a0, double j0, double t); double integ_v(double v0, double a0, double j0, double t); double integ_a(double a0, double j0, double t); diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 739ba6e7cef38..9d474a9a52bfa 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -271,8 +271,8 @@ bool calcStopVelocityWithConstantJerkAccLimit( } void updateStopVelocityStatus( - double v0, double a0, double jerk_acc, double jerk_dec, int type, std::vector times, - double t, double & x, double & v, double & a, double & j) + double v0, double a0, double jerk_acc, double jerk_dec, int type, + const std::vector & times, double t, double & x, double & v, double & a, double & j) { if (type == 1) { if (0 <= t && t < times.at(0)) { From c7432e1f2d41b9f5869417654368ebbec7ffd445 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 2 Aug 2024 10:51:02 +0900 Subject: [PATCH 016/126] feat(autonomous_emergency_braking): add info marker and override for state (#8312) add info marker and override for state Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking.param.yaml | 1 + .../autonomous_emergency_braking/node.hpp | 6 ++- .../src/node.cpp | 41 ++++++++++++------- 3 files changed, 33 insertions(+), 15 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 25db9448f758b..f13e95c6db8e5 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -6,6 +6,7 @@ use_pointcloud_data: true use_predicted_object_data: true use_object_velocity_calculation: true + check_autoware_state: true min_generated_path_length: 0.5 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 9fd6d0192439c..8ee0b08690d09 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -260,7 +260,8 @@ class AEB : public rclcpp::Node this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; - rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug + rclcpp::Publisher::SharedPtr debug_marker_publisher_; + rclcpp::Publisher::SharedPtr info_marker_publisher_; // timer rclcpp::TimerBase::SharedPtr timer_; @@ -308,6 +309,8 @@ class AEB : public rclcpp::Node void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); + void addVirtualStopWallMarker(MarkerArray & markers); + std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed); @@ -335,6 +338,7 @@ class AEB : public rclcpp::Node bool use_pointcloud_data_; bool use_predicted_object_data_; bool use_object_velocity_calculation_; + bool check_autoware_state_; double path_footprint_extra_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 69bedd02d201b..fc9ea51dd701f 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -125,7 +125,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) { pub_obstacle_pointcloud_ = this->create_publisher("~/debug/obstacle_pointcloud", 1); - debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); + debug_marker_publisher_ = this->create_publisher("~/debug/markers", 1); + info_marker_publisher_ = this->create_publisher("~/info/markers", 1); } // Diagnostics { @@ -140,6 +141,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); use_predicted_object_data_ = declare_parameter("use_predicted_object_data"); use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); + check_autoware_state_ = declare_parameter("check_autoware_state"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); detection_range_max_height_margin_ = @@ -193,6 +195,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); updateParam( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); + updateParam(parameters, "check_autoware_state", check_autoware_state_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); updateParam(parameters, "detection_range_min_height", detection_range_min_height_); updateParam( @@ -363,7 +366,7 @@ bool AEB::fetchLatestData() } autoware_state_ = sub_autoware_state_.takeData(); - if (!autoware_state_) { + if (check_autoware_state_ && !autoware_state_) { return missing("autoware_state"); } @@ -373,6 +376,7 @@ bool AEB::fetchLatestData() void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) { MarkerArray debug_markers; + MarkerArray info_markers; checkCollision(debug_markers); if (!collision_data_keeper_.checkCollisionExpired()) { @@ -386,6 +390,7 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) if (publish_debug_markers_) { addCollisionMarker(data, debug_markers); } + addVirtualStopWallMarker(info_markers); } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -393,7 +398,8 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) } // publish debug markers - debug_ego_path_publisher_->publish(debug_markers); + debug_marker_publisher_->publish(debug_markers); + info_marker_publisher_->publish(info_markers); } bool AEB::checkCollision(MarkerArray & debug_markers) @@ -406,7 +412,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) } // if not driving, disable aeb - if (autoware_state_->state != AutowareState::DRIVING) { + if (check_autoware_state_ && autoware_state_->state != AutowareState::DRIVING) { return false; } @@ -890,20 +896,17 @@ void AEB::addMarker( "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n"; closest_object_velocity_marker_array.text += "Object relative velocity to ego: " + std::to_string(obj.velocity - std::abs(ego_velocity)) + - " [m/s]"; + " [m/s]\n"; + closest_object_velocity_marker_array.text += + "Object distance to ego: " + std::to_string(obj.distance_to_object) + " [m]\n"; + closest_object_velocity_marker_array.text += + "RSS distance: " + std::to_string(obj.rss) + " [m]"; debug_markers.markers.push_back(closest_object_velocity_marker_array); } } -void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) +void AEB::addVirtualStopWallMarker(MarkerArray & markers) { - auto point_marker = autoware::universe_utils::createDefaultMarker( - "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, - autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3), - autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); - point_marker.pose.position = data.position; - debug_markers.markers.push_back(point_marker); - const auto ego_map_pose = std::invoke([this]() -> std::optional { geometry_msgs::msg::TransformStamped tf_current_pose; geometry_msgs::msg::Pose p; @@ -928,10 +931,20 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0); const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker( ego_front_pose, "autonomous_emergency_braking", this->now(), 0); - autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &debug_markers); + autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &markers); } } +void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) +{ + auto point_marker = autoware::universe_utils::createDefaultMarker( + "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); + point_marker.pose.position = data.position; + debug_markers.markers.push_back(point_marker); +} + } // namespace autoware::motion::control::autonomous_emergency_braking #include From 2e7eb3e41a20093b9857a8c2a090a3fac04c1171 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 2 Aug 2024 11:47:59 +0900 Subject: [PATCH 017/126] fix(autoware_behavior_velocity_occlusion_module): fix passedByValue (#8211) fix:passedByValue Signed-off-by: kobayu858 --- .../src/debug.cpp | 4 ++-- .../src/occlusion_spot_utils.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp index a32cb4018cda9..5760b6de66a71 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -131,7 +131,7 @@ MarkerArray makeDebugInfoMarkers(T & debug_data) } MarkerArray makePolygonMarker( - const BasicPolygons & polygons, const std::string ns, const int id, const double z) + const BasicPolygons & polygons, const std::string & ns, const int id, const double z) { MarkerArray debug_markers; Marker debug_marker; @@ -159,7 +159,7 @@ MarkerArray makePolygonMarker( } MarkerArray makeSlicePolygonMarker( - const Polygons2d & slices, const std::string ns, const int id, const double z) + const Polygons2d & slices, const std::string & ns, const int id, const double z) { MarkerArray debug_markers; Marker debug_marker; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 911842ddd57ed..6d40c2167961a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -102,7 +102,7 @@ void calcSlowDownPointsForPossibleCollision( } std::sort( possible_collisions.begin(), possible_collisions.end(), - [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { + [](const PossibleCollisionInfo & pc1, const PossibleCollisionInfo & pc2) { return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; }); From b00e8e9ba2c244d62ec59e71d9bd653f56c96d97 Mon Sep 17 00:00:00 2001 From: Hayate TOBA <105347690+bathteayo@users.noreply.github.com> Date: Fri, 2 Aug 2024 13:41:01 +0900 Subject: [PATCH 018/126] fix(autoware_perception_rviz_plugin): fix passedByValue (#8192) * fix: passedByValue Signed-off-by: bathteayo <105347690+bathteayo@users.noreply.github.com> * fix:passedByValue Signed-off-by: kobayu858 --------- Signed-off-by: bathteayo <105347690+bathteayo@users.noreply.github.com> Signed-off-by: kobayu858 Co-authored-by: kobayu858 --- .../object_detection/object_polygon_detail.hpp | 2 +- .../src/object_detection/object_polygon_detail.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index 8d29900e1da26..fed20e24f9787 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -104,7 +104,7 @@ get_2d_shape_marker_ptr( AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std::string label, const std_msgs::msg::ColorRGBA & color_rgba); + const std::string & label, const std_msgs::msg::ColorRGBA & color_rgba); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 910bd6ed815e6..9c25c2dc3781e 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -459,7 +459,7 @@ visualization_msgs::msg::Marker::SharedPtr get_uuid_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, - const std::string label, const std_msgs::msg::ColorRGBA & color_rgba) + const std::string & label, const std_msgs::msg::ColorRGBA & color_rgba) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; From f105db93899954b121b012bbea4da8c502c1c5f2 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 2 Aug 2024 13:41:49 +0900 Subject: [PATCH 019/126] fix(autoware_obstacle_stop_planner): fix passedByValue (#8189) fix:passedByValue Signed-off-by: kobayu858 --- .../src/adaptive_cruise_control.cpp | 2 +- .../src/adaptive_cruise_control.hpp | 2 +- planning/autoware_obstacle_stop_planner/src/planner_utils.cpp | 2 +- planning/autoware_obstacle_stop_planner/src/planner_utils.hpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp index c5fcf29466936..fda53f6b0b6a6 100644 --- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -748,7 +748,7 @@ void AdaptiveCruiseController::registerQueToVelocity( est_vel_que_.emplace_back(new_vel); } -double AdaptiveCruiseController::getMedianVel(const std::vector vel_que) +double AdaptiveCruiseController::getMedianVel(const std::vector & vel_que) { if (vel_que.size() == 0) { RCLCPP_WARN_STREAM(node_->get_logger(), "size of vel que is 0. Something has wrong."); diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp index 586866f9a94d6..579a0ccc1cedb 100644 --- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp +++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp @@ -183,7 +183,7 @@ class AdaptiveCruiseController }; Param param_; - double getMedianVel(const std::vector vel_que); + double getMedianVel(const std::vector & vel_que); double lowpass_filter(const double current_value, const double prev_value, const double gain); void calcDistanceToNearestPointOnPath( const TrajectoryPoints & trajectory, const int nearest_point_idx, diff --git a/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp b/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp index bff8f9588215e..28c5523f49620 100644 --- a/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/autoware_obstacle_stop_planner/src/planner_utils.cpp @@ -228,7 +228,7 @@ std::string jsonDumpsPose(const Pose & pose) return json_dumps_pose; } -DiagnosticStatus makeStopReasonDiag(const std::string stop_reason, const Pose & stop_pose) +DiagnosticStatus makeStopReasonDiag(const std::string & stop_reason, const Pose & stop_pose) { DiagnosticStatus stop_reason_diag; KeyValue stop_reason_diag_kv; diff --git a/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp b/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp index 21943c46f5ca5..228c71d08a9b9 100644 --- a/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp +++ b/planning/autoware_obstacle_stop_planner/src/planner_utils.hpp @@ -122,7 +122,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( std::string jsonDumpsPose(const Pose & pose); -DiagnosticStatus makeStopReasonDiag(const std::string stop_reason, const Pose & stop_pose); +DiagnosticStatus makeStopReasonDiag(const std::string & stop_reason, const Pose & stop_pose); TrajectoryPoint getBackwardPointFromBasePoint( const TrajectoryPoint & p_from, const TrajectoryPoint & p_to, const TrajectoryPoint & p_base, From 2adb6f838e84b743872201176840fa4d55f3765a Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Fri, 2 Aug 2024 14:35:08 +0900 Subject: [PATCH 020/126] fix(autoware_pure_pursuit): fix redundantInitialization redundantInitialization (#8225) * fix(autoware_pure_pursuit): fix redundantInitialization redundantInitialization Signed-off-by: Ryuta Kambe * style(pre-commit): autofix * Update control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp --------- Signed-off-by: Ryuta Kambe Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takayuki Murooka --- .../autoware_pure_pursuit_lateral_controller.cpp | 16 +++++++--------- .../autoware_pure_pursuit_core/interpolate.cpp | 3 +-- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp index 803a6ef1e8617..085addbe17fc9 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp @@ -450,15 +450,13 @@ boost::optional PurePursuitLateralController::calcTargetCurvature( const bool is_reverse = (target_vel < 0); const double min_lookahead_distance = is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; - double lookahead_distance = min_lookahead_distance; - if (is_control_output) { - lookahead_distance = calcLookaheadDistance( - lateral_error, current_curvature, current_odometry_.twist.twist.linear.x, - min_lookahead_distance, is_control_output); - } else { - lookahead_distance = calcLookaheadDistance( - lateral_error, current_curvature, target_vel, min_lookahead_distance, is_control_output); - } + double lookahead_distance = + is_control_output + ? calcLookaheadDistance( + lateral_error, current_curvature, current_odometry_.twist.twist.linear.x, + min_lookahead_distance, is_control_output) + : calcLookaheadDistance( + lateral_error, current_curvature, target_vel, min_lookahead_distance, is_control_output); // Set PurePursuit data pure_pursuit_->setCurrentPose(pose); diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp index 3a79b1f01a804..9b68594f167e8 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp @@ -113,9 +113,8 @@ void SplineInterpolate::generateSpline(const std::vector & x) std::vector w_; w_.push_back(0.0); - double tmp; for (int i = 1; i < N - 1; i++) { - tmp = 1.0 / (4.0 - w_[i - 1]); + const double tmp = 1.0 / (4.0 - w_[i - 1]); c_[i] = (c_[i] - c_[i - 1]) * tmp; w_.push_back(tmp); } From 1e297b196b5881df0a4f0ac9b61f37232e3d0207 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 2 Aug 2024 14:55:30 +0900 Subject: [PATCH 021/126] docs(autonomous_emergency_braking): update readme for new param (#8330) update readme for new param Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index db27ca1b4f699..1382a24d98b0a 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -197,6 +197,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | | use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | | use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | +| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | true | | detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | | detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | | voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | From 04a00eff1f69d8dde85b7b3dd05c32dbb4f014f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Fri, 2 Aug 2024 15:26:47 +0900 Subject: [PATCH 022/126] feat(shape_estimation): add ml shape estimation (#7860) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(shape_estimation): add ml shape estimation Signed-off-by: Kaan Çolak * style(pre-commit): autofix * feat(shape_estimation): fix exceed objects Signed-off-by: Kaan Çolak * style(pre-commit): autofix * feat(shape_estimation): fix indent Signed-off-by: Kaan Çolak --------- Signed-off-by: Kaan Çolak Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../autoware_shape_estimation/CMakeLists.txt | 38 +- .../autoware_shape_estimation/README.md | 127 ++++++- .../config/shape_estimation.param.yaml | 20 +- .../tensorrt_shape_estimator.hpp | 93 +++++ .../launch/shape_estimation.launch.xml | 6 +- .../lib/tensorrt_shape_estimator.cpp | 338 ++++++++++++++++++ .../autoware_shape_estimation/package.xml | 8 + .../schema/shape_estimation.schema.json | 31 ++ .../src/shape_estimation_node.cpp | 38 ++ .../src/shape_estimation_node.hpp | 13 + 10 files changed, 701 insertions(+), 11 deletions(-) create mode 100644 perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp create mode 100644 perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp diff --git a/perception/autoware_shape_estimation/CMakeLists.txt b/perception/autoware_shape_estimation/CMakeLists.txt index b19f422c28d27..96bf946ab11db 100644 --- a/perception/autoware_shape_estimation/CMakeLists.txt +++ b/perception/autoware_shape_estimation/CMakeLists.txt @@ -10,13 +10,17 @@ find_package(pcl_conversions REQUIRED) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) +find_package(CUDA) +find_package(CUDNN) +find_package(TENSORRT) + set(SHAPE_ESTIMATION_DEPENDENCIES PCL OpenCV Eigen3 ) -ament_auto_add_library(${PROJECT_NAME}_lib SHARED +set(${PROJECT_NAME}_SOURCES lib/shape_estimator.cpp lib/model/bounding_box.cpp lib/model/convex_hull.cpp @@ -31,6 +35,19 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED lib/corrector/no_corrector.cpp ) +if(${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND}) + message("CUDA found, including CUDA-specific sources") + list(APPEND ${PROJECT_NAME}_SOURCES lib/tensorrt_shape_estimator.cpp) + add_definitions(-DUSE_CUDA) +else() + message("CUDA not found, excluding CUDA-specific sources") +endif() + +ament_auto_add_library(${PROJECT_NAME}_lib + SHARED + ${${PROJECT_NAME}_SOURCES} +) + ament_target_dependencies(${PROJECT_NAME}_lib ${SHAPE_ESTIMATION_DEPENDENCIES}) target_include_directories(${PROJECT_NAME}_lib @@ -39,6 +56,13 @@ target_include_directories(${PROJECT_NAME}_lib "${EIGEN3_INCLUDE_DIR}" ) +if(${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND}) +target_include_directories(${PROJECT_NAME}_lib + SYSTEM PUBLIC + "${TENSORRT_INCLUDE_DIRS}" +) +endif() + ament_auto_add_library(${PROJECT_NAME} SHARED src/shape_estimation_node.cpp ) @@ -57,6 +81,16 @@ target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_lib ) +if(${CUDA_FOUND} AND ${CUDNN_FOUND} AND ${TENSORRT_FOUND}) + target_link_libraries(${PROJECT_NAME} + ${TENSORRT_LIBRARIES} + ) +endif() + +target_compile_definitions(${PROJECT_NAME} PRIVATE + TENSORRT_VERSION_MAJOR=${TENSORRT_VERSION_MAJOR} +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::shape_estimation::ShapeEstimationNode" EXECUTABLE shape_estimation_node @@ -79,6 +113,6 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_shape_estimation ${test_files}) target_link_libraries(test_shape_estimation - ${PROJECT_NAME} + ${PROJECT_NAME} ) endif() diff --git a/perception/autoware_shape_estimation/README.md b/perception/autoware_shape_estimation/README.md index 5f184c32f1805..d3cb88ae130d1 100644 --- a/perception/autoware_shape_estimation/README.md +++ b/perception/autoware_shape_estimation/README.md @@ -10,7 +10,8 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull - bounding box - L-shape fitting. See reference below for details. + - L-shape fitting: See reference below for details + - ML based shape fitting: See ML Based Shape Fitting Implementation section below for details - cylinder @@ -38,6 +39,118 @@ This node calculates a refined object shape (bounding box, cylinder, convex hull {{ json_to_markdown("perception/autoware_shape_estimation/schema/shape_estimation.schema.json") }} +## ML Based Shape Implementation + +The model takes a point cloud and object label(provided by camera detections/Apollo instance segmentation) as an input and outputs the 3D bounding box of the object. + +ML based shape estimation algorithm uses a PointNet model as a backbone to estimate the 3D bounding box of the object. The model is trained on the NuScenes dataset with vehicle labels (Car, Truck, Bus, Trailer). + +The implemented model is concatenated with STN (Spatial Transformer Network) to learn the transformation of the input point cloud to the canonical space and PointNet to predict the 3D bounding box of the object. +Bounding box estimation part of _Frustum PointNets for 3D Object Detection from RGB-D Data_ paper used as a reference. + +The model predicts the following outputs for each object: + +- x,y,z coordinates of the object center +- object heading angle classification result(Uses 12 bins for angle classification - 30 degrees each) +- object heading angle residuals +- object size classification result +- object size residuals + +### Training ML Based Shape Estimation Model + +To train the model, you need ground truth 3D bounding box annotations. When using the mmdetection3d repository for training a 3D object detection algorithm, these ground truth annotations are +saved and utilized for data augmentation. These annotations are used as an essential dataset for training the shape estimation model effectively. + +### Preparing the Dataset + +#### Install MMDetection3D prerequisites + +**Step 1.** Download and install Miniconda from the [official website](https://mmpretrain.readthedocs.io/en/latest/get_started.html). + +**Step 2.** Create a conda virtual environment and activate it + +```bash +conda create --name train-shape-estimation python=3.8 -y +conda activate train-shape-estimation +``` + +**Step 3.** Install PyTorch + +```bash +conda install pytorch torchvision -c pytorch +``` + +#### Install mmdetection3d + +**Step 1.** Install MMEngine, MMCV, and MMDetection using MIM + +```bash +pip install -U openmim +mim install mmengine +mim install 'mmcv>=2.0.0rc4' +mim install 'mmdet>=3.0.0rc5, <3.3.0' +``` + +**Step 2.** Install Autoware's MMDetection3D fork + +```bash +git clone https://github.com/autowarefoundation/mmdetection3d.git +cd mmdetection3d +pip install -v -e . +``` + +#### Preparing NuScenes dataset for training + +**Step 1.** Download the NuScenes dataset from the [official website](https://www.nuscenes.org/download) and extract the dataset to a folder of your choice. + +**Note:** The NuScenes dataset is large and requires significant disk space. Ensure you have enough storage available before proceeding. + +**Step 2.** Create a symbolic link to the dataset folder + +```bash +ln -s /path/to/nuscenes/dataset/ /path/to/mmdetection3d/data/nuscenes/ +``` + +**Step 3.** Prepare the NuScenes data by running: + +```bash +cd mmdetection3d +python tools/create_data.py nuscenes --root-path ./data/nuscenes --out-dir ./data/nuscenes --extra-tag nuscenes --only-gt-database True +``` + +#### Clone Bounding Box Estimator model + +```bash +git clone https://github.com/autowarefoundation/bbox_estimator.git +``` + +#### Split the dataset into training and validation sets + +```bash + +cd bbox_estimator +python3 utils/split_dbinfos.py --dataset_path /path/to/mmdetection3d/data/nuscenes --classes 'car' 'truck' 'bus' 'trailer' --train_ratio 0.8 +``` + +### Training and Deploying the model + +#### Training the model + +```bash +# Detailed training options can be found in the training script +# For more details, run `python3 train.py --help` +python3 train.py --dataset_path /path/to/mmdetection3d/data/nuscenes +``` + +#### Deploying the model + +```bash +# Convert the trained model to ONNX format +python3 onnx_converter.py --weight_path /path/to/best_checkpoint.pth --output_path /path/to/output.onnx +``` + +Give the output path of the ONNX model to the `model_path` parameter in the `shape_estimation` node launch file. + ## Assumptions / Known limits TBD @@ -56,3 +169,15 @@ month = {June}, keywords = {autonomous driving, laser scanner, perception, segmentation}, } ``` + +Frustum PointNets for 3D Object Detection from RGB-D Data: + +````bibtex +@inproceedings{qi2018frustum, +title={Frustum pointnets for 3d object detection from rgb-d data}, +author={Qi, Charles R and Liu, Wei and Wu, Chenxia and Su, Hao and Guibas, Leonidas J}, +booktitle={Proceedings of the IEEE conference on computer vision and pattern recognition}, +pages={918--927}, +year={2018} +}``` +```` diff --git a/perception/autoware_shape_estimation/config/shape_estimation.param.yaml b/perception/autoware_shape_estimation/config/shape_estimation.param.yaml index e277d99d70edd..00af06a3703f2 100644 --- a/perception/autoware_shape_estimation/config/shape_estimation.param.yaml +++ b/perception/autoware_shape_estimation/config/shape_estimation.param.yaml @@ -1,8 +1,14 @@ /**: - ros__parameters: - use_corrector: true - use_filter: true - use_vehicle_reference_yaw: false - use_vehicle_reference_shape_size: false - use_boost_bbox_optimizer: false - fix_filtered_objects_label_to_unknown: true + ros__parameters: + use_corrector: true + use_filter: true + use_vehicle_reference_yaw: false + use_vehicle_reference_shape_size: false + use_boost_bbox_optimizer: false + fix_filtered_objects_label_to_unknown: true + model_params: + use_ml_shape_estimator: false + minimum_points: 16 + precision: "fp32" + batch_size: 32 + build_only: false diff --git a/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp new file mode 100644 index 0000000000000..5d0b7032445c8 --- /dev/null +++ b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp @@ -0,0 +1,93 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__SHAPE_ESTIMATION__TENSORRT_SHAPE_ESTIMATOR_HPP_ +#define AUTOWARE__SHAPE_ESTIMATION__TENSORRT_SHAPE_ESTIMATOR_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::shape_estimation +{ +using cuda_utils::CudaUniquePtr; +using cuda_utils::CudaUniquePtrHost; +using cuda_utils::makeCudaStream; +using cuda_utils::StreamUniquePtr; + +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using Label = autoware_perception_msgs::msg::ObjectClassification; + +class TrtShapeEstimator +{ +public: + TrtShapeEstimator( + const std::string & model_path, const std::string & precision, + const tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size = (1 << 30), + const tensorrt_common::BuildConfig build_config = + tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0)); + + ~TrtShapeEstimator(); + + bool inference(const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output); + +private: + void preprocess(const DetectedObjectsWithFeature & input); + + bool feed_forward_and_decode( + const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output); + + void postprocess(); + + static double class2angle(int pred_cls, double residual, int num_class); + + StreamUniquePtr stream_{makeCudaStream()}; + std::unique_ptr trt_common_; + + std::vector input_pc_h_; + CudaUniquePtr input_pc_d_; + + std::vector input_one_hot_h_; + CudaUniquePtr input_one_hot_d_; + + size_t out_s1center_elem_num_; + size_t out_s1center_elem_num_per_batch_; + CudaUniquePtr out_s1center_prob_d_; + CudaUniquePtrHost out_s1center_prob_h_; + + size_t out_pred_elem_num_; + size_t out_pred_elem_num_per_batch_; + CudaUniquePtr out_pred_prob_d_; + CudaUniquePtrHost out_pred_prob_h_; + + std::vector> g_type_mean_size_; + size_t batch_size_; +}; +} // namespace autoware::shape_estimation +#endif // AUTOWARE__SHAPE_ESTIMATION__TENSORRT_SHAPE_ESTIMATOR_HPP_ diff --git a/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml b/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml index 816653013c6c8..40fc0a930ac23 100644 --- a/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml +++ b/perception/autoware_shape_estimation/launch/shape_estimation.launch.xml @@ -2,12 +2,16 @@ + + + - + + diff --git a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp new file mode 100644 index 0000000000000..a097060081dc9 --- /dev/null +++ b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp @@ -0,0 +1,338 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/shape_estimation/tensorrt_shape_estimator.hpp" + +#include + +#include + +#include +#include + +namespace autoware::shape_estimation +{ +TrtShapeEstimator::TrtShapeEstimator( + const std::string & model_path, const std::string & precision, + const tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size, + const tensorrt_common::BuildConfig build_config) +{ + trt_common_ = std::make_unique( + model_path, precision, nullptr, batch_config, max_workspace_size, build_config); + + trt_common_->setup(); + + if (!trt_common_->isInitialized()) { + std::cerr << "Failed to initialize TensorRT" << std::endl; + return; + } + + const auto pc_input_dims = trt_common_->getBindingDimensions(0); + const auto pc_input_size = std::accumulate( + pc_input_dims.d + 1, pc_input_dims.d + pc_input_dims.nbDims, 1, std::multiplies()); + input_pc_d_ = cuda_utils::make_unique(pc_input_size * batch_config[2]); + batch_size_ = batch_config[2]; + const auto one_hot_input_dims = trt_common_->getBindingDimensions(1); + const auto one_hot_input_size = std::accumulate( + one_hot_input_dims.d + 1, one_hot_input_dims.d + one_hot_input_dims.nbDims, 1, + std::multiplies()); + input_one_hot_d_ = cuda_utils::make_unique(one_hot_input_size * batch_config[2]); + + const auto stage1_center_out_dims = trt_common_->getBindingDimensions(2); + out_s1center_elem_num_ = std::accumulate( + stage1_center_out_dims.d + 1, stage1_center_out_dims.d + stage1_center_out_dims.nbDims, 1, + std::multiplies()); + out_s1center_elem_num_ = out_s1center_elem_num_ * batch_config[2]; + out_s1center_elem_num_per_batch_ = static_cast(out_s1center_elem_num_ / batch_config[2]); + out_s1center_prob_d_ = cuda_utils::make_unique(out_s1center_elem_num_); + out_s1center_prob_h_ = + cuda_utils::make_unique_host(out_s1center_elem_num_, cudaHostAllocPortable); + + const auto pred_out_dims = trt_common_->getBindingDimensions(3); + out_pred_elem_num_ = std::accumulate( + pred_out_dims.d + 1, pred_out_dims.d + pred_out_dims.nbDims, 1, std::multiplies()); + out_pred_elem_num_ = out_pred_elem_num_ * batch_config[2]; + out_pred_elem_num_per_batch_ = static_cast(out_pred_elem_num_ / batch_config[2]); + out_pred_prob_d_ = cuda_utils::make_unique(out_pred_elem_num_); + out_pred_prob_h_ = + cuda_utils::make_unique_host(out_pred_elem_num_, cudaHostAllocPortable); + + g_type_mean_size_ = {{4.6344314, 1.9600292, 1.7375569}, {6.936331, 2.5178623, 2.8506238}, + {11.194943, 2.9501154, 3.4918275}, {12.275775, 2.9231303, 3.87086}, + {0.80057803, 0.5983815, 1.27450867}, {1.76282397, 0.59706367, 1.73698127}, + {16.17150617, 2.53246914, 3.53079012}, {3.64300781, 1.54298177, 1.92320313}}; +} + +bool TrtShapeEstimator::inference( + const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output) +{ + if (!trt_common_->isInitialized()) { + return false; + } + + bool result = false; + + for (size_t i = 0; i < input.feature_objects.size(); i += batch_size_) { + DetectedObjectsWithFeature input_batch; + input_batch.header = input.header; + + for (size_t j = 0; j < batch_size_ && (i + j) < input.feature_objects.size(); ++j) { + input_batch.feature_objects.push_back(input.feature_objects[i + j]); + } + + preprocess(input_batch); + DetectedObjectsWithFeature output_batch; + result = feed_forward_and_decode(input_batch, output_batch); + + output.feature_objects.insert( + output.feature_objects.end(), output_batch.feature_objects.begin(), + output_batch.feature_objects.end()); + } + + return result; +} + +void TrtShapeEstimator::preprocess(const DetectedObjectsWithFeature & input) +{ + auto input_dims_pc = trt_common_->getBindingDimensions(0); + int batch_size = static_cast(input.feature_objects.size()); + + const auto input_chan = static_cast(input_dims_pc.d[1]); + const auto input_pc_size = static_cast(input_dims_pc.d[2]); + + auto input_dims_one_hot = trt_common_->getBindingDimensions(1); + const auto input_one_hot_size = static_cast(input_dims_one_hot.d[1]); + + int volume_pc = batch_size * input_chan * input_pc_size; + input_pc_h_.resize(volume_pc); + + int volume_one_hot = batch_size * input_one_hot_size; + input_one_hot_h_.resize(volume_one_hot); + + // fill point cloud + for (size_t i = 0; i < input.feature_objects.size(); i++) { + const auto & feature_object = input.feature_objects[i]; + + int point_size_of_cloud = + feature_object.feature.cluster.width * feature_object.feature.cluster.height; + + if (point_size_of_cloud <= input_pc_size) { + int offset = 0; + for (sensor_msgs::PointCloud2ConstIterator x_iter(feature_object.feature.cluster, "x"), + y_iter(feature_object.feature.cluster, "y"), z_iter(feature_object.feature.cluster, "z"); + x_iter != x_iter.end(); ++x_iter, ++y_iter, ++z_iter) { + input_pc_h_[i * input_chan * input_pc_size + 0 + offset] = -1 * (*y_iter); + input_pc_h_[i * input_chan * input_pc_size + 512 + offset] = -1 * (*z_iter); + input_pc_h_[i * input_chan * input_pc_size + 1024 + offset] = *x_iter; + offset++; + } + + int iter_count = static_cast(input_pc_size) / point_size_of_cloud; + int remainer_count = static_cast(input_pc_size) % point_size_of_cloud; + + for (int j = 1; j < iter_count; j++) { + for (int k = 0; k < point_size_of_cloud; k++) { + input_pc_h_[i * input_chan * input_pc_size + 0 + j * point_size_of_cloud + k] = + input_pc_h_[i * input_chan * input_pc_size + k]; + + input_pc_h_[i * input_chan * input_pc_size + 512 + j * point_size_of_cloud + k] = + input_pc_h_[i * input_chan * input_pc_size + 512 + k]; + + input_pc_h_[i * input_chan * input_pc_size + 1024 + j * point_size_of_cloud + k] = + input_pc_h_[i * input_chan * input_pc_size + 1024 + k]; + } + } + + for (int j = 0; j < remainer_count; j++) { + input_pc_h_[i * input_chan * input_pc_size + 0 + iter_count * point_size_of_cloud + j] = + input_pc_h_[i * input_chan * input_pc_size + j]; + + input_pc_h_[i * input_chan * input_pc_size + 512 + iter_count * point_size_of_cloud + j] = + input_pc_h_[i * input_chan * input_pc_size + 512 + j]; + + input_pc_h_[i * input_chan * input_pc_size + 1024 + iter_count * point_size_of_cloud + j] = + input_pc_h_[i * input_chan * input_pc_size + 1024 + j]; + } + + } else { + std::vector sampled_points; + std::vector indices( + feature_object.feature.cluster.width * feature_object.feature.cluster.height); + std::iota(indices.begin(), indices.end(), 0); + + std::sample( + indices.begin(), indices.end(), std::back_inserter(sampled_points), 512, + std::mt19937{std::random_device{}()}); + + // Create an iterator to read the points + sensor_msgs::PointCloud2ConstIterator iter_x(feature_object.feature.cluster, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(feature_object.feature.cluster, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(feature_object.feature.cluster, "z"); + + int offset = 0; + for (const auto & index : sampled_points) { + auto idx = index * feature_object.feature.cluster.point_step / sizeof(float); + input_pc_h_[i * input_chan * input_pc_size + 0 + offset] = (-1 * iter_y[idx]); + input_pc_h_[i * input_chan * input_pc_size + 512 + offset] = (-1 * iter_z[idx]); + input_pc_h_[i * input_chan * input_pc_size + 1024 + offset] = iter_x[idx]; + offset++; + } + } + + const auto & label = feature_object.object.classification.front().label; + + // Initialize all elements to 0.0f + input_one_hot_h_[i * 4 + 0] = 0.0f; + input_one_hot_h_[i * 4 + 1] = 0.0f; + input_one_hot_h_[i * 4 + 2] = 0.0f; + input_one_hot_h_[i * 4 + 3] = 0.0f; + + if (label == Label::CAR) { + input_one_hot_h_[i * 4 + 0] = 1.0f; + } else if (label == Label::TRUCK) { + input_one_hot_h_[i * 4 + 1] = 1.0f; + } else if (label == Label::BUS) { + input_one_hot_h_[i * 4 + 2] = 1.0f; + } else if (label == Label::TRAILER) { + input_one_hot_h_[i * 4 + 3] = 1.0f; + } + } + + CHECK_CUDA_ERROR(cudaMemcpy( + input_pc_d_.get(), input_pc_h_.data(), input_pc_h_.size() * sizeof(float), + cudaMemcpyHostToDevice)); + CHECK_CUDA_ERROR(cudaMemcpy( + input_one_hot_d_.get(), input_one_hot_h_.data(), input_one_hot_h_.size() * sizeof(float), + cudaMemcpyHostToDevice)); +} + +bool TrtShapeEstimator::feed_forward_and_decode( + const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output) +{ + int batch_size = static_cast(input.feature_objects.size()); + std::vector buffers = { + input_pc_d_.get(), input_one_hot_d_.get(), out_s1center_prob_d_.get(), out_pred_prob_d_.get()}; + trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + out_s1center_prob_h_.get(), out_s1center_prob_d_.get(), out_s1center_elem_num_ * sizeof(float), + cudaMemcpyDeviceToHost, *stream_)); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + out_pred_prob_h_.get(), out_pred_prob_d_.get(), out_pred_elem_num_ * sizeof(float), + cudaMemcpyDeviceToHost, *stream_)); + + cudaStreamSynchronize(*stream_); + + float * out_stage1_net = out_s1center_prob_h_.get(); + float * out_pred_net = out_pred_prob_h_.get(); + + output.feature_objects.resize(batch_size); + for (int i = 0; i < batch_size; i++) { + output.feature_objects.at(i) = input.feature_objects.at(i); + + autoware_perception_msgs::msg::Shape shape; + geometry_msgs::msg::Pose pose; + + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + + pose.position.x = out_stage1_net[i * out_s1center_elem_num_per_batch_ + 2] + + out_pred_net[i * out_pred_elem_num_per_batch_ + 2]; + pose.position.y = + -(out_stage1_net[i * out_s1center_elem_num_per_batch_ + 0] + + out_pred_net[i * out_pred_elem_num_per_batch_ + 0]); + pose.position.z = + -(out_stage1_net[i * out_s1center_elem_num_per_batch_ + 1] + + out_pred_net[i * out_pred_elem_num_per_batch_ + 1]); + + int idx = 0; + idx += 3; + + int NUM_HEADING_BIN = 12; + int NUM_SIZE_CLUSTER = 8; + + int heading_idx = 0; + float max = std::numeric_limits::lowest(); + for (int j = idx; j < idx + NUM_HEADING_BIN; j++) { + if (out_pred_net[i * out_pred_elem_num_per_batch_ + j] > max) { + max = out_pred_net[i * out_pred_elem_num_per_batch_ + j]; + heading_idx = j - idx; + } + } + + idx += NUM_HEADING_BIN; + + float heading_residual = + out_pred_net[i * out_pred_elem_num_per_batch_ + idx + heading_idx] * (M_PI / NUM_HEADING_BIN); + + idx += NUM_HEADING_BIN; + + float heading = class2angle(heading_idx, heading_residual, NUM_HEADING_BIN); + + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, -M_PI / 2 - heading); + + pose.orientation.x = quaternion.x(); + pose.orientation.y = quaternion.y(); + pose.orientation.z = quaternion.z(); + pose.orientation.w = quaternion.w(); + + int size_idx = 0; + max = std::numeric_limits::lowest(); + for (int j = idx; j < idx + NUM_SIZE_CLUSTER; j++) { + if (out_pred_net[i * out_pred_elem_num_per_batch_ + j] > max) { + max = out_pred_net[i * out_pred_elem_num_per_batch_ + j]; + size_idx = j - idx; + } + } + + idx += NUM_SIZE_CLUSTER; + float x_size = out_pred_net[i * out_pred_elem_num_per_batch_ + idx + size_idx * 3] * + g_type_mean_size_[size_idx][0] + + g_type_mean_size_[size_idx][0]; + + float y_size = out_pred_net[i * out_pred_elem_num_per_batch_ + idx + size_idx * 3 + 1] * + g_type_mean_size_[size_idx][1] + + g_type_mean_size_[size_idx][1]; + float z_size = out_pred_net[i * out_pred_elem_num_per_batch_ + idx + size_idx * 3 + 2] * + g_type_mean_size_[size_idx][2] + + g_type_mean_size_[size_idx][2]; + + shape.dimensions.x = x_size; + shape.dimensions.y = y_size; + shape.dimensions.z = z_size; + + output.feature_objects.at(i).object.shape = shape; + output.feature_objects.at(i).object.kinematics.pose_with_covariance.pose = pose; + } + + return true; +} + +double TrtShapeEstimator::class2angle(int pred_cls, double residual, int num_class) +{ + double angle_per_class = 2.0 * M_PI / static_cast(num_class); + double angle_center = static_cast(pred_cls) * angle_per_class; + double angle = angle_center + residual; + + // Adjust angle to be within the range [-pi, pi] + while (angle > M_PI) { + angle -= 2.0 * M_PI; + } + while (angle < -M_PI) { + angle += 2.0 * M_PI; + } + return angle; +}; +} // namespace autoware::shape_estimation diff --git a/perception/autoware_shape_estimation/package.xml b/perception/autoware_shape_estimation/package.xml index ad226a5cc2f66..fec1009428113 100644 --- a/perception/autoware_shape_estimation/package.xml +++ b/perception/autoware_shape_estimation/package.xml @@ -6,19 +6,27 @@ This package implements a shape estimation algorithm as a ROS 2 node Yukihiro Saito Yoshi Ri + Kaan Colak Apache License 2.0 ament_cmake autoware_cmake + cudnn_cmake_module + tensorrt_cmake_module + + cudnn_cmake_module + tensorrt_cmake_module autoware_perception_msgs autoware_universe_utils + cuda_utils eigen libopencv-dev libpcl-all-dev pcl_conversions rclcpp rclcpp_components + tensorrt_common tf2 tf2_eigen tf2_geometry_msgs diff --git a/perception/autoware_shape_estimation/schema/shape_estimation.schema.json b/perception/autoware_shape_estimation/schema/shape_estimation.schema.json index d81bfa636a923..6a7e3d25af534 100644 --- a/perception/autoware_shape_estimation/schema/shape_estimation.schema.json +++ b/perception/autoware_shape_estimation/schema/shape_estimation.schema.json @@ -30,6 +30,37 @@ "type": "boolean", "description": "The flag to use boost bbox optimizer", "default": "false" + }, + "model_params": { + "type": "object", + "description": "Parameters for model configuration.", + "properties": { + "use_ml_shape_estimator": { + "type": "boolean", + "description": "The flag to apply use ml bounding box estimator.", + "default": "true" + }, + "minimum_points": { + "type": "integer", + "description": "The minimum number of points to fit a bounding box.", + "default": "16" + }, + "precision": { + "type": "string", + "description": "The precision of the model.", + "default": "fp32" + }, + "batch_size": { + "type": "integer", + "description": "The batch size of the model.", + "default": "32" + }, + "build_only": { + "type": "boolean", + "description": "The flag to build the model only.", + "default": "false" + } + } } }, "required": [ diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp index 4a373b15d580f..9c9c996ba0c6d 100644 --- a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp +++ b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp @@ -56,6 +56,23 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); +#ifdef USE_CUDA + use_ml_shape_estimation_ = declare_parameter("model_params.use_ml_shape_estimator"); + if (use_ml_shape_estimation_) { + std::string model_path = declare_parameter("model_path"); + min_points_ = declare_parameter("model_params.minimum_points"); + std::string precision = declare_parameter("model_params.precision"); + int batch_size = declare_parameter("model_params.batch_size"); + tensorrt_common::BatchConfig batch_config{batch_size, batch_size, batch_size}; + tensorrt_shape_estimator_ = + std::make_unique(model_path, precision, batch_config); + if (this->declare_parameter("model_params.build_only", false)) { + RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); + rclcpp::shutdown(); + } + } +#endif + processing_time_publisher_ = std::make_unique(this, "shape_estimation"); stop_watch_ptr_ = @@ -94,6 +111,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared DetectedObjectsWithFeature output_msg; output_msg.header = input_msg->header; + // Create ml model input batch + DetectedObjectsWithFeature input_trt_batch; + // Estimate shape for each object and pack msg for (const auto & feature_object : input_msg->feature_objects) { const auto & object = feature_object.object; @@ -109,6 +129,14 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared continue; } +#ifdef USE_CUDA + // If ml based shape estimation is enabled, add object to input batch and continue + if (is_vehicle && use_ml_shape_estimation_ && cluster->size() > min_points_) { + input_trt_batch.feature_objects.push_back(feature_object); + continue; + } +#endif + // estimate shape and pose autoware_perception_msgs::msg::Shape shape; geometry_msgs::msg::Pose pose; @@ -138,6 +166,16 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared output_msg.feature_objects.back().object.kinematics.pose_with_covariance.pose = pose; } +#ifdef USE_CUDA + DetectedObjectsWithFeature output_model; + if (use_ml_shape_estimation_ && !input_trt_batch.feature_objects.empty()) { + tensorrt_shape_estimator_->inference(input_trt_batch, output_model); + } + for (auto & feature_object : output_model.feature_objects) { + output_msg.feature_objects.push_back(feature_object); + } +#endif + // Publish pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(pub_, output_msg.header.stamp); diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.hpp b/perception/autoware_shape_estimation/src/shape_estimation_node.hpp index d9f944346611a..a98d45453086b 100644 --- a/perception/autoware_shape_estimation/src/shape_estimation_node.hpp +++ b/perception/autoware_shape_estimation/src/shape_estimation_node.hpp @@ -17,6 +17,12 @@ #include "autoware/shape_estimation/shape_estimator.hpp" +#ifdef USE_CUDA +#include "autoware/shape_estimation/tensorrt_shape_estimator.hpp" + +#include +#endif + #include #include #include @@ -51,6 +57,13 @@ class ShapeEstimationNode : public rclcpp::Node bool use_vehicle_reference_shape_size_; bool fix_filtered_objects_label_to_unknown_; +#ifdef USE_CUDA + std::unique_ptr tensorrt_shape_estimator_; +#endif + + bool use_ml_shape_estimation_; + size_t min_points_; + public: explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options); }; From ad574d764f32a14621fdceb05ce23feff714132f Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Fri, 2 Aug 2024 15:43:53 +0900 Subject: [PATCH 023/126] fix(diagnostic_graph_aggregator): fix functionConst (#8279) * fix:functionConst Signed-off-by: kobayu858 * fix:functionConst Signed-off-by: kobayu858 * fix:clang format Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- system/diagnostic_graph_aggregator/src/common/graph/graph.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp b/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp index ed7bac0e96ce3..0e64f25860fda 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp @@ -31,7 +31,7 @@ class Graph { public: void create(const std::string & file, const std::string & id = ""); - void update(const rclcpp::Time & stamp); + void update(const rclcpp::Time & stamp); // cppcheck-suppress functionConst bool update(const rclcpp::Time & stamp, const DiagnosticStatus & status); const auto & nodes() const { return nodes_; } const auto & diags() const { return diags_; } From 43439ac89e2abc4a8f91da13f0a6f34367701752 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 2 Aug 2024 16:22:40 +0900 Subject: [PATCH 024/126] fix(lane_change): relax finish judge (#8133) * fix(lane_change): relax finish judge Signed-off-by: Muhammad Zulfaqar Azmi * documentation update Signed-off-by: Zulfaqar Azmi * update readme explanations Signed-off-by: Zulfaqar Azmi * update config Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../README.md | 66 ++++++++++++++++++- .../config/lane_change.param.yaml | 5 +- .../utils/data_structs.hpp | 5 +- .../utils/utils.hpp | 2 + .../src/manager.cpp | 9 ++- .../src/scene.cpp | 31 ++++++--- .../src/utils/utils.cpp | 11 ++++ 7 files changed, 114 insertions(+), 15 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 63f5a2ec05bf1..967e8352b830a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -618,6 +618,61 @@ The last behavior will also occur if the ego vehicle has departed from the curre ![stop](./images/lane_change-cant_cancel_no_abort.png) +## Lane change completion checks + +To determine if the ego vehicle has successfully changed lanes, one of two criteria must be met: either the longitudinal or the lateral criteria. + +For the longitudinal criteria, the ego vehicle must pass the lane-changing end pose and be within the `finish_judge_buffer` distance from it. The module then checks if the ego vehicle is in the target lane. If true, the module returns success. This check ensures that the planner manager updates the root lanelet correctly based on the ego vehicle's current pose. Without this check, if the ego vehicle is changing lanes while avoiding an obstacle and its current pose is in the original lane, the planner manager might set the root lanelet as the original lane. This would force the ego vehicle to perform the lane change again. With the target lane check, the ego vehicle is confirmed to be in the target lane, and the planner manager can correctly update the root lanelets. + +If the longitudinal criteria are not met, the module evaluates the lateral criteria. For the lateral criteria, the ego vehicle must be within `finish_judge_lateral_threshold` distance from the target lane's centerline, and the angle deviation must be within `finish_judge_lateral_angle_deviation` degrees. The angle deviation check ensures there is no sudden steering. If the angle deviation is set too high, the ego vehicle's orientation could deviate significantly from the centerline, causing the trajectory follower to aggressively correct the steering to return to the centerline. Keeping the angle deviation value as small as possible avoids this issue. + +The process of determining lane change completion is shown in the following diagram. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Lane change completion judge + +start + +:Calculate distance from current ego pose to lane change end pose; + +if (Is ego velocity < 1.0?) then (YES) + :Set finish_judge_buffer to 0.0; +else (NO) + :Set finish_judge_buffer to lane_change_finish_judge_buffer; +endif + +if (ego has passed the end_pose and ego is finish_judge_buffer meters away from end_pose?) then (YES) + if (Current ego pose is in target lanes' polygon?) then (YES) + :Lane change is completed; + stop + else (NO) +:Lane change is NOT completed; +stop + endif +else (NO) +endif + +if (ego's yaw deviation to centerline exceeds finish_judge_lateral_angle_deviation?) then (YES) + :Lane change is NOT completed; + stop +else (NO) + :Calculate distance to the target lanes' centerline; + if (abs(distance to the target lanes' centerline) is less than finish_judge_lateral_threshold?) then (YES) + :Lane change is completed; + stop + else (NO) + :Lane change is NOT completed; + stop + endif +endif + +@enduml +``` + ## Parameters ### Essential lane change parameters @@ -631,7 +686,6 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | | `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 2.0 | -| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | | `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | | `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | | `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | @@ -647,6 +701,16 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | | `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | +### Parameter to judge if lane change is completed + +The following parameters are used to judge lane change completion. + +| Name | Unit | Type | Description | Default value | +| :------------------------------------- | ----- | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------- | +| `lane_change_finish_judge_buffer` | [m] | double | The longitudinal distance starting from the lane change end pose. | 2.0 | +| `finish_judge_lateral_threshold` | [m] | double | The lateral distance from targets lanes' centerline. Used in addition with `finish_judge_lateral_angle_deviation` | 0.1 | +| `finish_judge_lateral_angle_deviation` | [deg] | double | Ego angle deviation with reference to target lanes' centerline. Used in addition with `finish_judge_lateral_threshold` | 2.0 | + ### Lane change regulations | Name | Unit | Type | Description | Default value | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index d2f695071649a..77cf9b7b11cbe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -6,7 +6,6 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] - lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] @@ -109,7 +108,9 @@ overhang_tolerance: 0.0 # [m] unsafe_hysteresis_threshold: 10 # [/] - finish_judge_lateral_threshold: 0.2 # [m] + lane_change_finish_judge_buffer: 2.0 # [m] + finish_judge_lateral_threshold: 0.1 # [m] + finish_judge_lateral_angle_deviation: 1.0 # [deg] # debug publish_debug_marker: false diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 495fe9012ecd0..696cf4bd58d32 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -109,7 +110,6 @@ struct Parameters double lane_changing_lateral_jerk{0.5}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.0}; - double lane_change_finish_judge_buffer{3.0}; LateralAccelerationMap lane_change_lat_acc_map; // parked vehicle @@ -157,7 +157,10 @@ struct Parameters // abort CancelParameters cancel{}; + // finish judge parameter + double lane_change_finish_judge_buffer{3.0}; double finish_judge_lateral_threshold{0.2}; + double finish_judge_lateral_angle_deviation{autoware::universe_utils::deg2rad(3.0)}; // debug marker bool publish_debug_marker{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 7bb6a06116a53..d566aceba413b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -314,6 +314,8 @@ bool is_ahead_of_ego( bool is_before_terminal( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const PredictedObject & object); + +double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 22c93f7d4bfd5..16c4058bb3911 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -166,8 +166,6 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("minimum_lane_changing_velocity")); p.minimum_lane_changing_velocity = std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration); - p.lane_change_finish_judge_buffer = - getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -224,8 +222,15 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) p.cancel.unsafe_hysteresis_threshold = getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); + // finish judge parameters + p.lane_change_finish_judge_buffer = + getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); p.finish_judge_lateral_threshold = getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); + const auto finish_judge_lateral_angle_deviation = + getOrDeclareParameter(*node, parameter("finish_judge_lateral_angle_deviation")); + p.finish_judge_lateral_angle_deviation = + autoware::universe_utils::deg2rad(finish_judge_lateral_angle_deviation); // debug marker p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 97eab5a03fcaa..5ae01c555b6f2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -23,6 +23,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include #include #include @@ -689,21 +690,33 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; const auto & target_lanes = get_target_lanes(); const double dist_to_lane_change_end = - utils::getSignedDistance(current_pose, lane_change_end, get_target_lanes()); - double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; + utils::getSignedDistance(current_pose, lane_change_end, target_lanes); - // If ego velocity is low, relax finish judge buffer - const double ego_velocity = getEgoVelocity(); - if (std::abs(ego_velocity) < 1.0) { - finish_judge_buffer = 0.0; - } + const auto finish_judge_buffer = std::invoke([&]() { + const double ego_velocity = getEgoVelocity(); + // If ego velocity is low, relax finish judge buffer + if (std::abs(ego_velocity) < 1.0) { + return 0.0; + } + return lane_change_parameters_->lane_change_finish_judge_buffer; + }); - const auto reach_lane_change_end = dist_to_lane_change_end + finish_judge_buffer < 0.0; + const auto has_passed_end_pose = dist_to_lane_change_end + finish_judge_buffer < 0.0; lane_change_debug_.distance_to_lane_change_finished = dist_to_lane_change_end + finish_judge_buffer; - if (!reach_lane_change_end) { + if (has_passed_end_pose) { + const auto & lanes_polygon = common_data_ptr_->lanes_polygon_ptr->target; + return !boost::geometry::disjoint( + lanes_polygon.value(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(current_pose.position))); + } + + const auto yaw_deviation_to_centerline = + utils::lane_change::calc_angle_to_lanelet_segment(target_lanes, current_pose); + + if (yaw_deviation_to_centerline > lane_change_parameters_->finish_judge_lateral_angle_deviation) { return false; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 176a7ece0084e..7e93365a59e7b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -1343,6 +1343,17 @@ bool is_before_terminal( } return current_max_dist >= 0.0; } + +double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose) +{ + lanelet::ConstLanelet closest_lanelet; + + if (!lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { + return autoware::universe_utils::deg2rad(180); + } + const auto closest_pose = lanelet::utils::getClosestCenterPose(closest_lanelet, pose.position); + return std::abs(autoware::universe_utils::calcYawDeviation(closest_pose, pose)); +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug From f07fa8edaf25e778e91e5b0fc5a92bdfcb418d25 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 2 Aug 2024 17:24:57 +0900 Subject: [PATCH 025/126] feat(lane_change): use different rss param to deal with parked vehicle (#8316) * different rss value for parked vehicle Signed-off-by: Muhammad Zulfaqar Azmi * Documentation and config file update Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../README.md | 12 ++++++++++++ .../config/lane_change.param.yaml | 8 ++++++++ .../utils/data_structs.hpp | 1 + .../src/manager.cpp | 17 +++++++++++++++++ .../src/scene.cpp | 7 ++++++- 5 files changed, 44 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 967e8352b830a..e42e6e2d47738 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -773,6 +773,18 @@ The following parameters are used to judge lane change completion. | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +#### safety constraints specifically for stopped or parked vehicles + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.parked.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.parked.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.0 | +| `safety_check.parked.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 1.0 | +| `safety_check.parked.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 0.8 | +| `safety_check.parked.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | +| `safety_check.parked.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.parked.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | + ##### safety constraints to cancel lane change path | Name | Unit | Type | Description | Default value | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 77cf9b7b11cbe..7a9c466fec260 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -38,6 +38,14 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + parked: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.0 cancel: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 696cf4bd58d32..7962c878d7d64 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -151,6 +151,7 @@ struct Parameters bool allow_loose_check_for_cancel{true}; double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; + utils::path_safety_checker::RSSparams rss_params_for_parked{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; utils::path_safety_checker::RSSparams rss_params_for_stuck{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 16c4058bb3911..f3f371ec02a9b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -122,6 +122,23 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.execution.lateral_distance_max_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_velocity_delta_time")); + p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_front_deceleration")); + p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_rear_deceleration")); + p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_reaction_time")); + p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin")); + p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.lateral_distance_max_threshold")); + p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 5ae01c555b6f2..022cf2c9be7bd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -2082,9 +2082,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); auto is_safe = true; + const auto selected_rss_param = + (obj.initial_twist.twist.linear.x <= + lane_change_parameters_->prepare_segment_ignore_object_velocity_thresh) + ? lane_change_parameters_->rss_params_for_parked + : rss_params; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0, get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold, current_debug_data.second); From 87df6ad358215b404a00241cd4cdfc8aabccb483 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 2 Aug 2024 18:14:26 +0900 Subject: [PATCH 026/126] feat(autonomous_emergency_braking): add some tests to aeb (#8126) * add initial tests Signed-off-by: Daniel Sanchez * add more tests Signed-off-by: Daniel Sanchez * more tests Signed-off-by: Daniel Sanchez * WIP add publishing and test subscription Signed-off-by: Daniel Sanchez * add more tests Signed-off-by: Daniel Sanchez * fix lint cmake Signed-off-by: Daniel Sanchez * WIP tf topic Signed-off-by: Daniel Sanchez * Revert "WIP tf topic" This reverts commit b5ef11b499e719b2cdbe0464bd7de7778de54e76. Signed-off-by: Daniel Sanchez * add path crop test Signed-off-by: Daniel Sanchez * add test for transform object Signed-off-by: Daniel Sanchez * add briefs Signed-off-by: Daniel Sanchez * delete repeated test Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../CMakeLists.txt | 7 +- .../autonomous_emergency_braking/node.hpp | 192 ++++++++++- ...re_autonomous_emergency_braking.launch.xml | 1 - .../package.xml | 8 +- .../src/node.cpp | 3 +- .../src/utils.cpp | 3 + .../test/test.cpp | 326 ++++++++++++++++++ .../test/test.hpp | 116 +++++++ .../launch/control.launch.xml | 1 - 9 files changed, 642 insertions(+), 15 deletions(-) create mode 100644 control/autoware_autonomous_emergency_braking/test/test.cpp create mode 100644 control/autoware_autonomous_emergency_braking/test/test.hpp diff --git a/control/autoware_autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt index 85290c6c66730..9f7a64c18da9a 100644 --- a/control/autoware_autonomous_emergency_braking/CMakeLists.txt +++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt @@ -30,8 +30,11 @@ rclcpp_components_register_node(${AEB_NODE} ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_aeb + test/test.cpp) + +target_link_libraries(test_aeb ${AEB_NODE}) + endif() ament_auto_package( diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 8ee0b08690d09..24eaa8516a732 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -71,6 +71,9 @@ using Vector3 = geometry_msgs::msg::Vector3; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; +/** + * @brief Struct to store object data + */ struct ObjectData { rclcpp::Time stamp; @@ -80,22 +83,44 @@ struct ObjectData double distance_to_object{0.0}; }; +/** + * @brief Class to manage collision data + */ class CollisionDataKeeper { public: + /** + * @brief Constructor for CollisionDataKeeper + * @param clock Shared pointer to the clock + */ explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } + /** + * @brief Set timeout values for collision and obstacle data + * @param collision_keep_time Time to keep collision data + * @param previous_obstacle_keep_time Time to keep previous obstacle data + */ void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time) { collision_keep_time_ = collision_keep_time; previous_obstacle_keep_time_ = previous_obstacle_keep_time; } + /** + * @brief Get timeout values for collision and obstacle data + * @return Pair of collision and obstacle data timeout values + */ std::pair getTimeout() { return {collision_keep_time_, previous_obstacle_keep_time_}; } + /** + * @brief Check if object data has expired + * @param data Optional reference to the object data + * @param timeout Timeout value to check against + * @return True if object data has expired, false otherwise + */ bool checkObjectDataExpired(std::optional & data, const double timeout) { if (!data.has_value()) return true; @@ -109,38 +134,70 @@ class CollisionDataKeeper return false; } + /** + * @brief Check if collision data has expired + * @return True if collision data has expired, false otherwise + */ bool checkCollisionExpired() { return this->checkObjectDataExpired(closest_object_, collision_keep_time_); } + /** + * @brief Check if previous object data has expired + * @return True if previous object data has expired, false otherwise + */ bool checkPreviousObjectDataExpired() { return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); } + /** + * @brief Get the closest object data + * @return Object data of the closest object + */ [[nodiscard]] ObjectData get() const { return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); } + /** + * @brief Get the previous closest object data + * @return Object data of the previous closest object + */ [[nodiscard]] ObjectData getPreviousObjectData() const { return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); } + /** + * @brief Set collision data + * @param data Object data to set + */ void setCollisionData(const ObjectData & data) { closest_object_ = std::make_optional(data); } + /** + * @brief Set previous object data + * @param data Object data to set + */ void setPreviousObjectData(const ObjectData & data) { prev_closest_object_ = std::make_optional(data); } + /** + * @brief Reset the obstacle velocity history + */ void resetVelocityHistory() { obstacle_velocity_history_.clear(); } + /** + * @brief Update the velocity history with current object velocity + * @param current_object_velocity Current object velocity + * @param current_object_velocity_time_stamp Timestamp of the current object velocity + */ void updateVelocityHistory( const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) { @@ -158,6 +215,10 @@ class CollisionDataKeeper current_object_velocity, current_object_velocity_time_stamp); } + /** + * @brief Get the median obstacle velocity from history + * @return Optional median obstacle velocity + */ [[nodiscard]] std::optional getMedianObstacleVelocity() const { if (obstacle_velocity_history_.empty()) return std::nullopt; @@ -177,6 +238,13 @@ class CollisionDataKeeper return (vel1 + vel2) / 2.0; } + /** + * @brief Calculate object speed from velocity history + * @param closest_object Closest object data + * @param path Ego vehicle path + * @param current_ego_speed Current ego vehicle speed + * @return Optional calculated object speed + */ std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed) { @@ -241,9 +309,16 @@ class CollisionDataKeeper rclcpp::Clock::SharedPtr clock_; }; +/** + * @brief Autonomous Emergency Braking (AEB) node + */ class AEB : public rclcpp::Node { public: + /** + * @brief Constructor for AEB + * @param node_options Options for the node + */ explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber @@ -267,53 +342,156 @@ class AEB : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // callback + /** + * @brief Callback for point cloud messages + * @param input_msg Shared pointer to the point cloud message + */ void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + + /** + * @brief Callback for IMU messages + * @param input_msg Shared pointer to the IMU message + */ void onImu(const Imu::ConstSharedPtr input_msg); + + /** + * @brief Timer callback function + */ void onTimer(); + + /** + * @brief Callback for parameter updates + * @param parameters Vector of updated parameters + * @return Set parameters result + */ rcl_interfaces::msg::SetParametersResult onParameter( const std::vector & parameters); + /** + * @brief Fetch the latest data from subscribers + * @return True if data fetch was successful, false otherwise + */ bool fetchLatestData(); - // main function + /** + * @brief Diagnostic check for collisions + * @param stat Diagnostic status wrapper + */ void onCheckCollision(DiagnosticStatusWrapper & stat); + + /** + * @brief Check for collisions + * @param debug_markers Marker array for debugging + * @return True if a collision is detected, false otherwise + */ bool checkCollision(MarkerArray & debug_markers); + + /** + * @brief Check if there is a collision with the closest object + * @param current_v Current velocity of the ego vehicle + * @param closest_object Data of the closest object + * @return True if a collision is detected, false otherwise + */ bool hasCollision(const double current_v, const ObjectData & closest_object); + /** + * @brief Generate the ego vehicle path + * @param curr_v Current velocity of the ego vehicle + * @param curr_w Current angular velocity of the ego vehicle + * @return Generated ego path + */ Path generateEgoPath(const double curr_v, const double curr_w); + + /** + * @brief Generate the ego vehicle path from the predicted trajectory + * @param predicted_traj Predicted trajectory of the ego vehicle + * @return Optional generated ego path + */ std::optional generateEgoPath(const Trajectory & predicted_traj); + + /** + * @brief Generate the footprint of the path with extra width margin + * @param path Ego vehicle path + * @param extra_width_margin Extra width margin for the footprint + * @return Vector of polygons representing the path footprint + */ std::vector generatePathFootprint(const Path & path, const double extra_width_margin); + /** + * @brief Create object data using point cloud clusters + * @param ego_path Ego vehicle path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param stamp Timestamp of the data + * @param objects Vector to store the created object data + * @param obstacle_points_ptr Pointer to the point cloud of obstacles + */ void createObjectDataUsingPointCloudClusters( const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr); + /** + * @brief Create object data using predicted objects + * @param ego_path Ego vehicle path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param objects Vector to store the created object data + */ void createObjectDataUsingPredictedObjects( const Path & ego_path, const std::vector & ego_polys, std::vector & objects); + /** + * @brief Crop the point cloud with the ego vehicle footprint path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param filtered_objects Pointer to the filtered point cloud of obstacles + */ void cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); - void createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects); - void cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys); - + /** + * @brief Add a marker for debugging + * @param current_time Current time + * @param path Ego vehicle path + * @param polygons Polygons representing the ego vehicle footprint + * @param objects Vector of object data + * @param closest_object Optional data of the closest object + * @param color_r Red color component + * @param color_g Green color component + * @param color_b Blue color component + * @param color_a Alpha (transparency) component + * @param ns Namespace for the marker + * @param debug_markers Marker array for debugging + */ void addMarker( const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, const std::vector & objects, const std::optional & closest_object, const double color_r, const double color_g, const double color_b, const double color_a, const std::string & ns, MarkerArray & debug_markers); + /** + * @brief Add a collision marker for debugging + * @param data Data of the collision object + * @param debug_markers Marker array for debugging + */ void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); + /** + * @brief Add an info marker stop wall in front of the ego vehicle + * @param markers Data of the closest object + */ void addVirtualStopWallMarker(MarkerArray & markers); + /** + * @brief Calculate object speed from history + * @param closest_object Data of the closest object + * @param path Ego vehicle path + * @param current_ego_speed Current speed of the ego vehicle + * @return Optional calculated object speed + */ std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed); + // Member variables PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; Vector3::SharedPtr angular_velocity_ptr_{nullptr}; @@ -330,7 +508,7 @@ class AEB : public rclcpp::Node // diag Updater updater_{this}; - // member variables + // Member variables bool publish_debug_pointcloud_; bool publish_debug_markers_; bool use_predicted_trajectory_; diff --git a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml index fe0df41ca89c7..34d0492799577 100644 --- a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -3,7 +3,6 @@ - diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index d0c75f893cd2d..173d419bf6d9a 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -14,12 +14,17 @@ ament_cmake autoware_cmake + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_test_utils autoware_control_msgs autoware_motion_utils autoware_planning_msgs autoware_pointcloud_preprocessor autoware_system_msgs + autoware_test_utils autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -38,9 +43,6 @@ tf2_ros visualization_msgs - ament_lint_auto - autoware_lint_common - ament_cmake diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index fc9ea51dd701f..cd97a8301ad9d 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -507,7 +507,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers) }; // Data of filtered point cloud - pcl::PointCloud::Ptr filtered_objects(new PointCloud); + pcl::PointCloud::Ptr filtered_objects = + pcl::make_shared>(); // evaluate if there is a collision for both paths const bool has_collision = has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index f3b5192855d84..1b9fad6af9860 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -49,6 +49,9 @@ PredictedObject transformObjectFrame( Polygon2d convertPolygonObjectToGeometryPolygon( const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { + if (obj_shape.footprint.points.empty()) { + return {}; + } Polygon2d object_polygon; tf2::Transform tf_map2obj; fromMsg(current_pose, tf_map2obj); diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp new file mode 100644 index 0000000000000..42054e718d09c --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -0,0 +1,326 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test.hpp" + +#include "autoware/autonomous_emergency_braking/node.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware::motion::control::autonomous_emergency_braking::test +{ +using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Vector3; +using std_msgs::msg::Header; + +Header get_header(const char * const frame_id, rclcpp::Time t) +{ + std_msgs::msg::Header header; + header.stamp = t; + header.frame_id = frame_id; + return header; +}; + +Imu make_imu_message( + const Header & header, const double ax, const double ay, const double yaw, + const double angular_velocity_z) +{ + Imu imu_msg; + imu_msg.header = header; + imu_msg.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + imu_msg.angular_velocity.z = angular_velocity_z; + imu_msg.linear_acceleration.x = ax; + imu_msg.linear_acceleration.y = ay; + return imu_msg; +}; + +VelocityReport make_velocity_report_msg( + const Header & header, const double lat_velocity, const double long_velocity, + const double heading_rate) +{ + VelocityReport velocity_msg; + velocity_msg.header = header; + velocity_msg.lateral_velocity = lat_velocity; + velocity_msg.longitudinal_velocity = long_velocity; + velocity_msg.heading_rate = heading_rate; + return velocity_msg; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto aeb_dir = + ament_index_cpp::get_package_share_directory("autoware_autonomous_emergency_braking"); + const auto vehicle_info_util_dir = + ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils"); + + node_options.arguments( + {"--ros-args", "--params-file", aeb_dir + "/config/autonomous_emergency_braking.param.yaml", + "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"}); + return std::make_shared(node_options); +}; + +std::shared_ptr generatePubSubNode() +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments({"--ros-args"}); + return std::make_shared(node_options); +}; + +PubSubNode::PubSubNode(const rclcpp::NodeOptions & node_options) +: Node("test_aeb_pubsub", node_options) +{ + rclcpp::QoS qos{1}; + qos.transient_local(); + + pub_imu_ = create_publisher("~/input/imu", qos); + pub_point_cloud_ = create_publisher("~/input/pointcloud", qos); + pub_velocity_ = create_publisher("~/input/velocity", qos); + pub_predicted_traj_ = create_publisher("~/input/predicted_trajectory", qos); + pub_predicted_objects_ = create_publisher("~/input/objects", qos); + pub_autoware_state_ = create_publisher("autoware/state", qos); +} + +TEST_F(TestAEB, checkCollision) +{ + constexpr double longitudinal_velocity = 3.0; + ObjectData object_collision; + object_collision.distance_to_object = 0.5; + object_collision.velocity = 0.1; + ASSERT_TRUE(aeb_node_->hasCollision(longitudinal_velocity, object_collision)); + + ObjectData object_no_collision; + object_no_collision.distance_to_object = 10.0; + object_no_collision.velocity = 0.1; + ASSERT_FALSE(aeb_node_->hasCollision(longitudinal_velocity, object_no_collision)); +} + +TEST_F(TestAEB, checkImuPathGeneration) +{ + constexpr double longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate); + ASSERT_FALSE(imu_path.empty()); + + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + ASSERT_TRUE(imu_path.size() >= static_cast(horizon / dt)); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); + + const auto stamp = rclcpp::Time(); + pcl::PointCloud::Ptr obstacle_points_ptr = + pcl::make_shared>(); + { + const double x_start{0.0}; + const double y_start{0.0}; + + for (size_t i = 0; i < 15; ++i) { + pcl::PointXYZ p1( + x_start + static_cast(i / 100.0), y_start - static_cast(i / 100.0), 0.5); + pcl::PointXYZ p2( + x_start + static_cast((i + 10) / 100.0), y_start - static_cast(i / 100.0), + 0.5); + obstacle_points_ptr->push_back(p1); + obstacle_points_ptr->push_back(p2); + } + } + std::vector objects; + aeb_node_->createObjectDataUsingPointCloudClusters( + imu_path, footprint, stamp, objects, obstacle_points_ptr); + ASSERT_FALSE(objects.empty()); +} + +TEST_F(TestAEB, checkIncompleteImuPathGeneration) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double min_generated_path_length = aeb_node_->min_generated_path_length_; + const double slow_velocity = min_generated_path_length / (2.0 * horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + ASSERT_TRUE(imu_path.size() >= static_cast(horizon / dt)); + ASSERT_TRUE(autoware::motion_utils::calcArcLength(imu_path) >= min_generated_path_length); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + +TEST_F(TestAEB, checkEmptyPathAtZeroSpeed) +{ + const double velocity = 0.0; + constexpr double yaw_rate = 0.0; + const auto imu_path = aeb_node_->generateEgoPath(velocity, yaw_rate); + ASSERT_EQ(imu_path.size(), 1); +} + +TEST_F(TestAEB, checkParamUpdate) +{ + std::vector parameters{rclcpp::Parameter("param")}; + const auto result = aeb_node_->onParameter(parameters); + ASSERT_TRUE(result.successful); +} + +TEST_F(TestAEB, checkEmptyFetchData) +{ + ASSERT_FALSE(aeb_node_->fetchLatestData()); +} + +TEST_F(TestAEB, checkConvertObjectToPolygon) +{ + using autoware_perception_msgs::msg::Shape; + PredictedObject obj_cylinder; + obj_cylinder.shape.type = Shape::CYLINDER; + obj_cylinder.shape.dimensions.x = 1.0; + Pose obj_cylinder_pose; + obj_cylinder_pose.position.x = 1.0; + obj_cylinder_pose.position.y = 1.0; + obj_cylinder.kinematics.initial_pose_with_covariance.pose = obj_cylinder_pose; + const auto cylinder_polygon = utils::convertObjToPolygon(obj_cylinder); + ASSERT_FALSE(cylinder_polygon.outer().empty()); + + PredictedObject obj_box; + obj_box.shape.type = Shape::BOUNDING_BOX; + obj_box.shape.dimensions.x = 1.0; + obj_box.shape.dimensions.y = 2.0; + Pose obj_box_pose; + obj_box_pose.position.x = 1.0; + obj_box_pose.position.y = 1.0; + obj_box.kinematics.initial_pose_with_covariance.pose = obj_box_pose; + const auto box_polygon = utils::convertObjToPolygon(obj_box); + ASSERT_FALSE(box_polygon.outer().empty()); + + geometry_msgs::msg::TransformStamped tf_stamped; + geometry_msgs::msg::Transform transform; + + constexpr double yaw{0.0}; + transform.rotation = autoware::universe_utils::createQuaternionFromYaw(yaw); + geometry_msgs::msg::Vector3 translation; + translation.x = 1.0; + translation.y = 0.0; + translation.z = 0.0; + transform.translation = translation; + tf_stamped.set__transform(transform); + const auto t_obj_box = utils::transformObjectFrame(obj_box, tf_stamped); + const auto t_pose = t_obj_box.kinematics.initial_pose_with_covariance.pose; + Pose expected_pose; + expected_pose.position.x = obj_box_pose.position.x + translation.x; + expected_pose.position.y = obj_box_pose.position.y + translation.y; + expected_pose.position.z = obj_box_pose.position.z + translation.z; + + ASSERT_DOUBLE_EQ(expected_pose.position.x, t_pose.position.x); + ASSERT_DOUBLE_EQ(expected_pose.position.y, t_pose.position.y); + ASSERT_DOUBLE_EQ(expected_pose.position.z, t_pose.position.z); +} + +TEST_F(TestAEB, CollisionDataKeeper) +{ + using namespace std::literals::chrono_literals; + constexpr double collision_keeping_sec{1.0}, previous_obstacle_keep_time{1.0}; + CollisionDataKeeper collision_data_keeper_(aeb_node_->get_clock()); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired()); + ASSERT_TRUE(collision_data_keeper_.checkPreviousObjectDataExpired()); + + ObjectData obj; + obj.stamp = aeb_node_->now(); + obj.velocity = 0.0; + obj.position.x = 0.0; + rclcpp::sleep_for(100ms); + + ObjectData obj2; + obj2.stamp = aeb_node_->now(); + obj2.velocity = 0.0; + obj2.position.x = 0.1; + rclcpp::sleep_for(100ms); + + constexpr double ego_longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.0; + const auto imu_path = aeb_node_->generateEgoPath(ego_longitudinal_velocity, yaw_rate); + + const auto speed_null = + collision_data_keeper_.calcObjectSpeedFromHistory(obj, imu_path, ego_longitudinal_velocity); + ASSERT_FALSE(speed_null.has_value()); + + const auto median_velocity = + collision_data_keeper_.calcObjectSpeedFromHistory(obj2, imu_path, ego_longitudinal_velocity); + ASSERT_TRUE(median_velocity.has_value()); + + // object speed is 1.0 m/s greater than ego's = 0.1 [m] / 0.1 [s] + longitudinal_velocity + ASSERT_TRUE(std::abs(median_velocity.value() - 4.0) < 1e-2); + rclcpp::sleep_for(1100ms); + ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired()); +} + +TEST_F(TestAEB, TestCropPointCloud) +{ + constexpr double longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate); + ASSERT_FALSE(imu_path.empty()); + + constexpr size_t n_points{15}; + // Create n_points inside the path and 1 point outside. + pcl::PointCloud::Ptr obstacle_points_ptr = + pcl::make_shared>(); + { + constexpr double x_start{0.0}; + constexpr double y_start{0.0}; + + for (size_t i = 0; i < n_points; ++i) { + const double offset_1 = static_cast(i / 100.0); + const double offset_2 = static_cast((i + 10) / 100.0); + pcl::PointXYZ p1(x_start + offset_1, y_start - offset_1, 0.5); + pcl::PointXYZ p2(x_start + offset_2, y_start - offset_1, 0.5); + obstacle_points_ptr->push_back(p1); + obstacle_points_ptr->push_back(p2); + } + pcl::PointXYZ p_out(x_start + 100.0, y_start + 100, 0.5); + obstacle_points_ptr->push_back(p_out); + } + aeb_node_->obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::toROSMsg(*obstacle_points_ptr, *aeb_node_->obstacle_ros_pointcloud_ptr_); + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + + pcl::PointCloud::Ptr filtered_objects = + pcl::make_shared>(); + aeb_node_->cropPointCloudWithEgoFootprintPath(footprint, filtered_objects); + // Check if the point outside the path was excluded + ASSERT_TRUE(filtered_objects->points.size() == 2 * n_points); +} + +} // namespace autoware::motion::control::autonomous_emergency_braking::test diff --git a/control/autoware_autonomous_emergency_braking/test/test.hpp b/control/autoware_autonomous_emergency_braking/test/test.hpp new file mode 100644 index 0000000000000..86edbf73a2a12 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/test/test.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HPP_ +#define TEST_HPP_ + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware_test_utils/mock_data_parser.hpp" +#include "gtest/gtest.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +namespace autoware::motion::control::autonomous_emergency_braking::test +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_system_msgs::msg::AutowareState; +using autoware_vehicle_msgs::msg::VelocityReport; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; +using sensor_msgs::msg::PointCloud2; +using PointCloud = pcl::PointCloud; +using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +using Path = std::vector; +using Vector3 = geometry_msgs::msg::Vector3; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using std_msgs::msg::Header; + +std::shared_ptr generateNode(); +Header get_header(const char * const frame_id, rclcpp::Time t); +Imu make_imu_message( + const Header & header, const double ax, const double ay, const double yaw, + const double angular_velocity_z); +VelocityReport make_velocity_report_msg( + const Header & header, const double lat_velocity, const double long_velocity, + const double heading_rate); +class PubSubNode : public rclcpp::Node +{ +public: + explicit PubSubNode(const rclcpp::NodeOptions & node_options); + // publisher + rclcpp::Publisher::SharedPtr pub_imu_; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + rclcpp::Publisher::SharedPtr pub_velocity_; + rclcpp::Publisher::SharedPtr pub_predicted_traj_; + rclcpp::Publisher::SharedPtr pub_predicted_objects_; + rclcpp::Publisher::SharedPtr pub_autoware_state_; + // timer + // rclcpp::TimerBase::SharedPtr timer_; + void publishDefaultTopicsNoSpin() + { + const auto header = get_header("base_link", now()); + const auto imu_msg = make_imu_message(header, 0.0, 0.0, 0.0, 0.05); + const auto velocity_msg = make_velocity_report_msg(header, 0.0, 3.0, 0.0); + + pub_imu_->publish(imu_msg); + pub_velocity_->publish(velocity_msg); + }; +}; + +std::shared_ptr generatePubSubNode(); + +class TestAEB : public ::testing::Test +{ +public: + TestAEB() {} + TestAEB(const TestAEB &) = delete; + TestAEB(TestAEB &&) = delete; + TestAEB & operator=(const TestAEB &) = delete; + TestAEB & operator=(TestAEB &&) = delete; + ~TestAEB() override = default; + + void SetUp() override + { + rclcpp::init(0, nullptr); + pub_sub_node_ = generatePubSubNode(); + aeb_node_ = generateNode(); + } + + void TearDown() override + { + aeb_node_.reset(); + pub_sub_node_.reset(); + rclcpp::shutdown(); + } + + std::shared_ptr pub_sub_node_; + std::shared_ptr aeb_node_; +}; +} // namespace autoware::motion::control::autonomous_emergency_braking::test + +#endif // TEST_HPP_ diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index f5582b7aeb305..0eda6faaf4e05 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -192,7 +192,6 @@ - From 19a1550bb669ac6a8393ce2f6c90f35703e20d58 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 06:58:29 +0900 Subject: [PATCH 027/126] fix(autoware_pointcloud_preprocessor): fix passedByValue (#8242) fix:passedByValue Signed-off-by: kobayu858 --- .../src/downsample_filter/robin_hood.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h index f04a84968dbbb..2818e34a0601f 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h @@ -1932,7 +1932,7 @@ class Table } template - iterator emplace_hint(const_iterator position, Args &&... args) + iterator emplace_hint(const const_iterator & position, Args &&... args) { (void)position; return emplace(std::forward(args)...).first; @@ -1951,14 +1951,14 @@ class Table } template - iterator try_emplace(const_iterator hint, const key_type & key, Args &&... args) + iterator try_emplace(const const_iterator & hint, const key_type & key, Args &&... args) { (void)hint; return try_emplace_impl(key, std::forward(args)...).first; } template - iterator try_emplace(const_iterator hint, key_type && key, Args &&... args) + iterator try_emplace(const const_iterator & hint, key_type && key, Args &&... args) { (void)hint; return try_emplace_impl(std::move(key), std::forward(args)...).first; @@ -1977,14 +1977,14 @@ class Table } template - iterator insert_or_assign(const_iterator hint, const key_type & key, Mapped && obj) + iterator insert_or_assign(const const_iterator & hint, const key_type & key, Mapped && obj) { (void)hint; return insertOrAssignImpl(key, std::forward(obj)).first; } template - iterator insert_or_assign(const_iterator hint, key_type && key, Mapped && obj) + iterator insert_or_assign(const const_iterator & hint, key_type && key, Mapped && obj) { (void)hint; return insertOrAssignImpl(std::move(key), std::forward(obj)).first; @@ -1996,7 +1996,7 @@ class Table return emplace(keyval); } - iterator insert(const_iterator hint, const value_type & keyval) + iterator insert(const const_iterator & hint, const value_type & keyval) { (void)hint; return emplace(keyval).first; @@ -2004,7 +2004,7 @@ class Table std::pair insert(value_type && keyval) { return emplace(std::move(keyval)); } - iterator insert(const_iterator hint, value_type && keyval) + iterator insert(const const_iterator & hint, value_type && keyval) { (void)hint; return emplace(std::move(keyval)).first; From f3dfbaf43c55a27a2257dc1a4e0596acd917ed4e Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 06:59:08 +0900 Subject: [PATCH 028/126] fix(autoware_probabilistic_occupancy_grid_map): fix functionConst (#8289) fix:functionConst Signed-off-by: kobayu858 --- .../lib/fusion_policy/fusion_policy.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp index 6d058b3bef6eb..993e220786de8 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/fusion_policy/fusion_policy.cpp @@ -202,17 +202,17 @@ struct dempsterShaferOccupancy } // calc conflict factor K - double calcK(const dempsterShaferOccupancy & other) + double calcK(const dempsterShaferOccupancy & other) const { return (occupied * other.empty + empty * other.occupied); } // calc sum of occupied probability mass - double calcOccupied(const dempsterShaferOccupancy & other) + double calcOccupied(const dempsterShaferOccupancy & other) const { return occupied * other.occupied + occupied * other.unknown + unknown * other.occupied; } // calc sum of empty probability mass - double calcEmpty(const dempsterShaferOccupancy & other) + double calcEmpty(const dempsterShaferOccupancy & other) const { return empty * other.empty + empty * other.unknown + unknown * other.empty; } @@ -240,7 +240,7 @@ struct dempsterShaferOccupancy } // get occupancy probability via Pignistic Probability - double getPignisticProbability() { return occupied + unknown / 2.0; } + double getPignisticProbability() const { return occupied + unknown / 2.0; } }; /** From 5fd4b2a62aa357a97986410ffdb092a6b98c2e34 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 06:59:40 +0900 Subject: [PATCH 029/126] fix(autoware_pointcloud_preprocessor): fix functionConst (#8280) fix:functionConst Signed-off-by: kobayu858 --- .../src/downsample_filter/robin_hood.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h index 2818e34a0601f..a50ea7f70b774 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h @@ -1134,7 +1134,7 @@ class Table map.deallocate(mData); } - void destroyDoNotDeallocate() noexcept { mData->~value_type(); } + void destroyDoNotDeallocate() const noexcept { mData->~value_type(); } value_type const * operator->() const noexcept { return mData; } From 513a2f2440447f8a6a602a62d0e1830c062c3b9d Mon Sep 17 00:00:00 2001 From: Hayate TOBA <105347690+bathteayo@users.noreply.github.com> Date: Mon, 5 Aug 2024 11:35:53 +0900 Subject: [PATCH 030/126] fix(autoware_vehicle_cmd_gate): fix passedByValue (#8243) fix: passedByValue Signed-off-by: bathteayo <105347690+bathteayo@users.noreply.github.com> --- .../src/marker_helper.hpp | 2 +- .../src/vehicle_cmd_filter.cpp | 14 +++++++------- .../src/vehicle_cmd_filter.hpp | 14 +++++++------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp b/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp index c9d7e86fa23dd..a2e357f6068a9 100644 --- a/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp +++ b/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp @@ -96,7 +96,7 @@ inline visualization_msgs::msg::Marker createMarker( inline visualization_msgs::msg::Marker createStringMarker( const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type, geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale, - const std_msgs::msg::ColorRGBA & color, const std::string text) + const std_msgs::msg::ColorRGBA & color, const std::string & text) { visualization_msgs::msg::Marker marker; diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index cec23b05b8191..7d2860d6372cf 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -40,43 +40,43 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam & param_ = p; return true; } -void VehicleCmdFilter::setSteerLim(LimitArray v) +void VehicleCmdFilter::setSteerLim(const LimitArray & v) { auto tmp = param_; tmp.steer_lim = v; setParameterWithValidation(tmp); } -void VehicleCmdFilter::setSteerRateLim(LimitArray v) +void VehicleCmdFilter::setSteerRateLim(const LimitArray & v) { auto tmp = param_; tmp.steer_rate_lim = v; setParameterWithValidation(tmp); } -void VehicleCmdFilter::setLonAccLim(LimitArray v) +void VehicleCmdFilter::setLonAccLim(const LimitArray & v) { auto tmp = param_; tmp.lon_acc_lim = v; setParameterWithValidation(tmp); } -void VehicleCmdFilter::setLonJerkLim(LimitArray v) +void VehicleCmdFilter::setLonJerkLim(const LimitArray & v) { auto tmp = param_; tmp.lon_jerk_lim = v; setParameterWithValidation(tmp); } -void VehicleCmdFilter::setLatAccLim(LimitArray v) +void VehicleCmdFilter::setLatAccLim(const LimitArray & v) { auto tmp = param_; tmp.lat_acc_lim = v; setParameterWithValidation(tmp); } -void VehicleCmdFilter::setLatJerkLim(LimitArray v) +void VehicleCmdFilter::setLatJerkLim(const LimitArray & v) { auto tmp = param_; tmp.lat_jerk_lim = v; setParameterWithValidation(tmp); } -void VehicleCmdFilter::setActualSteerDiffLim(LimitArray v) +void VehicleCmdFilter::setActualSteerDiffLim(const LimitArray & v) { auto tmp = param_; tmp.actual_steer_diff_lim = v; diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index 96663474f47a4..02c7874f584ef 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -49,13 +49,13 @@ class VehicleCmdFilter void setWheelBase(double v) { param_.wheel_base = v; } void setVelLim(double v) { param_.vel_lim = v; } - void setSteerLim(LimitArray v); - void setSteerRateLim(LimitArray v); - void setLonAccLim(LimitArray v); - void setLonJerkLim(LimitArray v); - void setLatAccLim(LimitArray v); - void setLatJerkLim(LimitArray v); - void setActualSteerDiffLim(LimitArray v); + void setSteerLim(const LimitArray & v); + void setSteerRateLim(const LimitArray & v); + void setLonAccLim(const LimitArray & v); + void setLonJerkLim(const LimitArray & v); + void setLatAccLim(const LimitArray & v); + void setLatJerkLim(const LimitArray & v); + void setActualSteerDiffLim(const LimitArray & v); void setCurrentSpeed(double v) { current_speed_ = v; } void setParam(const VehicleCmdFilterParam & p); VehicleCmdFilterParam getParam() const; From d3afe89e08598d0825b15815849ee86871b91398 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 11:36:48 +0900 Subject: [PATCH 031/126] fix(autoware_vehicle_cmd_gate): fix uninitMemberVar (#8339) fix:uninitMemberVar Signed-off-by: kobayu858 --- control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 7d2860d6372cf..fc7d6e5436142 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -20,7 +20,7 @@ namespace autoware::vehicle_cmd_gate { -VehicleCmdFilter::VehicleCmdFilter() +VehicleCmdFilter::VehicleCmdFilter() : param_() { } From 65140b314772be6f92ef32f5fceb89e6c3c4b6d1 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 11:37:14 +0900 Subject: [PATCH 032/126] fix(autoware_behavior_velocity_detection_area_module): fix uninitMemberVar (#8332) fix:uninitMemberVar Signed-off-by: kobayu858 --- .../src/scene.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 9cbe962e88cae..d6a1747259962 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -45,7 +45,8 @@ DetectionAreaModule::DetectionAreaModule( lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), - planner_param_(planner_param) + planner_param_(planner_param), + debug_data_() { velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } From f79f9bb2ac0d6af88e855dd8a2f44729e54daa59 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 11:38:54 +0900 Subject: [PATCH 033/126] fix(autoware_behavior_velocity_speed_bump_module): fix uninitMemberVar (#8320) * fix:uninitMemberVar Signed-off-by: kobayu858 * fix:clang format Signed-off-by: kobayu858 * fix:uninitMemberVar Signed-off-by: kobayu858 * fix:clang format Signed-off-by: kobayu858 * fix:clang format Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../src/occlusion_spot_utils.hpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 992cc5787bf58..7bff65d59157d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -163,7 +163,14 @@ struct PossibleCollisionInfo Pose collision_pose; // only use this for debugging Pose intersection_pose; // For egp path and hidden obstacle lanelet::ArcCoordinates arc_lane_dist_at_collision; // For ego distance to obstacle in s-d - PossibleCollisionInfo() = default; + PossibleCollisionInfo() + : obstacle_info(), + collision_with_margin(), + collision_pose(), + intersection_pose(), + arc_lane_dist_at_collision() + { + } PossibleCollisionInfo( const ObstacleInfo & obstacle_info, const PathPoint & collision_with_margin, const Pose & intersection_pose, const lanelet::ArcCoordinates & arc_lane_dist_to_occlusion) From 3e30f323c96e4426331433e15933637152f9afa6 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 11:39:13 +0900 Subject: [PATCH 034/126] fix(autoware_behavior_velocity_speed_bump_module): fix uninitMemberVar (#8319) fix:uninitMemberVar Signed-off-by: kobayu858 --- .../autoware_behavior_velocity_speed_bump_module/src/scene.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index f0064a52891db..dfe5d4de85300 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -35,7 +35,8 @@ SpeedBumpModule::SpeedBumpModule( module_id_(module_id), lane_id_(lane_id), speed_bump_reg_elem_(std::move(speed_bump_reg_elem)), - planner_param_(planner_param) + planner_param_(planner_param), + debug_data_() { // Read speed bump height [m] from map const auto speed_bump_height = From 3efbad0a0cbe2a83f9b3c4f40dde35d59dcdea32 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 11:39:29 +0900 Subject: [PATCH 035/126] fix(autoware_behavior_velocity_stop_line_module): fix uninitMemberVar (#8318) fix:uninitMemberVar Signed-off-by: kobayu858 --- .../autoware_behavior_velocity_stop_line_module/src/scene.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index c5079452f5ada..50b85613ef08f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -33,7 +33,8 @@ StopLineModule::StopLineModule( : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), stop_line_(stop_line), - state_(State::APPROACH) + state_(State::APPROACH), + debug_data_() { velocity_factor_.init(PlanningBehavior::STOP_SIGN); planner_param_ = planner_param; From 330ab8534eb4a2ac4ff7fa5340d5148a9cc851d6 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 11:39:46 +0900 Subject: [PATCH 036/126] fix(autoware_behavior_velocity_traffic_light_module): fix uninitMemberVar (#8317) fix:funinitMemberVar Signed-off-by: kobayu858 --- .../src/scene.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 7770a480f8401..af16cd85741e5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -170,6 +170,7 @@ TrafficLightModule::TrafficLightModule( traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), state_(State::APPROACH), + debug_data_(), is_prev_state_stop_(false) { velocity_factor_.init(PlanningBehavior::TRAFFIC_SIGNAL); From fd48788c343fc61d50dc598a4332d621e29c138f Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 11:40:00 +0900 Subject: [PATCH 037/126] fix(autoware_behavior_velocity_intersection_module): fix functionConst (#8283) fix:functionConst Signed-off-by: kobayu858 --- .../src/object_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp index d98f750dacf13..7f7fe7429b6c9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp @@ -248,7 +248,7 @@ class ObjectInfoManager std::vector> allObjects() const; const std::unordered_map> & - getObjectsMap() + getObjectsMap() const { return objects_info_; } From 1fb846a05a923258e5dedc5578b34b669d0af854 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 11:40:20 +0900 Subject: [PATCH 038/126] fix(autoware_obstacle_stop_planner): fix functionConst (#8282) fix:functionConst Signed-off-by: kobayu858 --- .../src/adaptive_cruise_control.cpp | 2 +- .../src/adaptive_cruise_control.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp index fda53f6b0b6a6..ee2eadac6f780 100644 --- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -605,7 +605,7 @@ double AdaptiveCruiseController::calcBaseDistToForwardObstacle( } double AdaptiveCruiseController::calcTargetVelocity_P( - const double target_dist, const double current_dist) + const double target_dist, const double current_dist) const { const double diff_dist = current_dist - target_dist; double add_vel_p; diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp index 579a0ccc1cedb..f8d3f4ba7d5de 100644 --- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp +++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp @@ -204,7 +204,7 @@ class AdaptiveCruiseController double calcUpperVelocity(const double dist_to_col, const double obj_vel, const double self_vel); double calcThreshDistToForwardObstacle(const double current_vel, const double obj_vel); double calcBaseDistToForwardObstacle(const double current_vel, const double obj_vel); - double calcTargetVelocity_P(const double target_dist, const double current_dist); + double calcTargetVelocity_P(const double target_dist, const double current_dist) const; double calcTargetVelocity_I(const double target_dist, const double current_dist); double calcTargetVelocity_D(const double target_dist, const double current_dist); double calcTargetVelocityByPID( From 396263e09edb720b3437029b5ef078064cd407c3 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:17:15 +0900 Subject: [PATCH 039/126] fix(autoware_motion_velocity_obstacle_velocity_limiter_module): fix uninitMemberVar (#8314) fix:funinitMemberVar Signed-off-by: kobayu858 --- .../src/parameters.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp index c9fe26cc2bb50..86ec8335e6362 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp @@ -46,9 +46,9 @@ struct ObstacleParameters double dynamic_obstacles_buffer{}; double dynamic_obstacles_min_vel{}; std::vector static_map_tags{}; - bool filter_envelope; - bool ignore_on_path; - double ignore_extra_distance; + bool filter_envelope{}; + bool ignore_on_path{}; + double ignore_extra_distance{}; size_t rtree_min_points{}; size_t rtree_min_segments{}; From 75348e5503d924052909f3cafac03bc8f0024be8 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:17:29 +0900 Subject: [PATCH 040/126] fix(diagnostic_graph_aggregator): fix uninitMemberVar (#8313) * fix:funinitMemberVar Signed-off-by: kobayu858 * fix:funinitMemberVar Signed-off-by: kobayu858 * fix:uninitMemberVar Signed-off-by: kobayu858 * fix:clang format Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- system/diagnostic_graph_aggregator/src/common/graph/config.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/system/diagnostic_graph_aggregator/src/common/graph/config.hpp b/system/diagnostic_graph_aggregator/src/common/graph/config.hpp index a22daf907a27e..935b65203afbe 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/config.hpp +++ b/system/diagnostic_graph_aggregator/src/common/graph/config.hpp @@ -18,6 +18,7 @@ #include "data.hpp" #include "types.hpp" +#include #include #include #include @@ -54,7 +55,7 @@ struct UnitConfig std::string path; LinkConfig * item = nullptr; std::vector list; - size_t index; + size_t index = std::numeric_limits::max(); }; struct FileConfig From b43fff8b6b45b9b3d24bc9b51787fe4432797c00 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:17:44 +0900 Subject: [PATCH 041/126] fix(autoware_behavior_velocity_no_stopping_area_module): fix uninitMemberVar (#8321) fix:uninitMemberVar Signed-off-by: kobayu858 --- .../src/scene_no_stopping_area.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 0478aea34a44f..f2bcc0b5abf44 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -48,7 +48,8 @@ NoStoppingAreaModule::NoStoppingAreaModule( : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), - planner_param_(planner_param) + planner_param_(planner_param), + debug_data_() { velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); From 8d7bbe92d3f7854fc6bc5461ba559b15713ad3d6 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:18:22 +0900 Subject: [PATCH 042/126] fix(autoware_behavior_velocity_no_drivable_lane_module): fix uninitMemberVar (#8322) * fix:uninitMemberVar Signed-off-by: kobayu858 * fix:uninitMemberVar Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../src/scene.cpp | 1 + .../src/scene.hpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 465fd770049e9..c3af04bbd1d1f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -31,6 +31,7 @@ NoDrivableLaneModule::NoDrivableLaneModule( : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), planner_param_(planner_param), + debug_data_(), state_(State::INIT) { velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index 1e8e93834f89e..d9e6121a8ae51 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -76,7 +76,7 @@ class NoDrivableLaneModule : public SceneModuleInterface PathWithNoDrivableLanePolygonIntersection path_no_drivable_lane_polygon_intersection; geometry_msgs::msg::Point first_intersection_point; - double distance_ego_first_intersection; + double distance_ego_first_intersection{}; void handle_init_state(); void handle_approaching_state(PathWithLaneId * path, StopReason * stop_reason); From 0a0994c1ed0d622e5608ff48723084f3d78d4fb4 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:18:43 +0900 Subject: [PATCH 043/126] fix(autoware_probabilistic_occupancy_grid_map): fix uninitMemberVar (#8333) fix:uninitMemberVar Signed-off-by: kobayu858 --- .../lib/costmap_2d/occupancy_grid_map_fixed.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index b940b932db0ef..518052a1a4b8c 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -81,9 +81,9 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( : range(_range), wx(_wx), wy(_wy) { } - double range; - double wx; - double wy; + double range{}; + double wx{}; + double wy{}; }; std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); From b1e84ca70e8f2438b52074a8e8bf248ce3e2ffb0 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:21:02 +0900 Subject: [PATCH 044/126] fix(autoware_multi_object_tracker): fix uninitMemberVar (#8335) fix:uninitMemberVar Signed-off-by: kobayu858 --- .../src/processor/input_manager.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index 432a4105d815f..99944731cc503 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -94,7 +94,7 @@ class InputStream std::string input_topic_; std::string long_name_; std::string short_name_; - bool is_spawn_enabled_; + bool is_spawn_enabled_{}; size_t que_size_{30}; std::deque objects_que_; @@ -103,11 +103,11 @@ class InputStream // bool is_time_initialized_{false}; int initial_count_{0}; - double expected_interval_; - double latency_mean_; - double latency_var_; - double interval_mean_; - double interval_var_; + double expected_interval_{}; + double latency_mean_{}; + double latency_var_{}; + double interval_mean_{}; + double interval_var_{}; rclcpp::Time latest_measurement_time_; rclcpp::Time latest_message_time_; @@ -136,7 +136,7 @@ class InputManager bool is_initialized_{false}; rclcpp::Time latest_exported_object_time_; - size_t input_size_; + size_t input_size_{}; std::vector> input_streams_; std::function func_trigger_; From 3d17d84d080a77e964658e6b7e820f497fc9c067 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:41:58 +0900 Subject: [PATCH 045/126] fix(autoware_traffic_light_visualization): fix passedByValue (#8241) fix:passedByValue Signed-off-by: kobayu858 --- .../src/traffic_light_map_visualizer/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index 90b73e643ae8e..ccfaf37fd7c6d 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -38,7 +38,7 @@ namespace } bool isAttributeValue( - const lanelet::ConstPoint3d p, const std::string attr_str, const std::string value_str) + const lanelet::ConstPoint3d p, const std::string & attr_str, const std::string & value_str) { lanelet::Attribute attr = p.attribute(attr_str); if (attr.value().compare(value_str) == 0) { @@ -49,7 +49,7 @@ bool isAttributeValue( void lightAsMarker( const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, - lanelet::ConstPoint3d p, visualization_msgs::msg::Marker * marker, const std::string ns, + lanelet::ConstPoint3d p, visualization_msgs::msg::Marker * marker, const std::string & ns, const rclcpp::Time & current_time) { if (marker == nullptr) { From 983b1331b8befea8e92fc61540cbbbeb8464d8b4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 5 Aug 2024 17:17:51 +0900 Subject: [PATCH 046/126] fix(autoware_multi_object_tracker): revert latency reduction logic and bring back to timer trigger (#8277) * fix: revert latency reduction logic and bring back to timer trigger Signed-off-by: Taekjin LEE * style(pre-commit): autofix * chore: remove unused variables Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/multi_object_tracker_node.cpp | 26 +------------------ .../src/multi_object_tracker_node.hpp | 1 - 2 files changed, 1 insertion(+), 26 deletions(-) diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 45955ac983dde..89d8cbaeb3bd8 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -153,9 +153,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // If the delay compensation is enabled, the timer is used to publish the output at the correct // time. if (enable_delay_compensation) { - publisher_period_ = 1.0 / publish_rate; // [s] - constexpr double timer_multiplier = 20.0; // 20 times frequent for publish timing check - const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period(); + const auto timer_period = rclcpp::Rate(publish_rate).period(); publish_timer_ = rclcpp::create_timer( this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); } @@ -210,35 +208,13 @@ void MultiObjectTracker::onTrigger() ObjectsList objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (!is_objects_ready) return; - onMessage(objects_list); - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); - - // Publish objects if the timer is not enabled - if (publish_timer_ == nullptr) { - // if the delay compensation is disabled, publish the objects in the latest time - publish(latest_time); - } else { - // Publish if the next publish time is close - const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period - const rclcpp::Time publish_time = this->now(); - if ((publish_time - last_published_time_).seconds() > minimum_publish_interval) { - checkAndPublish(publish_time); - } - } } void MultiObjectTracker::onTimer() { const rclcpp::Time current_time = this->now(); - // Check the publish period - const auto elapsed_time = (current_time - last_published_time_).seconds(); - // If the elapsed time is over the period, publish objects with prediction - constexpr double maximum_latency_ratio = 1.11; // 11% margin - const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio; - if (elapsed_time < maximum_publish_latency) return; - // get objects from the input manager and run process ObjectsList objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index db5eaaa88ca8c..e917acbda9fcc 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -78,7 +78,6 @@ class MultiObjectTracker : public rclcpp::Node // publish timer rclcpp::TimerBase::SharedPtr publish_timer_; rclcpp::Time last_published_time_; - double publisher_period_; // internal states std::string world_frame_id_; // tracking frame From 6aefc8a20c2a31f7af56b7043b8bd9a922f74d6d Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 5 Aug 2024 18:48:29 +0900 Subject: [PATCH 047/126] ci: use self-hosted machine for build-and-test-differential-cuda (#8327) Signed-off-by: Ryohsuke Mitsudome --- .github/workflows/build-and-test-daily.yaml | 2 +- .github/workflows/build-and-test-differential.yaml | 6 +++++- .github/workflows/build-and-test.yaml | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-and-test-daily.yaml b/.github/workflows/build-and-test-daily.yaml index 0b21ba56640a7..66275444482b5 100644 --- a/.github/workflows/build-and-test-daily.yaml +++ b/.github/workflows/build-and-test-daily.yaml @@ -7,7 +7,7 @@ on: jobs: build-and-test-daily: - runs-on: [self-hosted, linux, X64] + runs-on: [self-hosted, linux, X64, gpu] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 8d2158f3b911c..d41e6cf481b76 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -21,7 +21,7 @@ jobs: build-and-test-differential: needs: make-sure-label-is-present if: ${{ needs.make-sure-label-is-present.outputs.result == 'true' }} - runs-on: ubuntu-latest + runs-on: ${{ matrix.runner }} container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false @@ -35,6 +35,10 @@ jobs: - rosdistro: humble container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe build-depends-repos: build_depends.repos + - container-suffix: -cuda + runner: [self-hosted, linux, X64, cpu] + - container-suffix: "" + runner: ubuntu-latest steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 85dc6c2fb89bf..716d6b8df5fbc 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -12,7 +12,7 @@ env: jobs: build-and-test: - runs-on: [self-hosted, linux, X64] + runs-on: [self-hosted, linux, X64, gpu] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false From 50a699d1fc82ecd6eb8c0f31d9c9e79d93d6d835 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 5 Aug 2024 19:02:05 +0900 Subject: [PATCH 048/126] fix(start/goal_planner): align geometric parall parking start pose with center line (#8326) Signed-off-by: kosuke55 --- .../geometric_parallel_parking.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 7afce28019c3f..dc0f13270bca2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -17,9 +17,11 @@ #include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +#include #include #include @@ -333,8 +335,20 @@ std::optional GeometricParallelParking::calcStartPose( } const double dx_sign = is_forward ? -1 : 1; const double dx = 2 * std::sqrt(squared_distance_to_arc_connect) * dx_sign; - const Pose start_pose = - calcOffsetPose(goal_pose, dx + start_pose_offset, -arc_coordinates.distance, 0); + + // Assuming parallel poses, calculate the approximate start pose on the centerline from the goal + // pose + const Pose approximate_start_pose = calcOffsetPose(goal_pose, dx, -arc_coordinates.distance, 0); + lanelet::ConstLanelet closest_road_lane{}; + + // Calculate start pose on the centerline, then offset it. + lanelet::utils::query::getClosestLanelet(road_lanes, approximate_start_pose, &closest_road_lane); + const Pose start_pose_no_offset = + lanelet::utils::getClosestCenterPose(closest_road_lane, approximate_start_pose.position); + const auto road_lane_path = planner_data_->route_handler->getCenterLinePath( + road_lanes, 0.0, std::numeric_limits::max()); + const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( + road_lane_path.points, start_pose_no_offset.position, start_pose_offset); return start_pose; } From 9dba70882c35dd5c92b1d1815750e8d05929eed4 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 19:15:52 +0900 Subject: [PATCH 049/126] fix(autoware_ground_segmentation): fix uninitMemberVar (#8336) fix:uninitMemberVar Signed-off-by: kobayu858 --- .../src/scan_ground_filter/node.hpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index f9a888ad9dfcf..94ef16042ff33 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -91,7 +91,14 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt std::vector height_list; PointsCentroid() - : radius_sum(0.0f), height_sum(0.0f), radius_avg(0.0f), height_avg(0.0f), point_num(0) + : radius_sum(0.0f), + height_sum(0.0f), + radius_avg(0.0f), + height_avg(0.0f), + height_max(0.0f), + height_min(10.0f), + point_num(0), + grid_id(0) { } From e01f8538c94366132998da3e5a2ae8a0bdad22cb Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 5 Aug 2024 19:16:23 +0900 Subject: [PATCH 050/126] fix(autoware_compare_map_segmentation): fix uninitMemberVar (#8338) fix:uninitMemberVar Signed-off-by: kobayu858 --- .../src/voxel_grid_map_loader/voxel_grid_map_loader.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index 07ed0aa7b92db..107729088d5dc 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp @@ -97,7 +97,7 @@ class VoxelGridMapLoader rclcpp::Logger logger_; std::mutex * mutex_ptr_; double voxel_leaf_size_; - double voxel_leaf_size_z_; + double voxel_leaf_size_z_{}; double downsize_ratio_z_axis_; rclcpp::Publisher::SharedPtr downsampled_map_pub_; bool debug_ = false; From 3e79b8f67f5a3428c04efb7545c57b6401c83d17 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 5 Aug 2024 20:04:21 +0900 Subject: [PATCH 051/126] fix(freespace_planner): disable randomly failing tests (#8337) Signed-off-by: Maxime CLEMENT --- .../test/test_freespace_planner_node_interface.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index 7b4c8e3856b38..f43b4401865b2 100644 --- a/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -60,7 +60,8 @@ void publishMandatoryTopics( test_manager->publishParkingScenario(test_target_node, "freespace_planner/input/scenario"); } -TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput) +// the following tests are disable because they randomly fail +TEST(PlanningModuleInterfaceTest, DISABLED_testPlanningInterfaceWithVariousTrajectoryInput) { rclcpp::init(0, nullptr); @@ -77,7 +78,7 @@ TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInpu rclcpp::shutdown(); } -TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +TEST(PlanningModuleInterfaceTest, DISABLED_NodeTestWithOffTrackEgoPose) { rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); From 26016426322c0064d1e12fcdd0b94ed5cf02b6cf Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Mon, 5 Aug 2024 22:32:04 +0900 Subject: [PATCH 052/126] perf(autoware_pointcloud_preprocessor): lazy & managed TF listeners (#8174) * perf(autoware_pointcloud_preprocessor): lazy & managed TF listeners Signed-off-by: amadeuszsz * fix(autoware_pointcloud_preprocessor): param names & reverse frames transform logic Signed-off-by: amadeuszsz * fix(autoware_ground_segmentation): add missing TF listener Signed-off-by: amadeuszsz * feat(autoware_ground_segmentation): change to static TF buffer Signed-off-by: amadeuszsz * refactor(autoware_pointcloud_preprocessor): move StaticTransformListener to universe utils Signed-off-by: amadeuszsz * perf(autoware_universe_utils): skip redundant transform Signed-off-by: amadeuszsz * fix(autoware_universe_utils): change checks order Signed-off-by: amadeuszsz * doc(autoware_universe_utils): add docstring Signed-off-by: amadeuszsz --------- Signed-off-by: amadeuszsz --- .../ros/static_transform_buffer.hpp | 165 ++++++++++++++ common/autoware_universe_utils/package.xml | 1 + .../src/ros/test_static_transform_buffer.cpp | 210 ++++++++++++++++++ .../src/ransac_ground_filter/node.cpp | 18 +- .../src/ransac_ground_filter/node.hpp | 4 + .../concatenate_and_time_sync_nodelet.hpp | 6 +- .../concatenate_pointclouds.hpp | 12 +- .../distortion_corrector.hpp | 10 +- .../pointcloud_preprocessor/filter.hpp | 8 +- .../time_synchronizer_nodelet.hpp | 12 +- .../vector_map_inside_area_filter.hpp | 5 + .../concatenate_and_time_sync_nodelet.cpp | 31 +-- .../concatenate_pointclouds.cpp | 30 +-- .../distortion_corrector.cpp | 92 +++----- .../src/filter.cpp | 76 +------ .../time_synchronizer_nodelet.cpp | 36 +-- .../vector_map_inside_area_filter.cpp | 10 + 17 files changed, 467 insertions(+), 259 deletions(-) create mode 100644 common/autoware_universe_utils/include/autoware/universe_utils/ros/static_transform_buffer.hpp create mode 100644 common/autoware_universe_utils/test/src/ros/test_static_transform_buffer.cpp diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/static_transform_buffer.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/static_transform_buffer.hpp new file mode 100644 index 0000000000000..175d1934e973d --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/static_transform_buffer.hpp @@ -0,0 +1,165 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_ + +#include "autoware/universe_utils/ros/transform_listener.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace std +{ +template <> +struct hash> +{ + size_t operator()(const std::pair & p) const + { + size_t h1 = std::hash{}(p.first); + size_t h2 = std::hash{}(p.second); + return h1 ^ (h2 << 1u); + } +}; +} // namespace std + +namespace autoware::universe_utils +{ +using std::chrono_literals::operator""ms; +using Key = std::pair; +struct PairEqual +{ + bool operator()(const Key & p1, const Key & p2) const + { + return p1.first == p2.first && p1.second == p2.second; + } +}; +using TFMap = std::unordered_map, PairEqual>; + +class StaticTransformBuffer +{ +public: + StaticTransformBuffer() = default; + + /** + * @brief Retrieves a transform between two static frames. + * + * This function attempts to retrieve a transform between the target frame and the source frame. + * If success, the transform matrix is set to the output parameter and the function returns true. + * Otherwise, transform matrix is set to identity and the function returns false. Transform + * Listener is destroyed after function call. + * + * @param node A pointer to the ROS node. + * @param target_frame The target frame. + * @param source_frame The source frame. + * @param eigen_transform The output Eigen transform matrix. Is set to identity if the transform + * is not found. + * @return True if the transform was successfully retrieved, false otherwise. + */ + bool getTransform( + rclcpp::Node * node, const std::string & target_frame, const std::string & source_frame, + Eigen::Matrix4f & eigen_transform) + { + auto key = std::make_pair(target_frame, source_frame); + auto key_inv = std::make_pair(source_frame, target_frame); + + // Check if the transform is already in the buffer + auto it = buffer_.find(key); + if (it != buffer_.end()) { + eigen_transform = it->second; + return true; + } + + // Check if the inverse transform is already in the buffer + auto it_inv = buffer_.find(key_inv); + if (it_inv != buffer_.end()) { + eigen_transform = it_inv->second.inverse(); + buffer_[key] = eigen_transform; + return true; + } + + // Check if transform is needed + if (target_frame == source_frame) { + eigen_transform = Eigen::Matrix4f::Identity(); + buffer_[key] = eigen_transform; + return true; + } + + // Get the transform from the TF tree + auto tf_listener = std::make_unique(node); + auto tf = tf_listener->getTransform( + target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration(1000ms)); + RCLCPP_DEBUG( + node->get_logger(), "Trying to enqueue %s -> %s transform to static TFs buffer...", + target_frame.c_str(), source_frame.c_str()); + if (tf == nullptr) { + eigen_transform = Eigen::Matrix4f::Identity(); + return false; + } + pcl_ros::transformAsMatrix(*tf, eigen_transform); + buffer_[key] = eigen_transform; + return true; + } + + /** + * Transforms a point cloud from one frame to another. + * + * @param node A pointer to the ROS node. + * @param target_frame The target frame to transform the point cloud to. + * @param cloud_in The input point cloud to be transformed. + * @param cloud_out The transformed point cloud. + * @return True if the transformation is successful, false otherwise. + */ + bool transformPointcloud( + rclcpp::Node * node, const std::string & target_frame, + const sensor_msgs::msg::PointCloud2 & cloud_in, sensor_msgs::msg::PointCloud2 & cloud_out) + { + if ( + pcl::getFieldIndex(cloud_in, "x") == -1 || pcl::getFieldIndex(cloud_in, "y") == -1 || + pcl::getFieldIndex(cloud_in, "z") == -1) { + RCLCPP_ERROR(node->get_logger(), "Input pointcloud does not have xyz fields"); + return false; + } + if (target_frame == cloud_in.header.frame_id) { + cloud_out = cloud_in; + return true; + } + Eigen::Matrix4f eigen_transform; + if (!getTransform(node, target_frame, cloud_in.header.frame_id, eigen_transform)) { + return false; + } + pcl_ros::transformPointCloud(eigen_transform, cloud_in, cloud_out); + cloud_out.header.frame_id = target_frame; + return true; + } + +private: + TFMap buffer_; +}; + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_ diff --git a/common/autoware_universe_utils/package.xml b/common/autoware_universe_utils/package.xml index de0461b5a841d..b4809e720b8d4 100644 --- a/common/autoware_universe_utils/package.xml +++ b/common/autoware_universe_utils/package.xml @@ -24,6 +24,7 @@ pcl_ros rclcpp tf2 + tf2_eigen tf2_geometry_msgs tier4_debug_msgs tier4_planning_msgs diff --git a/common/autoware_universe_utils/test/src/ros/test_static_transform_buffer.cpp b/common/autoware_universe_utils/test/src/ros/test_static_transform_buffer.cpp new file mode 100644 index 0000000000000..5200378e3bf52 --- /dev/null +++ b/common/autoware_universe_utils/test/src/ros/test_static_transform_buffer.cpp @@ -0,0 +1,210 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/universe_utils/ros/static_transform_buffer.hpp" + +#include +#include + +#include +#include + +#include + +class TestStaticTransformBuffer : public ::testing::Test +{ +protected: + std::shared_ptr node_{nullptr}; + std::shared_ptr static_tf_buffer_{nullptr}; + std::shared_ptr tf_broadcaster_; + geometry_msgs::msg::TransformStamped tf_base_to_lidar_; + Eigen::Matrix4f eigen_base_to_lidar_; + std::unique_ptr cloud_in_; + + geometry_msgs::msg::TransformStamped generateTransformMsg( + const int32_t seconds, const uint32_t nanoseconds, const std::string & parent_frame, + const std::string & child_frame, double x, double y, double z, double qx, double qy, double qz, + double qw) + { + rclcpp::Time timestamp(seconds, nanoseconds, RCL_ROS_TIME); + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = timestamp; + tf_msg.header.frame_id = parent_frame; + tf_msg.child_frame_id = child_frame; + tf_msg.transform.translation.x = x; + tf_msg.transform.translation.y = y; + tf_msg.transform.translation.z = z; + tf_msg.transform.rotation.x = qx; + tf_msg.transform.rotation.y = qy; + tf_msg.transform.rotation.z = qz; + tf_msg.transform.rotation.w = qw; + return tf_msg; + } + + void SetUp() override + { + node_ = std::make_shared("test_static_transform_buffer"); + static_tf_buffer_ = std::make_shared(); + tf_broadcaster_ = std::make_shared(node_); + + tf_base_to_lidar_ = generateTransformMsg( + 10, 100'000'000, "base_link", "lidar_top", 0.690, 0.000, 2.100, -0.007, -0.007, 0.692, 0.722); + eigen_base_to_lidar_ = tf2::transformToEigen(tf_base_to_lidar_).matrix().cast(); + tf_broadcaster_->sendTransform(tf_base_to_lidar_); + cloud_in_ = std::make_unique(); + + // Set up the fields for x, y, and z coordinates + cloud_in_->fields.resize(3); + sensor_msgs::PointCloud2Modifier modifier(*cloud_in_); + modifier.setPointCloud2FieldsByString(1, "xyz"); + + // Resize the cloud to hold points_per_pointcloud_ points + modifier.resize(10); + + // Create an iterator for the x, y, z fields + sensor_msgs::PointCloud2Iterator iter_x(*cloud_in_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud_in_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud_in_, "z"); + + // Populate the point cloud + for (size_t i = 0; i < modifier.size(); ++i, ++iter_x, ++iter_y, ++iter_z) { + *iter_x = static_cast(i); + *iter_y = static_cast(i); + *iter_z = static_cast(i); + } + + // Set up cloud header + cloud_in_->header.frame_id = "lidar_top"; + cloud_in_->header.stamp = rclcpp::Time(10, 100'000'000); + + ASSERT_TRUE(rclcpp::ok()); + } + + void TearDown() override { static_tf_buffer_.reset(); } +}; + +TEST_F(TestStaticTransformBuffer, TestTransformNoExist) +{ + Eigen::Matrix4f transform; + auto success = static_tf_buffer_->getTransform(node_.get(), "base_link", "fake_link", transform); + EXPECT_TRUE(transform.isIdentity()); + EXPECT_FALSE(success); +} + +TEST_F(TestStaticTransformBuffer, TestTransformBase) +{ + Eigen::Matrix4f eigen_base_to_lidar; + auto success = + static_tf_buffer_->getTransform(node_.get(), "base_link", "lidar_top", eigen_base_to_lidar); + EXPECT_TRUE(eigen_base_to_lidar.isApprox(eigen_base_to_lidar_, 0.001)); + EXPECT_TRUE(success); +} + +TEST_F(TestStaticTransformBuffer, TestTransformSameFrame) +{ + Eigen::Matrix4f eigen_base_to_base; + auto success = + static_tf_buffer_->getTransform(node_.get(), "base_link", "base_link", eigen_base_to_base); + EXPECT_TRUE(eigen_base_to_base.isApprox(Eigen::Matrix4f::Identity(), 0.001)); + EXPECT_TRUE(success); +} + +TEST_F(TestStaticTransformBuffer, TestTransformInverse) +{ + Eigen::Matrix4f eigen_lidar_to_base; + auto success = + static_tf_buffer_->getTransform(node_.get(), "lidar_top", "base_link", eigen_lidar_to_base); + EXPECT_TRUE(eigen_lidar_to_base.isApprox(eigen_base_to_lidar_.inverse(), 0.001)); + EXPECT_TRUE(success); +} + +TEST_F(TestStaticTransformBuffer, TestTransformMultipleCall) +{ + Eigen::Matrix4f eigen_transform; + EXPECT_FALSE( + static_tf_buffer_->getTransform(node_.get(), "base_link", "fake_link", eigen_transform)); + EXPECT_TRUE(eigen_transform.isApprox(Eigen::Matrix4f::Identity(), 0.001)); + EXPECT_TRUE( + static_tf_buffer_->getTransform(node_.get(), "lidar_top", "base_link", eigen_transform)); + EXPECT_TRUE(eigen_transform.isApprox(eigen_base_to_lidar_.inverse(), 0.001)); + EXPECT_TRUE( + static_tf_buffer_->getTransform(node_.get(), "fake_link", "fake_link", eigen_transform)); + EXPECT_TRUE(eigen_transform.isApprox(Eigen::Matrix4f::Identity(), 0.001)); + EXPECT_TRUE( + static_tf_buffer_->getTransform(node_.get(), "base_link", "lidar_top", eigen_transform)); + EXPECT_TRUE(eigen_transform.isApprox(eigen_base_to_lidar_, 0.001)); + EXPECT_FALSE( + static_tf_buffer_->getTransform(node_.get(), "fake_link", "lidar_top", eigen_transform)); + EXPECT_TRUE(eigen_transform.isApprox(Eigen::Matrix4f::Identity(), 0.001)); + EXPECT_TRUE( + static_tf_buffer_->getTransform(node_.get(), "base_link", "lidar_top", eigen_transform)); + EXPECT_TRUE(eigen_transform.isApprox(eigen_base_to_lidar_, 0.001)); +} + +TEST_F(TestStaticTransformBuffer, TestTransformEmptyPointCloud) +{ + auto cloud_in = std::make_unique(); + cloud_in->header.frame_id = "lidar_top"; + cloud_in->header.stamp = rclcpp::Time(10, 100'000'000); + auto cloud_out = std::make_unique(); + + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in, *cloud_out)); + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in, *cloud_out)); + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in, *cloud_out)); +} + +TEST_F(TestStaticTransformBuffer, TestTransformEmptyPointCloudNoHeader) +{ + auto cloud_in = std::make_unique(); + auto cloud_out = std::make_unique(); + + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in, *cloud_out)); + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in, *cloud_out)); + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in, *cloud_out)); +} + +TEST_F(TestStaticTransformBuffer, TestTransformPointCloud) +{ + auto cloud_out = std::make_unique(); + + // Transform cloud with header + EXPECT_TRUE( + static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in_, *cloud_out)); + EXPECT_TRUE( + static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in_, *cloud_out)); + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in_, *cloud_out)); +} + +TEST_F(TestStaticTransformBuffer, TestTransformPointCloudNoHeader) +{ + auto cloud_out = std::make_unique(); + + // Transform cloud without header + auto cloud_in = std::make_unique(*cloud_in_); + cloud_in->header.frame_id = ""; + cloud_in->header.stamp = rclcpp::Time(0, 0); + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "lidar_top", *cloud_in, *cloud_out)); + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "base_link", *cloud_in, *cloud_out)); + EXPECT_FALSE( + static_tf_buffer_->transformPointcloud(node_.get(), "fake_link", *cloud_in, *cloud_out)); +} diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp index dd5ce96a858cd..4d45f5df07027 100644 --- a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp @@ -115,6 +115,8 @@ RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptio std::bind(&RANSACGroundFilterComponent::paramCallback, this, _1)); pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); + + static_tf_buffer_ = std::make_unique(); } void RANSACGroundFilterComponent::setDebugPublisher() @@ -161,20 +163,8 @@ bool RANSACGroundFilterComponent::transformPointCloud( return true; } - geometry_msgs::msg::TransformStamped transform_stamped; - try { - transform_stamped = tf_buffer_->lookupTransform( - in_target_frame, in_cloud_ptr->header.frame_id, in_cloud_ptr->header.stamp, - rclcpp::Duration::from_seconds(1.0)); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - return false; - } - // tf2::doTransform(*in_cloud_ptr, *out_cloud_ptr, transform_stamped); - Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - pcl_ros::transformPointCloud(mat, *in_cloud_ptr, *out_cloud_ptr); - out_cloud_ptr->header.frame_id = in_target_frame; - return true; + return static_tf_buffer_->transformPointcloud( + this, in_target_frame, *in_cloud_ptr, *out_cloud_ptr); } void RANSACGroundFilterComponent::extractPointsIndices( diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp index e173fe131602f..bdc217e80650c 100644 --- a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp @@ -17,6 +17,8 @@ #include "autoware/pointcloud_preprocessor/filter.hpp" +#include + #include #include @@ -35,6 +37,7 @@ #include #include +#include #include #include @@ -80,6 +83,7 @@ class RANSACGroundFilterComponent : public autoware::pointcloud_preprocessor::Fi bool debug_ = false; bool is_initialized_debug_message_ = false; Eigen::Vector3d unit_vec_ = Eigen::Vector3d::UnitZ(); + std::unique_ptr static_tf_buffer_{nullptr}; /*! * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index 2443e45fe4c63..549a60e49d748 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -64,6 +64,7 @@ #include "autoware_point_types/types.hpp" #include +#include #include #include #include @@ -149,9 +150,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node // XmlRpc::XmlRpcValue input_topics_; std::vector input_topics_; - /** \brief TF listener object. */ - std::shared_ptr tf2_buffer_; - std::shared_ptr tf2_listener_; + std::unique_ptr static_tf_buffer_{nullptr}; std::deque twist_ptr_queue_; @@ -162,7 +161,6 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::vector input_offset_; std::map offset_map_; - void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); Eigen::Matrix4f computeTransformToAdjustForOldTimestamp( const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); std::map combineClouds( diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index 36b1910a4d798..b982553a8c920 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -61,10 +61,10 @@ #include // ROS includes -#include "autoware_point_types/types.hpp" - #include +#include #include +#include #include #include @@ -139,9 +139,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node // XmlRpc::XmlRpcValue input_topics_; std::vector input_topics_; - /** \brief TF listener object. */ - std::shared_ptr tf2_buffer_; - std::shared_ptr tf2_listener_; + std::unique_ptr static_tf_buffer_{nullptr}; std::deque twist_ptr_queue_; @@ -152,10 +150,6 @@ class PointCloudConcatenationComponent : public rclcpp::Node std::vector input_offset_; std::map offset_map_; - void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); - void transformPointCloud( - const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, - const std::string & target_frame); void checkSyncStatus(); void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index 8226cb6eb774c..7ba419770af17 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__POINTCLOUD_PREPROCESSOR__DISTORTION_CORRECTOR__DISTORTION_CORRECTOR_HPP_ #include +#include #include #include @@ -25,6 +26,7 @@ #include #include +#include #include #include @@ -72,8 +74,7 @@ class DistortionCorrector : public DistortionCorrectorBase bool pointcloud_transform_exists_{false}; bool imu_transform_exists_{false}; rclcpp::Node * node_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; + std::unique_ptr static_tf_buffer_{nullptr}; std::deque twist_queue_; std::deque angular_velocity_queue_; @@ -95,11 +96,12 @@ class DistortionCorrector : public DistortionCorrectorBase static_cast(this)->undistortPointImplementation( it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid); }; + void convertMatrixToTransform(const Eigen::Matrix4f & matrix, tf2::Transform & transform); public: - explicit DistortionCorrector(rclcpp::Node * node) - : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) + explicit DistortionCorrector(rclcpp::Node * node) : node_(node) { + static_tf_buffer_ = std::make_unique(); } bool pointcloud_transform_exists(); bool pointcloud_transform_needed(); diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp index a6e113412231a..789f377e7ec95 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp @@ -82,6 +82,7 @@ // Include tier4 autoware utils #include #include +#include #include namespace autoware::pointcloud_preprocessor @@ -239,8 +240,7 @@ class Filter : public rclcpp::Node * versus an exact one (false by default). */ bool approximate_sync_ = false; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; + std::unique_ptr static_tf_buffer_{nullptr}; inline bool isValid( const PointCloud2ConstPtr & cloud, const std::string & /*topic_name*/ = "input") @@ -289,10 +289,6 @@ class Filter : public rclcpp::Node const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from, TransformInfo & transform_info /*output*/); - bool _calculate_transform_matrix( - const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from, - const tf2_ros::Buffer & tf_buffer, Eigen::Matrix4f & eigen_transform /*output*/); - bool convert_output_costly(std::unique_ptr & output); // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 00704b20b85ef..4f08ae3a3ce67 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -61,10 +61,10 @@ #include // ROS includes -#include "autoware_point_types/types.hpp" - #include +#include #include +#include #include #include @@ -144,9 +144,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node // XmlRpc::XmlRpcValue input_topics_; std::vector input_topics_; - /** \brief TF listener object. */ - std::shared_ptr tf2_buffer_; - std::shared_ptr tf2_listener_; + std::unique_ptr static_tf_buffer_{nullptr}; std::deque twist_ptr_queue_; @@ -157,10 +155,6 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node std::vector input_offset_; std::map offset_map_; - void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); - void transformPointCloud( - const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, - const std::string & target_frame); Eigen::Matrix4f computeTransformToAdjustForOldTimestamp( const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); std::map synchronizeClouds(); diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index a6b497b40cd77..fef3c8da79714 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -24,6 +24,7 @@ #include +#include #include using autoware::universe_utils::MultiPoint2d; @@ -46,6 +47,10 @@ class VectorMapInsideAreaFilterComponent : public autoware::pointcloud_preproces bool use_z_filter_ = false; float z_threshold_; + // tf2 listener + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + public: PCL_MAKE_ALIGNED_OPERATOR_NEW explicit VectorMapInsideAreaFilterComponent(const rclcpp::NodeOptions & options); diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 95c353212484c..2410a9dccebea 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -137,8 +137,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // tf2 listener { - tf2_buffer_ = std::make_shared(this->get_clock()); - tf2_listener_ = std::make_shared(*tf2_buffer_); + static_tf_buffer_ = std::make_unique(); } // Output Publishers @@ -224,25 +223,6 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro } } -////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenateDataSynchronizerComponent::transformPointCloud( - const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out) -{ - // Transform the point clouds into the specified output frame - if (output_frame_ != in->header.frame_id) { - // TODO(YamatoAndo): use TF2 - if (!pcl_ros::transformPointCloud(output_frame_, *in, *out, *tf2_buffer_)) { - RCLCPP_ERROR( - this->get_logger(), - "[transformPointCloud] Error converting first input dataset from %s to %s.", - in->header.frame_id.c_str(), output_frame_.c_str()); - return; - } - } else { - out = std::make_shared(*in); - } -} - std::string PointCloudConcatenateDataSynchronizerComponent::replaceSyncTopicNamePostfix( const std::string & original_topic_name, const std::string & postfix) { @@ -381,7 +361,8 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( } sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( new sensor_msgs::msg::PointCloud2()); - transformPointCloud(e.second, transformed_cloud_ptr); + static_tf_buffer_->transformPointcloud( + this, output_frame_, *e.second, *transformed_cloud_ptr); // calculate transforms to oldest stamp Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); @@ -411,9 +392,9 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) { sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr_in_sensor_frame( new sensor_msgs::msg::PointCloud2()); - pcl_ros::transformPointCloud( - (std::string)e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr, - *transformed_cloud_ptr_in_sensor_frame, *tf2_buffer_); + static_tf_buffer_->transformPointcloud( + this, e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr, + *transformed_cloud_ptr_in_sensor_frame); transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp; transformed_cloud_ptr_in_sensor_frame->header.frame_id = e.second->header.frame_id; transformed_clouds[e.first] = transformed_cloud_ptr_in_sensor_frame; diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index 24c4b572181fe..2c5d7f24cb80c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -93,8 +93,7 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( // tf2 listener { - tf2_buffer_ = std::make_shared(this->get_clock()); - tf2_listener_ = std::make_shared(*tf2_buffer_); + static_tf_buffer_ = std::make_unique(); } // Output Publishers @@ -149,30 +148,6 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( } ////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenationComponent::transformPointCloud( - const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out) -{ - transformPointCloud(in, out, output_frame_); -} - -void PointCloudConcatenationComponent::transformPointCloud( - const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, - const std::string & target_frame) -{ - // Transform the point clouds into the specified output frame - if (target_frame != in->header.frame_id) { - // TODO(YamatoAndo): use TF2 - if (!pcl_ros::transformPointCloud(target_frame, *in, *out, *tf2_buffer_)) { - RCLCPP_ERROR( - this->get_logger(), - "[transformPointCloud] Error converting first input dataset from %s to %s.", - in->header.frame_id.c_str(), target_frame.c_str()); - return; - } - } else { - out = std::make_shared(*in); - } -} void PointCloudConcatenationComponent::checkSyncStatus() { @@ -262,7 +237,8 @@ void PointCloudConcatenationComponent::combineClouds( // transform to output frame sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( new sensor_msgs::msg::PointCloud2()); - transformPointCloud(e.second, transformed_cloud_ptr); + static_tf_buffer_->transformPointcloud( + this, output_frame_, *e.second, *transformed_cloud_ptr); // concatenate if (concat_cloud_ptr == nullptr) { diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 652edd9126358..381c90dda7de5 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -19,6 +19,8 @@ #include #include +#include + namespace autoware::pointcloud_preprocessor { @@ -85,29 +87,13 @@ void DistortionCorrector::getIMUTransformation( return; } - geometry_imu_to_base_link_ptr_ = std::make_shared(); - tf2::Transform tf2_imu_to_base_link; - if (base_frame == imu_frame) { - tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - imu_transform_exists_ = true; - } else { - try { - const auto transform_msg = - tf_buffer_.lookupTransform(base_frame, imu_frame, tf2::TimePointZero); - tf2::convert(transform_msg.transform, tf2_imu_to_base_link); - imu_transform_exists_ = true; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); - RCLCPP_ERROR( - node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), imu_frame.c_str()); - - tf2_imu_to_base_link.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_imu_to_base_link.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - } - } + Eigen::Matrix4f eigen_imu_to_base_link; + imu_transform_exists_ = + static_tf_buffer_->getTransform(node_, base_frame, imu_frame, eigen_imu_to_base_link); + convertMatrixToTransform(eigen_imu_to_base_link, tf2_imu_to_base_link); + geometry_imu_to_base_link_ptr_ = std::make_shared(); geometry_imu_to_base_link_ptr_->transform.rotation = tf2::toMsg(tf2_imu_to_base_link.getRotation()); } @@ -295,6 +281,16 @@ void DistortionCorrector::warnIfTimestampIsTooLate( } } +template +void DistortionCorrector::convertMatrixToTransform( + const Eigen::Matrix4f & matrix, tf2::Transform & transform) +{ + transform.setOrigin(tf2::Vector3(matrix(0, 3), matrix(1, 3), matrix(2, 3))); + transform.setBasis(tf2::Matrix3x3( + matrix(0, 0), matrix(0, 1), matrix(0, 2), matrix(1, 0), matrix(1, 1), matrix(1, 2), + matrix(2, 0), matrix(2, 1), matrix(2, 2))); +} + ///////////////////////// Functions for different undistortion strategies ///////////////////////// void DistortionCorrector2D::initialize() @@ -316,29 +312,12 @@ void DistortionCorrector2D::setPointCloudTransform( return; } - if (base_frame == lidar_frame) { - tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; - pointcloud_transform_exists_ = true; - } else { - try { - const auto transform_msg = - tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero); - tf2::convert(transform_msg.transform, tf2_lidar_to_base_link_); - tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); - pointcloud_transform_exists_ = true; - pointcloud_transform_needed_ = true; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); - RCLCPP_ERROR( - node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str()); - - tf2_lidar_to_base_link_.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); - tf2_lidar_to_base_link_.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0)); - tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_; - } - } + Eigen::Matrix4f eigen_lidar_to_base_link; + pointcloud_transform_exists_ = + static_tf_buffer_->getTransform(node_, base_frame, lidar_frame, eigen_lidar_to_base_link); + convertMatrixToTransform(eigen_lidar_to_base_link, tf2_lidar_to_base_link_); + tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse(); + pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_; } void DistortionCorrector3D::setPointCloudTransform( @@ -348,27 +327,10 @@ void DistortionCorrector3D::setPointCloudTransform( return; } - if (base_frame == lidar_frame) { - eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); - eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); - pointcloud_transform_exists_ = true; - } - - try { - const auto transform_msg = - tf_buffer_.lookupTransform(base_frame, lidar_frame, tf2::TimePointZero); - eigen_lidar_to_base_link_ = - tf2::transformToEigen(transform_msg.transform).matrix().cast(); - eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); - pointcloud_transform_exists_ = true; - pointcloud_transform_needed_ = true; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); - RCLCPP_ERROR( - node_->get_logger(), "Please publish TF %s to %s", base_frame.c_str(), lidar_frame.c_str()); - eigen_lidar_to_base_link_ = Eigen::Matrix4f::Identity(); - eigen_base_link_to_lidar_ = Eigen::Matrix4f::Identity(); - } + pointcloud_transform_exists_ = + static_tf_buffer_->getTransform(node_, base_frame, lidar_frame, eigen_lidar_to_base_link_); + eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse(); + pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_; } inline void DistortionCorrector2D::undistortPointImplementation( diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp index 95e85f8450840..854e93ed52ebc 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp @@ -114,8 +114,7 @@ autoware::pointcloud_preprocessor::Filter::Filter( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void autoware::pointcloud_preprocessor::Filter::setupTF() { - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); + static_tf_buffer_ = std::make_unique(); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -270,20 +269,7 @@ void autoware::pointcloud_preprocessor::Filter::input_indices_callback( // Convert the cloud into the different frame PointCloud2 cloud_transformed; - if (!tf_buffer_->canTransform( - tf_input_frame_, cloud->header.frame_id, this->now(), - rclcpp::Duration::from_seconds(1.0))) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "[input_indices_callback] timeout tf: " << cloud->header.frame_id - << "->" << tf_input_frame_); - return; - } - - if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, *tf_buffer_)) { - RCLCPP_ERROR( - this->get_logger(), - "[input_indices_callback] Error converting input dataset from %s to %s.", - cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + if (!static_tf_buffer_->transformPointcloud(this, tf_input_frame_, *cloud, cloud_transformed)) { return; } cloud_tf = std::make_shared(cloud_transformed); @@ -299,35 +285,6 @@ void autoware::pointcloud_preprocessor::Filter::input_indices_callback( computePublish(cloud_tf, vindices); } -// For performance reason, we get only a transformation matrix here. -// The implementation is based on the one shown in the URL below. -// https://github.com/ros-perception/perception_pcl/blob/628aaec1dc73ef4adea01e9d28f11eb417b948fd/pcl_ros/src/transforms.cpp#L61-L94 -bool autoware::pointcloud_preprocessor::Filter::_calculate_transform_matrix( - const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from, - const tf2_ros::Buffer & tf_buffer, Eigen::Matrix4f & eigen_transform /*output*/) -{ - if (from.header.frame_id == target_frame) { - eigen_transform = Eigen::Matrix4f::Identity(4, 4); - return true; - } - - geometry_msgs::msg::TransformStamped transform; - - try { - transform = tf_buffer.lookupTransform( - target_frame, from.header.frame_id, tf2_ros::fromMsg(from.header.stamp)); - } catch (tf2::LookupException & e) { - RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - return false; - } catch (tf2::ExtrapolationException & e) { - RCLCPP_ERROR(this->get_logger(), "%s", e.what()); - return false; - } - - pcl_ros::transformAsMatrix(transform, eigen_transform); - return true; -} - // Returns false in error cases bool autoware::pointcloud_preprocessor::Filter::calculate_transform_matrix( const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from, @@ -341,20 +298,8 @@ bool autoware::pointcloud_preprocessor::Filter::calculate_transform_matrix( this->get_logger(), "[get_transform_matrix] Transforming input dataset from %s to %s.", from.header.frame_id.c_str(), target_frame.c_str()); - if (!tf_buffer_->canTransform( - target_frame, from.header.frame_id, this->now(), rclcpp::Duration::from_seconds(1.0))) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "[get_transform_matrix] timeout tf: " << from.header.frame_id << "->" << target_frame); - return false; - } - - if (!_calculate_transform_matrix( - target_frame, from, *tf_buffer_, transform_info.eigen_transform /*output*/)) { - RCLCPP_ERROR( - this->get_logger(), - "[calculate_transform_matrix] Error converting input dataset from %s to %s.", - from.header.frame_id.c_str(), target_frame.c_str()); + if (!static_tf_buffer_->getTransform( + this, target_frame, from.header.frame_id, transform_info.eigen_transform)) { return false; } @@ -375,7 +320,9 @@ bool autoware::pointcloud_preprocessor::Filter::convert_output_costly( // Convert the cloud into the different frame auto cloud_transformed = std::make_unique(); - if (!pcl_ros::transformPointCloud(tf_output_frame_, *output, *cloud_transformed, *tf_buffer_)) { + + if (!static_tf_buffer_->transformPointcloud( + this, tf_output_frame_, *output, *cloud_transformed)) { RCLCPP_ERROR( this->get_logger(), "[convert_output_costly] Error converting output dataset from %s to %s.", @@ -394,12 +341,9 @@ bool autoware::pointcloud_preprocessor::Filter::convert_output_costly( output->header.frame_id.c_str(), tf_input_orig_frame_.c_str()); auto cloud_transformed = std::make_unique(); - if (!pcl_ros::transformPointCloud( - tf_input_orig_frame_, *output, *cloud_transformed, *tf_buffer_)) { - RCLCPP_ERROR( - this->get_logger(), - "[convert_output_costly] Error converting output dataset from %s back to %s.", - output->header.frame_id.c_str(), tf_input_orig_frame_.c_str()); + + if (!static_tf_buffer_->transformPointcloud( + this, tf_input_orig_frame_, *output, *cloud_transformed)) { return false; } diff --git a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index edbe30f8f0ea7..121e983f152c8 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -110,8 +110,7 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( // tf2 listener { - tf2_buffer_ = std::make_shared(this->get_clock()); - tf2_listener_ = std::make_shared(*tf2_buffer_); + static_tf_buffer_ = std::make_unique(); } // Subscribers @@ -217,30 +216,6 @@ std::string PointCloudDataSynchronizerComponent::replaceSyncTopicNamePostfix( ////////////////////////////////////////////////////////////////////////////////////////////// // overloaded functions -void PointCloudDataSynchronizerComponent::transformPointCloud( - const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out) -{ - transformPointCloud(in, out, output_frame_); -} - -void PointCloudDataSynchronizerComponent::transformPointCloud( - const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, - const std::string & target_frame) -{ - // Transform the point clouds into the specified output frame - if (target_frame != in->header.frame_id) { - // TODO(YamatoAndo): use TF2 - if (!pcl_ros::transformPointCloud(target_frame, *in, *out, *tf2_buffer_)) { - RCLCPP_ERROR( - this->get_logger(), - "[transformPointCloud] Error converting first input dataset from %s to %s.", - in->header.frame_id.c_str(), target_frame.c_str()); - return; - } - } else { - out = std::make_shared(*in); - } -} /** * @brief compute transform to adjust for old timestamp @@ -346,7 +321,8 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() continue; } // transform pointcloud to output frame - transformPointCloud(e.second, transformed_cloud_ptr); + static_tf_buffer_->transformPointcloud( + this, output_frame_, *e.second, *transformed_cloud_ptr); // calculate transforms to oldest stamp and transform pointcloud to oldest stamp Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); @@ -366,9 +342,9 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr_in_input_frame( new sensor_msgs::msg::PointCloud2()); - transformPointCloud( - transformed_delay_compensated_cloud_ptr, - transformed_delay_compensated_cloud_ptr_in_input_frame, e.second->header.frame_id); + static_tf_buffer_->transformPointcloud( + this, e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr, + *transformed_delay_compensated_cloud_ptr_in_input_frame); transformed_delay_compensated_cloud_ptr = transformed_delay_compensated_cloud_ptr_in_input_frame; } diff --git a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index 6272fe30e47c0..81baa173e76ce 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -79,6 +79,16 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent( // Set parameters use_z_filter_ = declare_parameter("use_z_filter", false); z_threshold_ = declare_parameter("z_threshold", 0.0f); // defined in the base_link frame + + // Set tf + { + rclcpp::Clock::SharedPtr ros_clock = std::make_shared(RCL_ROS_TIME); + tf_buffer_ = std::make_shared(ros_clock); + auto timer_interface = std::make_shared( + get_node_base_interface(), get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + } } void VectorMapInsideAreaFilterComponent::filter( From ca32505bed03f14e59bef41553ab3733cf71c535 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Tue, 6 Aug 2024 09:03:34 +0900 Subject: [PATCH 053/126] refactor(twist2accel)!: prefix package and namespace with autoware (#8299) * add autoware_ prefix Signed-off-by: a-maumau * add autoware_ prefix Signed-off-by: a-maumau * add autoware_ prefix Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: SakodaShintaro --- .github/CODEOWNERS | 2 +- .../pose_twist_fusion_filter.launch.xml | 2 +- .../{twist2accel => autoware_twist2accel}/CMakeLists.txt | 4 ++-- .../{twist2accel => autoware_twist2accel}/README.md | 4 ++-- .../config/twist2accel.param.yaml | 0 .../launch/twist2accel.launch.xml | 4 ++-- .../{twist2accel => autoware_twist2accel}/package.xml | 2 +- .../schema/twist2accel.schema.json | 0 .../src/twist2accel.cpp | 7 +++++-- .../src}/twist2accel.hpp | 9 ++++++--- 10 files changed, 20 insertions(+), 14 deletions(-) rename localization/{twist2accel => autoware_twist2accel}/CMakeLists.txt (81%) rename localization/{twist2accel => autoware_twist2accel}/README.md (89%) rename localization/{twist2accel => autoware_twist2accel}/config/twist2accel.param.yaml (100%) rename localization/{twist2accel => autoware_twist2accel}/launch/twist2accel.launch.xml (65%) rename localization/{twist2accel => autoware_twist2accel}/package.xml (97%) rename localization/{twist2accel => autoware_twist2accel}/schema/twist2accel.schema.json (100%) rename localization/{twist2accel => autoware_twist2accel}/src/twist2accel.cpp (96%) rename localization/{twist2accel/include/twist2accel => autoware_twist2accel/src}/twist2accel.hpp (94%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 9f530e0dea9e3..8d302d98dc196 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -93,7 +93,7 @@ localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masah localization/pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/autoware_twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index b39346de8f8da..fa479c71dcbac 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -26,7 +26,7 @@ - + diff --git a/localization/twist2accel/CMakeLists.txt b/localization/autoware_twist2accel/CMakeLists.txt similarity index 81% rename from localization/twist2accel/CMakeLists.txt rename to localization/autoware_twist2accel/CMakeLists.txt index 9178bf02288a8..c5e3aadfb7fe0 100644 --- a/localization/twist2accel/CMakeLists.txt +++ b/localization/autoware_twist2accel/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(twist2accel) +project(autoware_twist2accel) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "Twist2Accel" + PLUGIN "autoware::twist2accel::Twist2Accel" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR SingleThreadedExecutor ) diff --git a/localization/twist2accel/README.md b/localization/autoware_twist2accel/README.md similarity index 89% rename from localization/twist2accel/README.md rename to localization/autoware_twist2accel/README.md index 5a540dca895d4..4bdf29947e0e7 100644 --- a/localization/twist2accel/README.md +++ b/localization/autoware_twist2accel/README.md @@ -1,4 +1,4 @@ -# twist2accel +# autoware_twist2accel ## Purpose @@ -21,7 +21,7 @@ This package is responsible for estimating acceleration using the output of `ekf ## Parameters -{{ json_to_markdown("localization/twist2accel/schema/twist2accel.schema.json") }} +{{ json_to_markdown("localization/autoware_twist2accel/schema/twist2accel.schema.json") }} ## Future work diff --git a/localization/twist2accel/config/twist2accel.param.yaml b/localization/autoware_twist2accel/config/twist2accel.param.yaml similarity index 100% rename from localization/twist2accel/config/twist2accel.param.yaml rename to localization/autoware_twist2accel/config/twist2accel.param.yaml diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/autoware_twist2accel/launch/twist2accel.launch.xml similarity index 65% rename from localization/twist2accel/launch/twist2accel.launch.xml rename to localization/autoware_twist2accel/launch/twist2accel.launch.xml index c534a288aac3e..6fe10a8c80c32 100644 --- a/localization/twist2accel/launch/twist2accel.launch.xml +++ b/localization/autoware_twist2accel/launch/twist2accel.launch.xml @@ -1,10 +1,10 @@ - + - + diff --git a/localization/twist2accel/package.xml b/localization/autoware_twist2accel/package.xml similarity index 97% rename from localization/twist2accel/package.xml rename to localization/autoware_twist2accel/package.xml index 158d8a8a8d283..af8938fd74a9f 100644 --- a/localization/twist2accel/package.xml +++ b/localization/autoware_twist2accel/package.xml @@ -1,7 +1,7 @@ - twist2accel + autoware_twist2accel 0.0.0 The acceleration estimation package Yamato Ando diff --git a/localization/twist2accel/schema/twist2accel.schema.json b/localization/autoware_twist2accel/schema/twist2accel.schema.json similarity index 100% rename from localization/twist2accel/schema/twist2accel.schema.json rename to localization/autoware_twist2accel/schema/twist2accel.schema.json diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/autoware_twist2accel/src/twist2accel.cpp similarity index 96% rename from localization/twist2accel/src/twist2accel.cpp rename to localization/autoware_twist2accel/src/twist2accel.cpp index 800d696b1dea8..2205a9d3a674e 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/autoware_twist2accel/src/twist2accel.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "twist2accel/twist2accel.hpp" +#include "twist2accel.hpp" #include @@ -22,6 +22,8 @@ #include #include +namespace autoware::twist2accel +{ using std::placeholders::_1; Twist2Accel::Twist2Accel(const rclcpp::NodeOptions & node_options) @@ -103,6 +105,7 @@ void Twist2Accel::estimate_accel(const geometry_msgs::msg::TwistStamped::SharedP pub_accel_->publish(accel_msg); prev_twist_ptr_ = msg; } +} // namespace autoware::twist2accel #include -RCLCPP_COMPONENTS_REGISTER_NODE(Twist2Accel) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::twist2accel::Twist2Accel) diff --git a/localization/twist2accel/include/twist2accel/twist2accel.hpp b/localization/autoware_twist2accel/src/twist2accel.hpp similarity index 94% rename from localization/twist2accel/include/twist2accel/twist2accel.hpp rename to localization/autoware_twist2accel/src/twist2accel.hpp index e2fab219b11b4..88c9cfad13fe9 100644 --- a/localization/twist2accel/include/twist2accel/twist2accel.hpp +++ b/localization/autoware_twist2accel/src/twist2accel.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TWIST2ACCEL__TWIST2ACCEL_HPP_ -#define TWIST2ACCEL__TWIST2ACCEL_HPP_ +#ifndef TWIST2ACCEL_HPP_ +#define TWIST2ACCEL_HPP_ #include "signal_processing/lowpass_filter_1d.hpp" @@ -37,6 +37,8 @@ #include #include +namespace autoware::twist2accel +{ class Twist2Accel : public rclcpp::Node { public: @@ -68,4 +70,5 @@ class Twist2Accel : public rclcpp::Node void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg); void estimate_accel(const geometry_msgs::msg::TwistStamped::SharedPtr msg); }; -#endif // TWIST2ACCEL__TWIST2ACCEL_HPP_ +} // namespace autoware::twist2accel +#endif // TWIST2ACCEL_HPP_ From ea8ff9dfeb4d445547b7abe6d195b6afe0f3b064 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 6 Aug 2024 10:03:33 +0900 Subject: [PATCH 054/126] fix(autoware_accel_brake_map_calibrator): fix redundantInitialization (#8230) Signed-off-by: Ryuta Kambe --- .../src/accel_brake_map_calibrator_node.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 6a88d5c61a2d8..b361a0d2725c7 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/autoware_accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -1326,15 +1326,12 @@ void AccelBrakeMapCalibrator::check_update_suggest( using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; using CalibrationStatus = CalibrationStatus; CalibrationStatus accel_brake_map_status; + accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP; + accel_brake_map_status.status = CalibrationStatus::NORMAL; int8_t level = DiagStatus::OK; - std::string msg; + std::string msg = "OK"; - accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP; - if (is_default_map_) { - accel_brake_map_status.status = CalibrationStatus::NORMAL; - level = DiagStatus::OK; - msg = "OK"; - } else { + if (!is_default_map_) { accel_brake_map_status.status = CalibrationStatus::UNAVAILABLE; level = DiagStatus::ERROR; msg = "Default map is not found in " + csv_default_map_dir_; From 38f1e28bd1a26b89e6dba18fd4195856b6fef062 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 6 Aug 2024 10:36:37 +0900 Subject: [PATCH 055/126] chore(ci): remove unnecessary redundantInitialization cppcheck suppression (#8368) Signed-off-by: veqcc --- .cppcheck_suppressions | 1 - 1 file changed, 1 deletion(-) diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index f52eeb4e3ab2c..79cd117930541 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -9,7 +9,6 @@ missingInclude missingIncludeSystem noConstructor passedByValue -redundantInitialization // cspell: ignore uninit uninitMemberVar unknownMacro From 5a76869242dc47a95a73c9d3e83aae4443b1d6d3 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 6 Aug 2024 12:16:45 +0900 Subject: [PATCH 056/126] fix(traffic_light_visualizer): remove cerr temporarily to avoid flooding logs (#8294) * fix(traffic_light_visualizer): remove cerr temporarily to avoid flooding logs Signed-off-by: kminoda * fix precommit Signed-off-by: kminoda * fix Signed-off-by: kminoda --------- Signed-off-by: kminoda --- .../src/traffic_light_roi_visualizer/shape_draw.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp index c44b1415a09dc..c685bc0639601 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp @@ -52,7 +52,10 @@ void drawShape( if ( position.x < 0 || position.y < 0 || position.x + shapeImg.cols > params.image.cols || position.y + shapeImg.rows > params.image.rows) { - std::cerr << "Adjusted position is out of image bounds." << std::endl; + // TODO(KhalilSelyan): This error message may flood the terminal logs, so commented out + // temporarily. Need to consider a better way. + + // std::cerr << "Adjusted position is out of image bounds." << std::endl; return; } From 2705a63817f02ecfa705960459438763225ea6cf Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 6 Aug 2024 12:44:26 +0900 Subject: [PATCH 057/126] fix(static_obstacle_avoidance): remove invalid small shift lines (#8344) Signed-off-by: satoshi-ota --- .../src/shift_line_generator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 5e58466fa4229..57a6e340bf853 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -827,6 +827,8 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal( data, debug.step1_front_shift_line); + applySmallShiftFilter(ret, 1e-3); + return ret; } From a7a75ea06c988b519ad0e8fa360b52b84b0e288a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 6 Aug 2024 13:24:06 +0900 Subject: [PATCH 058/126] fix(static_obstacle_avoidance): check opposite lane (#8345) Signed-off-by: satoshi-ota --- .../src/utils.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 40373f00d4620..6038fe3daae16 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -693,6 +693,15 @@ bool isNeverAvoidanceTarget( } } + const auto right_opposite_lanes = + planner_data->route_handler->getRightOppositeLanelets(object.overhang_lanelet); + if (!right_opposite_lanes.empty() && isOnRight(object)) { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } + const auto left_lane = planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, true); if (left_lane.has_value() && !isOnRight(object)) { @@ -715,6 +724,15 @@ bool isNeverAvoidanceTarget( return true; } } + + const auto left_opposite_lanes = + planner_data->route_handler->getLeftOppositeLanelets(object.overhang_lanelet); + if (!left_opposite_lanes.empty() && !isOnRight(object)) { + object.info = ObjectInfo::IS_NOT_PARKING_OBJECT; + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); + return true; + } } if (isCloseToStopFactor(object, data, planner_data, parameters)) { From 550f3f7fbd591fde3a28a2ebad33e967fcd2b3c5 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Tue, 6 Aug 2024 17:15:24 +0900 Subject: [PATCH 059/126] chore(codecov): ignore test directory in codecov (#8374) Signed-off-by: Y.Hisaki --- codecov.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/codecov.yaml b/codecov.yaml index ecc77815e97e6..255312a29ccf7 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -26,3 +26,7 @@ flag_management: type: patch target: 0% # Make CI always succeed threshold: 100% # Make CI always succeed + +ignore: + - "**/test/*" + - "**/test/**/*" From f7027ff964e4c4927ea2a25312f4c1bbb3dda48b Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Tue, 6 Aug 2024 17:16:25 +0900 Subject: [PATCH 060/126] chore(readme): add codecov badge (#8375) Signed-off-by: Y.Hisaki --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 23d0b172554fd..35818990cee95 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ # Autoware Universe +[![codecov](https://codecov.io/github/autowarefoundation/autoware.universe/graph/badge.svg?token=KQP68YQ65D)](https://codecov.io/github/autowarefoundation/autoware.universe) + ## Welcome to Autoware Universe Autoware Universe serves as a foundational pillar within the Autoware ecosystem, playing a critical role in enhancing the core functionalities of autonomous driving technologies. From d7bde29aa03759c40b9663f6826a013678b8b18d Mon Sep 17 00:00:00 2001 From: taisa1 <38522559+taisa1@users.noreply.github.com> Date: Tue, 6 Aug 2024 19:41:44 +0900 Subject: [PATCH 061/126] fix(autoware_pointcloud_preprocessor): fix cppcheck warnings of functionStatic (#8163) fix: deal with functionStatic warnings Signed-off-by: taisa1 Co-authored-by: Yi-Hsiang Fang (Vivid) <146902905+vividf@users.noreply.github.com> --- .../src/downsample_filter/robin_hood.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h index a50ea7f70b774..51f40765e9ea1 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h @@ -567,7 +567,7 @@ template struct NodeAllocator { // we are not using the data, so just free it. - void addOrFree(void * ptr, size_t ROBIN_HOOD_UNUSED(numBytes) /*unused*/) noexcept + static void addOrFree(void * ptr, size_t ROBIN_HOOD_UNUSED(numBytes) /*unused*/) noexcept { ROBIN_HOOD_LOG("std::free") std::free(ptr); @@ -1051,8 +1051,8 @@ class Table } // doesn't do anything - void destroy(M & ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {} - void destroyDoNotDeallocate() noexcept {} + static void destroy(M & ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {} + static void destroyDoNotDeallocate() noexcept {} value_type const * operator->() const noexcept { return &mData; } value_type * operator->() noexcept { return &mData; } @@ -1262,15 +1262,15 @@ class Table template struct Destroyer { - void nodes(M & m) const noexcept { m.mNumElements = 0; } + static void nodes(M & m) noexcept { m.mNumElements = 0; } - void nodesDoNotDeallocate(M & m) const noexcept { m.mNumElements = 0; } + static void nodesDoNotDeallocate(M & m) noexcept { m.mNumElements = 0; } }; template struct Destroyer { - void nodes(M & m) const noexcept + static void nodes(M & m) noexcept { m.mNumElements = 0; // clear also resets mInfo to 0, that's sometimes not necessary. @@ -1285,7 +1285,7 @@ class Table } } - void nodesDoNotDeallocate(M & m) const noexcept + static void nodesDoNotDeallocate(M & m) noexcept { m.mNumElements = 0; // clear also resets mInfo to 0, that's sometimes not necessary. From b29c84eaa4b1e75eef7041f52dfbe3032ae5b10c Mon Sep 17 00:00:00 2001 From: taisa1 <38522559+taisa1@users.noreply.github.com> Date: Tue, 6 Aug 2024 19:42:42 +0900 Subject: [PATCH 062/126] fix(autoware_detected_object_validation): fix cppcheck warnings of functionStatic (#8256) fix: deal with functionStatic warnings Signed-off-by: taisa1 --- .../src/obstacle_pointcloud/debugger.hpp | 2 +- .../src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp index 5886474d51b34..e097755c6c506 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/debugger.hpp @@ -97,7 +97,7 @@ class Debugger pcl::PointCloud::Ptr pointcloud_within_polygon_; private: - inline pcl::PointCloud::Ptr toXYZ( + inline static pcl::PointCloud::Ptr toXYZ( const pcl::PointCloud::Ptr & pointcloud) { pcl::PointCloud::Ptr pointcloud_xyz(new pcl::PointCloud); diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index 75f513ba803c2..6d9a11634ea08 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -92,7 +92,7 @@ class Validator2D : public Validator public: explicit Validator2D(PointsNumThresholdParam & points_num_threshold_param); - pcl::PointCloud::Ptr convertToXYZ( + static pcl::PointCloud::Ptr convertToXYZ( const pcl::PointCloud::Ptr & pointcloud_xy); inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() override { From f59209c533c6caea0e974a7c2c4880d0afbbaa9e Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Tue, 6 Aug 2024 20:47:26 +0900 Subject: [PATCH 063/126] fix(autoware_traffic_light_occlusion_predictor): fix functionConst (#8286) fix:functionConst Signed-off-by: kobayu858 --- .../src/occlusion_predictor.cpp | 2 +- .../src/occlusion_predictor.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 74878ba0b93af..7fd5ac8489226 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -148,7 +148,7 @@ void CloudOcclusionPredictor::calcRoiVector3D( void CloudOcclusionPredictor::filterCloud( const pcl::PointCloud & cloud_in, const std::vector & roi_tls, - const std::vector & roi_brs, pcl::PointCloud & cloud_out) + const std::vector & roi_brs, pcl::PointCloud & cloud_out) const { float min_x = 0, max_x = 0, min_y = 0, max_y = 0, min_z = 0, max_z = 0; for (const auto & pt : roi_tls) { diff --git a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp index 435ee478eea22..701b15f7c92b8 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp +++ b/perception/autoware_traffic_light_occlusion_predictor/src/occlusion_predictor.hpp @@ -73,7 +73,7 @@ class CloudOcclusionPredictor void filterCloud( const pcl::PointCloud & cloud_in, const std::vector & roi_tls, - const std::vector & roi_brs, pcl::PointCloud & cloud_out); + const std::vector & roi_brs, pcl::PointCloud & cloud_out) const; void sampleTrafficLightRoi( const pcl::PointXYZ & top_left, const pcl::PointXYZ & bottom_right, From e88d4825e74435298567072e2d09f7b61c547fef Mon Sep 17 00:00:00 2001 From: taisa1 <38522559+taisa1@users.noreply.github.com> Date: Tue, 6 Aug 2024 22:24:55 +0900 Subject: [PATCH 064/126] fix(autoware_auto_common): fix cppcheck warnings of functionStatic (#8265) fix: deal with functionStatic warnings Signed-off-by: taisa1 --- common/autoware_auto_common/test/test_template_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_auto_common/test/test_template_utils.cpp b/common/autoware_auto_common/test/test_template_utils.cpp index a670aaab83cfa..05e468dc4787e 100644 --- a/common/autoware_auto_common/test/test_template_utils.cpp +++ b/common/autoware_auto_common/test/test_template_utils.cpp @@ -27,7 +27,7 @@ struct FalseType struct Foo { - CorrectType bar(CorrectType, const CorrectType &, CorrectType *) { return CorrectType{}; } + static CorrectType bar(CorrectType, const CorrectType &, CorrectType *) { return CorrectType{}; } }; template