From 62291d07d9f2e0fd80a6905a31c07500862c10c1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 26 Apr 2023 13:56:52 +0900 Subject: [PATCH 1/2] refactor(behavior_path_planner): rename pull_over to goal_planner (#3501) Signed-off-by: kosuke55 --- .../src/rtc_manager_panel.cpp | 6 +- .../src/velocity_steering_factors_panel.cpp | 4 +- .../launch/planning.launch.xml | 2 +- .../behavior_planning.launch.py | 6 +- launch/tier4_planning_launch/package.xml | 2 +- planning/behavior_path_planner/CMakeLists.txt | 15 +- planning/behavior_path_planner/README.md | 34 ++--- .../goal_planner.param.yaml} | 2 +- .../config/scene_module_manager.param.yaml | 2 +- ...avior_path_planner_drivable_area_design.md | 2 +- ...avior_path_planner_goal_planner_design.md} | 16 +-- ...ehavior_path_planner_turn_signal_design.md | 4 +- .../behavior_path_planner_node.hpp | 10 +- .../behavior_path_planner/parameters.hpp | 2 +- .../goal_planner_module.hpp} | 36 ++--- .../{pull_over => goal_planner}/manager.hpp | 20 +-- .../scene_module/scene_module_visitor.hpp | 2 +- .../default_fixed_goal_planner.hpp | 8 +- .../fixed_goal_planner_base.hpp | 6 +- .../freespace_pull_over.hpp | 10 +- .../geometric_pull_over.hpp | 10 +- .../goal_planner_parameters.hpp} | 8 +- .../goal_searcher.hpp | 10 +- .../goal_searcher_base.hpp | 12 +- .../pull_over_planner_base.hpp | 12 +- .../shift_pull_over.hpp | 10 +- .../{pull_over => goal_planner}/util.hpp | 12 +- planning/behavior_path_planner/package.xml | 2 +- .../src/behavior_path_planner_node.cpp | 37 ++--- .../goal_planner_module.cpp} | 136 +++++++++--------- .../{pull_over => goal_planner}/manager.cpp | 8 +- .../default_fixed_goal_planner.cpp | 4 +- .../freespace_pull_over.cpp | 6 +- .../geometric_pull_over.cpp | 9 +- .../goal_searcher.cpp | 12 +- .../shift_pull_over.cpp | 9 +- .../{pull_over => goal_planner}/util.cpp | 6 +- ...t_behavior_path_planner_node_interface.cpp | 2 +- .../config/rtc_auto_mode_manager.param.yaml | 4 +- .../src/rtc_auto_mode_manager_interface.cpp | 4 +- planning/rtc_interface/src/rtc_interface.cpp | 4 +- .../rtc_replayer/src/rtc_replayer_node.cpp | 4 +- system/default_ad_api/src/planning.cpp | 2 +- .../ad_api_visualizers/planning_factors.py | 2 +- 44 files changed, 259 insertions(+), 255 deletions(-) rename planning/behavior_path_planner/config/{pull_over/pull_over.param.yaml => goal_planner/goal_planner.param.yaml} (99%) rename planning/behavior_path_planner/docs/{behavior_path_planner_pull_over_design.md => behavior_path_planner_goal_planner_design.md} (98%) rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/{pull_over/pull_over_module.hpp => goal_planner/goal_planner_module.hpp} (86%) rename planning/behavior_path_planner/include/behavior_path_planner/scene_module/{pull_over => goal_planner}/manager.hpp (62%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over => goal_planner}/default_fixed_goal_planner.hpp (80%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over => goal_planner}/fixed_goal_planner_base.hpp (84%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over => goal_planner}/freespace_pull_over.hpp (80%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over => goal_planner}/geometric_pull_over.hpp (87%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over/pull_over_parameters.hpp => goal_planner/goal_planner_parameters.hpp} (92%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over => goal_planner}/goal_searcher.hpp (83%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over => goal_planner}/goal_searcher_base.hpp (82%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over => goal_planner}/pull_over_planner_base.hpp (87%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over => goal_planner}/shift_pull_over.hpp (87%) rename planning/behavior_path_planner/include/behavior_path_planner/utils/{pull_over => goal_planner}/util.hpp (88%) rename planning/behavior_path_planner/src/scene_module/{pull_over/pull_over_module.cpp => goal_planner/goal_planner_module.cpp} (92%) rename planning/behavior_path_planner/src/scene_module/{pull_over => goal_planner}/manager.cpp (84%) rename planning/behavior_path_planner/src/utils/{pull_over => goal_planner}/default_fixed_goal_planner.cpp (95%) rename planning/behavior_path_planner/src/utils/{pull_over => goal_planner}/freespace_pull_over.cpp (95%) rename planning/behavior_path_planner/src/utils/{pull_over => goal_planner}/geometric_pull_over.cpp (88%) rename planning/behavior_path_planner/src/utils/{pull_over => goal_planner}/goal_searcher.cpp (96%) rename planning/behavior_path_planner/src/utils/{pull_over => goal_planner}/shift_pull_over.cpp (97%) rename planning/behavior_path_planner/src/utils/{pull_over => goal_planner}/util.cpp (97%) diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index b28c42f68ed7f..5e795ae764092 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -63,8 +63,8 @@ std::string getModuleName(const uint8_t module_type) case Module::AVOIDANCE_RIGHT: { return "avoidance_right"; } - case Module::PULL_OVER: { - return "pull_over"; + case Module::GOAL_PLANNER: { + return "goal_planner"; } case Module::PULL_OUT: { return "pull_out"; @@ -105,7 +105,7 @@ bool isPathChangeModule(const uint8_t module_type) module_type == Module::EXT_REQUEST_LANE_CHANGE_RIGHT || module_type == Module::AVOIDANCE_BY_LC_LEFT || module_type == Module::AVOIDANCE_BY_LC_RIGHT || module_type == Module::AVOIDANCE_LEFT || module_type == Module::AVOIDANCE_RIGHT || - module_type == Module::PULL_OVER || module_type == Module::PULL_OUT) { + module_type == Module::GOAL_PLANNER || module_type == Module::PULL_OUT) { return true; } return false; diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index e97c08d169042..48bb671a49a22 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -234,8 +234,8 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: case SteeringFactor::PULL_OUT: label->setText("PULL_OUT"); break; - case SteeringFactor::PULL_OVER: - label->setText("PULL_OVER"); + case SteeringFactor::GOAL_PLANNER: + label->setText("GOAL_PLANNER"); break; case SteeringFactor::EMERGENCY_OPERATION: label->setText("EMERGENCY_OPERATION"); diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index b6c503be9f6ba..128ed0f9cd0ae 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -22,7 +22,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index dc0769abac58e..b37c177dd79e6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -51,8 +51,8 @@ def launch_setup(context, *args, **kwargs): avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f: lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("pull_over_param_path").perform(context), "r") as f: - pull_over_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f: + goal_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("pull_out_param_path").perform(context), "r") as f: pull_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f: @@ -91,7 +91,7 @@ def launch_setup(context, *args, **kwargs): avoidance_param, avoidance_by_lc_param, lane_change_param, - pull_over_param, + goal_planner_param, pull_out_param, drivable_area_expansion_param, scene_module_manager_param, diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 050968fe70329..c17e94a86568f 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -7,7 +7,7 @@ Zulfaqar Azmi - + Kosuke Takeuchi Kosuke Takeuchi diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 6b79dd9c9ea47..5f2231bd3ebe2 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -16,7 +16,7 @@ set(common_src src/scene_module/avoidance/avoidance_module.cpp src/scene_module/avoidance_by_lc/module.cpp src/scene_module/pull_out/pull_out_module.cpp - src/scene_module/pull_over/pull_over_module.cpp + src/scene_module/goal_planner/goal_planner_module.cpp src/scene_module/side_shift/side_shift_module.cpp src/scene_module/lane_change/interface.cpp src/scene_module/lane_change/lane_change_module.cpp @@ -30,12 +30,12 @@ set(common_src src/utils/avoidance/util.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp - src/utils/pull_over/util.cpp - src/utils/pull_over/shift_pull_over.cpp - src/utils/pull_over/geometric_pull_over.cpp - src/utils/pull_over/freespace_pull_over.cpp - src/utils/pull_over/goal_searcher.cpp - src/utils/pull_over/default_fixed_goal_planner.cpp + src/utils/goal_planner/util.cpp + src/utils/goal_planner/shift_pull_over.cpp + src/utils/goal_planner/geometric_pull_over.cpp + src/utils/goal_planner/freespace_pull_over.cpp + src/utils/goal_planner/goal_searcher.cpp + src/utils/goal_planner/default_fixed_goal_planner.cpp src/utils/pull_out/util.cpp src/utils/pull_out/shift_pull_out.cpp src/utils/pull_out/geometric_pull_out.cpp @@ -56,7 +56,6 @@ if(COMPILE_WITH_OLD_ARCHITECTURE) src/behavior_tree_manager.cpp src/scene_module/scene_module_bt_node_interface.cpp src/scene_module/lane_following/lane_following_module.cpp - src/scene_module/pull_over/pull_over_module.cpp ${common_src} ) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index fb4a96d4b9f44..489d1095c0f2e 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -16,16 +16,16 @@ The `behavior_path_planner` module is responsible to generate Behavior path planner has following scene modules. -| Name | Description | Details | -| :------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_design.md) | -| Avoidance By LC | this module generates lane change path when there is objects that should be avoid. | LINK | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](./docs/behavior_path_planner_lane_change_design.md) | -| External Lane Change | WIP | LINK | -| Pull Over | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](./docs/behavior_path_planner_pull_over_design.md) | -| Pull Out | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_pull_out_design.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](./docs/behavior_path_planner_side_shift_design.md) | +| Name | Description | Details | +| :------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------- | +| Lane Following | this module generates reference path from lanelet centerline. | LINK | +| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_design.md) | +| Avoidance By LC | this module generates lane change path when there is objects that should be avoid. | LINK | +| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](./docs/behavior_path_planner_lane_change_design.md) | +| External Lane Change | WIP | LINK | +| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](./docs/behavior_path_planner_goal_planner_design.md) | +| Pull Out | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_pull_out_design.md) | +| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](./docs/behavior_path_planner_side_shift_design.md) | ![behavior_modules](./image/behavior_modules.png) @@ -91,13 +91,13 @@ The current behavior tree structure is shown below. Each modules (LaneChange, Av ### input -| Name | Type | Description | -| :----------------------------- | :----------------------------------------------------- | :--------------------------------------------------------------------------------- | -| ~/input/route | `autoware_auto_mapping_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/vector_map | `autoware_auto_mapping_msgs::msg::HADMapBin` | map information. | -| ~/input/objects | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Pull Over module. | -| ~/input/kinematic_state | `nav_msgs::msg::Odometry` | for ego velocity. | +| Name | Type | Description | +| :----------------------------- | :----------------------------------------------------- | :------------------------------------------------------------------------------------ | +| ~/input/route | `autoware_auto_mapping_msgs::msg::LaneletRoute` | current route from start to goal. | +| ~/input/vector_map | `autoware_auto_mapping_msgs::msg::HADMapBin` | map information. | +| ~/input/objects | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | +| ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | +| ~/input/kinematic_state | `nav_msgs::msg::Odometry` | for ego velocity. | ## General features of behavior path planner diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml similarity index 99% rename from planning/behavior_path_planner/config/pull_over/pull_over.param.yaml rename to planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index f777581150369..5a840098e5956 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - pull_over: + goal_planner: minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index 488fdce103216..f5f1afe805d9b 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -45,7 +45,7 @@ priority: 2 max_module_size: 1 - pull_over: + goal_planner: enable_module: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md index 426d062ac68a2..e5c18a9ca35b4 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design.md @@ -57,7 +57,7 @@ This section gives details of the generation of the drivable area (`left_bound` ### Drivable Lanes Generation -Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Pull Over`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes. +Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Goal Planner`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes. ```cpp struct DrivalbleLanes diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_pull_over_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md similarity index 98% rename from planning/behavior_path_planner/docs/behavior_path_planner_pull_over_design.md rename to planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 4d95dfd638491..06f81758f163f 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_pull_over_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -1,14 +1,14 @@ -# Pull Over design +# Goal Planner design ## Purpose / Role -Search for a space where there are no objects and pull over there. +Search for a space where there are no objects and goal planner there. ## Design ```plantuml @startuml -package pull_over{ +package goal_planner{ abstract class PullOverPlannerBase { } abstract class GoalSeacherBase { @@ -29,7 +29,7 @@ package pull_over{ class GoalSeacher { } - class PullOverModule { + class GoalPlannerModule { } @@ -54,7 +54,7 @@ package freespace_planning_algorithms class RRTStar{} } -' pull over +' goal planner ShiftPullOver --|> PullOverPlannerBase GeometricPullOver --|> PullOverPlannerBase FreeSpacePullOver --|> PullOverPlannerBase @@ -65,8 +65,8 @@ GeometricParallelParking --o GeometricPullOver AstarSearch --o FreeSpacePullOver RRTStar --o FreeSpacePullOver -PullOverPlannerBase --o PullOverModule -GoalSeacherBase --o PullOverModule +PullOverPlannerBase --o GoalPlannerModule +GoalSeacherBase --o GoalPlannerModule PullOverPath --o PullOverPlannerBase GoalCandidates --o GoalSeacherBase @@ -74,7 +74,7 @@ GoalCandidates --o GoalSeacherBase @enduml ``` -## General parameters for pull_over +## General parameters for goal_planner | Name | Unit | Type | Description | Default value | | :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md index ede77e8a38e12..8b64db997aec6 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md @@ -53,7 +53,7 @@ When turning on the blinker, it decides whether or not to turn on the specified ![activation_distance](../image/turn_signal_decider/activation_distance.drawio.svg) -For left turn, right turn, avoidance, lane change, pull over and pull out, we define these two sections, which are elaborated in the following part. +For left turn, right turn, avoidance, lane change, goal planner and pull out, we define these two sections, which are elaborated in the following part. #### 1. Left and Right turn @@ -141,7 +141,7 @@ Second section ![section_pull_out](../image/turn_signal_decider/pull_out.drawio.svg) -#### 5. Pull over +#### 5. Goal Planner - desired start point `v * turn_signal_search_time` meters before the start point of the pull over path. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 8dc258462f34e..843c0cfdab20f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -21,18 +21,18 @@ #ifdef USE_OLD_ARCHITECTURE #include "behavior_path_planner/behavior_tree_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" +#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp" #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" -#include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" #else #include "behavior_path_planner/planner_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/manager.hpp" #include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp" +#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/scene_module/lane_change/manager.hpp" #include "behavior_path_planner/scene_module/pull_out/manager.hpp" -#include "behavior_path_planner/scene_module/pull_over/manager.hpp" #include "behavior_path_planner/scene_module/side_shift/manager.hpp" #endif @@ -40,10 +40,10 @@ #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" #include "behavior_path_planner/utils/pull_out/pull_out_parameters.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp" #include "behavior_path_planner/utils/side_shift/side_shift_parameters.hpp" #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" @@ -152,7 +152,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr side_shift_param_ptr_; std::shared_ptr lane_change_param_ptr_; std::shared_ptr pull_out_param_ptr_; - std::shared_ptr pull_over_param_ptr_; + std::shared_ptr goal_planner_param_ptr_; BehaviorPathPlannerParameters getCommonParam(); @@ -163,7 +163,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node AvoidanceParameters getAvoidanceParam(); LaneChangeParameters getLaneChangeParam(); SideShiftParameters getSideShiftParam(); - PullOverParameters getPullOverParam(); + GoalPlannerParameters getGoalPlannerParam(); PullOutParameters getPullOutParam(); AvoidanceByLCParameters getAvoidanceByLCParam( const std::shared_ptr & avoidance_param, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index a49a13b0ffe05..45ceb9d527a2e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -36,7 +36,7 @@ struct BehaviorPathPlannerParameters ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; ModuleConfigParameters config_pull_out; - ModuleConfigParameters config_pull_over; + ModuleConfigParameters config_goal_planner; ModuleConfigParameters config_side_shift; ModuleConfigParameters config_lane_change_left; ModuleConfigParameters config_lane_change_right; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 3e89b24455ef9..500e37b293a97 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" +#include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" +#include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/pull_over/default_fixed_goal_planner.hpp" -#include "behavior_path_planner/utils/pull_over/freespace_pull_over.hpp" -#include "behavior_path_planner/utils/pull_over/geometric_pull_over.hpp" -#include "behavior_path_planner/utils/pull_over/goal_searcher.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp" -#include "behavior_path_planner/utils/pull_over/shift_pull_over.hpp" #include #include @@ -85,20 +85,20 @@ struct PUllOverStatus bool during_freespace_parking{false}; }; -class PullOverModule : public SceneModuleInterface +class GoalPlannerModule : public SceneModuleInterface { public: #ifdef USE_OLD_ARCHITECTURE - PullOverModule( + GoalPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters); #else - PullOverModule( + GoalPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, + const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map); - void updateModuleParams(const std::shared_ptr & parameters) + void updateModuleParams(const std::shared_ptr & parameters) { parameters_ = parameters; } @@ -116,7 +116,7 @@ class PullOverModule : public SceneModuleInterface void processOnEntry() override; void processOnExit() override; - void setParameters(const std::shared_ptr & parameters); + void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override @@ -124,7 +124,7 @@ class PullOverModule : public SceneModuleInterface } private: - std::shared_ptr parameters_; + std::shared_ptr parameters_; std::vector> pull_over_planners_; std::unique_ptr freespace_planner_; @@ -165,7 +165,7 @@ class PullOverModule : public SceneModuleInterface bool incrementPathIndex(); PathWithLaneId getCurrentPath() const; Pose calcRefinedGoal(const Pose & goal_pose) const; - ParallelParkingParameters getGeometricPullOverParameters() const; + ParallelParkingParameters getGeometricGoalPlannerParameters() const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; void decelerateBeforeSearchStart(const Pose & search_start_pose, PathWithLaneId & path) const; @@ -225,4 +225,4 @@ class PullOverModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp similarity index 62% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index fbad4d6df736b..31920ecd640f0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" +#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include @@ -27,26 +27,26 @@ namespace behavior_path_planner { -class PullOverModuleManager : public SceneModuleManagerInterface +class GoalPlannerModuleManager : public SceneModuleManagerInterface { public: - PullOverModuleManager( + GoalPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters); std::shared_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; private: - std::shared_ptr parameters_; + std::shared_ptr parameters_; - std::vector> registered_modules_; + std::vector> registered_modules_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp index afc53fc04700f..03473709446c3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp @@ -34,7 +34,7 @@ class LaneChangeInterface; class LaneChangeBTInterface; class LaneFollowingModule; class PullOutModule; -class PullOverModule; +class GoalPlannerModule; class SideShiftModule; using tier4_planning_msgs::msg::AvoidanceDebugMsg; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/default_fixed_goal_planner.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp index 7a898afbb6e0f..bca97847c7960 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#include "behavior_path_planner/utils/pull_over/fixed_goal_planner_base.hpp" +#include "behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp" #include @@ -39,4 +39,4 @@ class DefaultFixedGoalPlanner : public FixedGoalPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/fixed_goal_planner_base.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp index 6941747911a64..0840b5bb78008 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FIXED_GOAL_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FIXED_GOAL_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/parameters.hpp" @@ -42,4 +42,4 @@ class FixedGoalPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FIXED_GOAL_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/freespace_pull_over.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp index d8f671163722e..1378be5461a64 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FREESPACE_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FREESPACE_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ -#include "behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp" +#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" #include #include @@ -36,7 +36,7 @@ class FreespacePullOver : public PullOverPlannerBase { public: FreespacePullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } @@ -50,4 +50,4 @@ class FreespacePullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FREESPACE_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/geometric_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp similarity index 87% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/geometric_pull_over.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp index 935eed75837d8..6177a6197a142 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp" #include @@ -33,7 +33,7 @@ class GeometricPullOver : public PullOverPlannerBase { public: GeometricPullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const ParallelParkingParameters & parallel_parking_parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr occupancy_grid_map, @@ -70,4 +70,4 @@ class GeometricPullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GEOMETRIC_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 477f6dae46562..ff091a88ed44a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ #include #include @@ -35,7 +35,7 @@ enum class ParkingPolicy { RIGHT_SIDE, }; -struct PullOverParameters +struct GoalPlannerParameters { double minimum_request_length; double th_arrived_distance; @@ -112,4 +112,4 @@ struct PullOverParameters }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index 01f77a37418e6..be6e1c0046449 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ +#include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/pull_over/goal_searcher_base.hpp" #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" @@ -33,7 +33,7 @@ class GoalSearcher : public GoalSearcherBase { public: GoalSearcher( - const PullOverParameters & parameters, const LinearRing2d & vehicle_footprint, + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map); GoalCandidates search(const Pose & original_goal_pose) override; @@ -53,4 +53,4 @@ class GoalSearcher : public GoalSearcherBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp similarity index 82% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher_base.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index 8bdbca45f4bb5..bab13a287960a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/parameters.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include @@ -54,7 +54,7 @@ using GoalCandidates = std::vector; class GoalSearcherBase { public: - explicit GoalSearcherBase(const PullOverParameters & parameters) { parameters_ = parameters; } + explicit GoalSearcherBase(const GoalPlannerParameters & parameters) { parameters_ = parameters; } virtual ~GoalSearcherBase() = default; void setPlannerData(const std::shared_ptr & planner_data) @@ -67,10 +67,10 @@ class GoalSearcherBase virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } protected: - PullOverParameters parameters_; + GoalPlannerParameters parameters_; std::shared_ptr planner_data_; MultiPolygon2d area_polygons_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__GOAL_SEARCHER_BASE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp similarity index 87% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index dbf50363676f1..4b800917d4aec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_parameters.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include #include @@ -87,7 +87,7 @@ struct PullOverPath class PullOverPlannerBase { public: - PullOverPlannerBase(rclcpp::Node & node, const PullOverParameters & parameters) + PullOverPlannerBase(rclcpp::Node & node, const GoalPlannerParameters & parameters) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); vehicle_footprint_ = createVehicleFootprint(vehicle_info_); @@ -107,8 +107,8 @@ class PullOverPlannerBase std::shared_ptr planner_data_; vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; - PullOverParameters parameters_; + GoalPlannerParameters parameters_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__PULL_OVER_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp similarity index 87% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/shift_pull_over.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp index 27207e10f7ea3..445161f631bb7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__SHIFT_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__SHIFT_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ +#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/pull_over/pull_over_planner_base.hpp" #include @@ -33,7 +33,7 @@ class ShiftPullOver : public PullOverPlannerBase { public: ShiftPullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr & occupancy_grid_map); @@ -62,4 +62,4 @@ class ShiftPullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__SHIFT_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp similarity index 88% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/util.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index dec0f25131d2a..b1f548425d48f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ -#include "behavior_path_planner/utils/pull_over/goal_searcher_base.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -34,7 +34,7 @@ namespace behavior_path_planner { -namespace pull_over_utils +namespace goal_planner_utils { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; @@ -61,7 +61,7 @@ MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); -} // namespace pull_over_utils +} // namespace goal_planner_utils } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__UTIL_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 376c468e2d605..4e8f546753bf5 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -7,7 +7,7 @@ Zulfaqar Azmi - + Kosuke Takeuchi Kosuke Takeuchi diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 69186102ac179..5a78759ea9b56 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -123,7 +123,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod avoidance_param_ptr_ = std::make_shared(getAvoidanceParam()); lane_change_param_ptr_ = std::make_shared(getLaneChangeParam()); pull_out_param_ptr_ = std::make_shared(getPullOutParam()); - pull_over_param_ptr_ = std::make_shared(getPullOverParam()); + goal_planner_param_ptr_ = std::make_shared(getGoalPlannerParam()); side_shift_param_ptr_ = std::make_shared(getSideShiftParam()); avoidance_by_lc_param_ptr_ = std::make_shared( getAvoidanceByLCParam(avoidance_param_ptr_, lane_change_param_ptr_)); @@ -177,11 +177,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "LaneChange", create_publisher(path_candidate_name_space + "lane_change", 1)); bt_manager_->registerSceneModule(lane_change_module); - auto pull_over_module = - std::make_shared("PullOver", *this, pull_over_param_ptr_); + auto goal_planner = + std::make_shared("GoalPlanner", *this, goal_planner_param_ptr_); path_candidate_publishers_.emplace( - "PullOver", create_publisher(path_candidate_name_space + "pull_over", 1)); - bt_manager_->registerSceneModule(pull_over_module); + "GoalPlanner", create_publisher(path_candidate_name_space + "goal_planner", 1)); + bt_manager_->registerSceneModule(goal_planner); auto pull_out_module = std::make_shared("PullOut", *this, pull_out_param_ptr_); path_candidate_publishers_.emplace( @@ -221,14 +221,14 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "pull_out", create_publisher(path_reference_name_space + "pull_out", 1)); } - if (p.config_pull_over.enable_module) { - auto manager = std::make_shared( - this, "pull_over", p.config_pull_over, pull_over_param_ptr_); + if (p.config_goal_planner.enable_module) { + auto manager = std::make_shared( + this, "goal_planner", p.config_goal_planner, goal_planner_param_ptr_); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( - "pull_over", create_publisher(path_candidate_name_space + "pull_over", 1)); + "goal_planner", create_publisher(path_candidate_name_space + "goal_planner", 1)); path_reference_publishers_.emplace( - "pull_over", create_publisher(path_reference_name_space + "pull_over", 1)); + "goal_planner", create_publisher(path_reference_name_space + "goal_planner", 1)); } if (p.config_side_shift.enable_module) { @@ -339,7 +339,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() }; p.config_pull_out = get_scene_module_manager_param("pull_out."); - p.config_pull_over = get_scene_module_manager_param("pull_over."); + p.config_goal_planner = get_scene_module_manager_param("goal_planner."); p.config_side_shift = get_scene_module_manager_param("side_shift."); p.config_lane_change_left = get_scene_module_manager_param("lane_change_left."); p.config_lane_change_right = get_scene_module_manager_param("lane_change_right."); @@ -718,12 +718,12 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() return p; } -PullOverParameters BehaviorPathPlannerNode::getPullOverParam() +GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() { - PullOverParameters p; + GoalPlannerParameters p; { - std::string ns = "pull_over."; + std::string ns = "goal_planner."; p.minimum_request_length = declare_parameter(ns + "minimum_request_length"); p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); @@ -823,13 +823,14 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.parking_policy = ParkingPolicy::RIGHT_SIDE; } else { RCLCPP_ERROR_STREAM( - get_logger(), "[pull_over] invalid parking_policy: " << parking_policy_name << std::endl); + get_logger(), + "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); exit(EXIT_FAILURE); } } { - std::string ns = "pull_over.freespace_parking."; + std::string ns = "goal_planner.freespace_parking."; // search configs p.algorithm = declare_parameter(ns + "planning_algorithm"); p.freespace_parking_velocity = declare_parameter(ns + "velocity"); @@ -858,7 +859,7 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() } { - std::string ns = "pull_over.freespace_parking.astar."; + std::string ns = "goal_planner.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = declare_parameter(ns + "use_back"); @@ -867,7 +868,7 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() } { - std::string ns = "pull_over.freespace_parking.rrtstar."; + std::string ns = "goal_planner.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = declare_parameter(ns + "use_informed_sampling"); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp similarity index 92% rename from planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp rename to planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index e93fa1b41a7fe..40e1d86738c0e 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp" +#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -48,17 +48,17 @@ using tier4_autoware_utils::inverseTransformPose; namespace behavior_path_planner { #ifdef USE_OLD_ARCHITECTURE -PullOverModule::PullOverModule( +GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters) + const std::shared_ptr & parameters) : SceneModuleInterface{name, node, createRTCInterfaceMap(node, name, {""})}, parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { #else -PullOverModule::PullOverModule( +GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, + const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, @@ -85,13 +85,13 @@ PullOverModule::PullOverModule( if (parameters_->enable_arc_forward_parking) { constexpr bool is_forward = true; pull_over_planners_.push_back(std::make_shared( - node, *parameters, getGeometricPullOverParameters(), lane_departure_checker, + node, *parameters, getGeometricGoalPlannerParameters(), lane_departure_checker, occupancy_grid_map_, is_forward)); } if (parameters_->enable_arc_backward_parking) { constexpr bool is_forward = false; pull_over_planners_.push_back(std::make_shared( - node, *parameters, getGeometricPullOverParameters(), lane_departure_checker, + node, *parameters, getGeometricGoalPlannerParameters(), lane_departure_checker, occupancy_grid_map_, is_forward)); } } @@ -114,7 +114,7 @@ PullOverModule::PullOverModule( lane_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); lane_parking_timer_ = rclcpp::create_timer( - &node, clock_, lane_parking_period_ns, std::bind(&PullOverModule::onTimer, this), + &node, clock_, lane_parking_period_ns, std::bind(&GoalPlannerModule::onTimer, this), lane_parking_timer_cb_group_); // freespace parking @@ -125,13 +125,14 @@ PullOverModule::PullOverModule( node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); freespace_parking_timer_ = rclcpp::create_timer( &node, clock_, freespace_parking_period_ns, - std::bind(&PullOverModule::onFreespaceParkingTimer, this), freespace_parking_timer_cb_group_); + std::bind(&GoalPlannerModule::onFreespaceParkingTimer, this), + freespace_parking_timer_cb_group_); } resetStatus(); } -void PullOverModule::resetStatus() +void GoalPlannerModule::resetStatus() { PUllOverStatus initial_status{}; status_ = initial_status; @@ -142,7 +143,7 @@ void PullOverModule::resetStatus() } // This function is needed for waiting for planner_data_ -void PullOverModule::updateOccupancyGrid() +void GoalPlannerModule::updateOccupancyGrid() { if (!planner_data_->occupancy_grid) { RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "occupancy_grid is not ready"); @@ -152,7 +153,7 @@ void PullOverModule::updateOccupancyGrid() } // generate pull over candidate paths -void PullOverModule::onTimer() +void GoalPlannerModule::onTimer() { // already generated pull over candidate paths if (!pull_over_path_candidates_.empty()) { @@ -218,7 +219,7 @@ void PullOverModule::onTimer() mutex_.unlock(); } -void PullOverModule::onFreespaceParkingTimer() +void GoalPlannerModule::onFreespaceParkingTimer() { if (!planner_data_) { return; @@ -235,7 +236,7 @@ void PullOverModule::onFreespaceParkingTimer() } } -BehaviorModuleOutput PullOverModule::run() +BehaviorModuleOutput GoalPlannerModule::run() { current_state_ = ModuleStatus::RUNNING; updateOccupancyGrid(); @@ -249,7 +250,7 @@ BehaviorModuleOutput PullOverModule::run() return plan(); } -ParallelParkingParameters PullOverModule::getGeometricPullOverParameters() const +ParallelParkingParameters GoalPlannerModule::getGeometricGoalPlannerParameters() const { ParallelParkingParameters params{}; @@ -271,7 +272,7 @@ ParallelParkingParameters PullOverModule::getGeometricPullOverParameters() const return params; } -void PullOverModule::processOnEntry() +void GoalPlannerModule::processOnEntry() { const auto & route_handler = planner_data_->route_handler; @@ -299,7 +300,7 @@ void PullOverModule::processOnEntry() // initialize when receiving new route if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) { // Initialize parallel parking planner status - parallel_parking_parameters_ = getGeometricPullOverParameters(); + parallel_parking_parameters_ = getGeometricGoalPlannerParameters(); resetStatus(); // calculate goal candidates @@ -318,14 +319,14 @@ void PullOverModule::processOnEntry() last_received_time_ = std::make_unique(route_handler->getRouteHeader().stamp); } -void PullOverModule::processOnExit() +void GoalPlannerModule::processOnExit() { resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); } -bool PullOverModule::isExecutionRequested() const +bool GoalPlannerModule::isExecutionRequested() const { if (current_state_ == ModuleStatus::RUNNING) { return true; @@ -364,7 +365,7 @@ bool PullOverModule::isExecutionRequested() const // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes const lanelet::ConstLanelets pull_over_lanes = - pull_over_utils::getPullOverLanes(*(route_handler), left_side_parking_); + goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); if (!isCrossingPossible(current_lane, target_lane)) { @@ -374,12 +375,12 @@ bool PullOverModule::isExecutionRequested() const return true; } -bool PullOverModule::isExecutionReady() const +bool GoalPlannerModule::isExecutionReady() const { return true; } -double PullOverModule::calcModuleRequestLength() const +double GoalPlannerModule::calcModuleRequestLength() const { const auto min_stop_distance = calcFeasibleDecelDistance(0.0); if (!min_stop_distance) { @@ -392,10 +393,10 @@ double PullOverModule::calcModuleRequestLength() const return std::max(minimum_request_length, parameters_->minimum_request_length); } -Pose PullOverModule::calcRefinedGoal(const Pose & goal_pose) const +Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const { const lanelet::ConstLanelets pull_over_lanes = - pull_over_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); lanelet::Lanelet closest_pull_over_lanelet{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); @@ -439,14 +440,14 @@ Pose PullOverModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } -ModuleStatus PullOverModule::updateState() +ModuleStatus GoalPlannerModule::updateState() { // pull_out module will be run when setting new goal, so not need finishing pull_over module. // Finishing it causes wrong lane_following path generation. return current_state_; } -bool PullOverModule::planFreespacePath() +bool GoalPlannerModule::planFreespacePath() { mutex_.lock(); goal_searcher_->update(goal_candidates_); @@ -475,7 +476,7 @@ bool PullOverModule::planFreespacePath() return false; } -bool PullOverModule::returnToLaneParking() +bool GoalPlannerModule::returnToLaneParking() { if (!status_.lane_parking_pull_over_path) { return false; @@ -504,7 +505,7 @@ bool PullOverModule::returnToLaneParking() return true; } -BehaviorModuleOutput PullOverModule::plan() +BehaviorModuleOutput GoalPlannerModule::plan() { if (allow_goal_modification_) { return planWithGoalModification(); @@ -513,7 +514,7 @@ BehaviorModuleOutput PullOverModule::plan() } } -BehaviorModuleOutput PullOverModule::planWithGoalModification() +BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { const auto & current_pose = planner_data_->self_odometry->pose.pose; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; @@ -525,7 +526,8 @@ BehaviorModuleOutput PullOverModule::planWithGoalModification() resetPathReference(); status_.current_lanes = utils::getExtendedCurrentLanes(planner_data_); - status_.pull_over_lanes = pull_over_utils::getPullOverLanes(*(route_handler), left_side_parking_); + status_.pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); status_.lanes = utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); @@ -740,7 +742,7 @@ BehaviorModuleOutput PullOverModule::planWithGoalModification() // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, - {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, + {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::GOAL_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); // For evaluations @@ -754,12 +756,12 @@ BehaviorModuleOutput PullOverModule::planWithGoalModification() // This const function can not change the menber variables like the goal. // so implement generating candidate path in planWaitingApproval(). // No specific path for the candidate. It's same to the one generated by plan(). -CandidateOutput PullOverModule::planCandidate() const +CandidateOutput GoalPlannerModule::planCandidate() const { return CandidateOutput{}; } -BehaviorModuleOutput PullOverModule::planWaitingApproval() +BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { if (allow_goal_modification_) { return planWaitingApprovalWithGoalModification(); @@ -768,7 +770,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() } } -BehaviorModuleOutput PullOverModule::planWaitingApprovalWithGoalModification() +BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification() { updateOccupancyGrid(); BehaviorModuleOutput out; @@ -794,14 +796,14 @@ BehaviorModuleOutput PullOverModule::planWaitingApprovalWithGoalModification() steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, - {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::PULL_OVER, + {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::GOAL_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); waitApproval(); return out; } -std::pair PullOverModule::calcDistanceToPathChange() const +std::pair GoalPlannerModule::calcDistanceToPathChange() const { const auto & full_path = status_.pull_over_path->getFullPath(); @@ -826,12 +828,12 @@ std::pair PullOverModule::calcDistanceToPathChange() const return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } -void PullOverModule::setParameters(const std::shared_ptr & parameters) +void GoalPlannerModule::setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; } -PathWithLaneId PullOverModule::generateStopPath() +PathWithLaneId GoalPlannerModule::generateStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -899,7 +901,7 @@ PathWithLaneId PullOverModule::generateStopPath() return reference_path; } -PathWithLaneId PullOverModule::generateFeasibleStopPath() +PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -934,7 +936,7 @@ PathWithLaneId PullOverModule::generateFeasibleStopPath() return stop_path; } -bool PullOverModule::incrementPathIndex() +bool GoalPlannerModule::incrementPathIndex() { if (status_.current_path_idx == status_.pull_over_path->partial_paths.size() - 1) { return false; @@ -943,12 +945,12 @@ bool PullOverModule::incrementPathIndex() return true; } -PathWithLaneId PullOverModule::getCurrentPath() const +PathWithLaneId GoalPlannerModule::getCurrentPath() const { return status_.pull_over_path->partial_paths.at(status_.current_path_idx); } -bool PullOverModule::isStopped( +bool GoalPlannerModule::isStopped( std::deque & odometry_buffer, const double time) { odometry_buffer.push_back(planner_data_->self_odometry); @@ -972,12 +974,12 @@ bool PullOverModule::isStopped( return is_stopped; } -bool PullOverModule::isStopped() +bool GoalPlannerModule::isStopped() { return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); } -bool PullOverModule::isStuck() +bool GoalPlannerModule::isStuck() { if (!status_.pull_over_path) { return false; @@ -986,7 +988,7 @@ bool PullOverModule::isStuck() return isStopped(odometry_buffer_stuck_, stuck_time) && checkCollision(getCurrentPath()); } -bool PullOverModule::hasFinishedCurrentPath() +bool GoalPlannerModule::hasFinishedCurrentPath() { const auto & current_path_end = getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; @@ -996,19 +998,19 @@ bool PullOverModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -bool PullOverModule::isOnGoal() const +bool GoalPlannerModule::isOnGoal() const { const auto current_pose = planner_data_->self_odometry->pose.pose; return calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < parameters_->th_arrived_distance; } -bool PullOverModule::hasFinishedPullOver() +bool GoalPlannerModule::hasFinishedPullOver() { return isOnGoal() && isStopped(); } -TurnSignalInfo PullOverModule::calcTurnSignalInfo() const +TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const { TurnSignalInfo turn_signal{}; // output @@ -1040,7 +1042,7 @@ TurnSignalInfo PullOverModule::calcTurnSignalInfo() const return turn_signal; } -bool PullOverModule::checkCollision(const PathWithLaneId & path) const +bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const { if (parameters_->use_occupancy_grid || !occupancy_grid_map_) { const bool check_out_of_range = false; @@ -1060,7 +1062,7 @@ bool PullOverModule::checkCollision(const PathWithLaneId & path) const return false; } -bool PullOverModule::hasEnoughDistance(const PullOverPath & pull_over_path) const +bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; @@ -1089,7 +1091,7 @@ bool PullOverModule::hasEnoughDistance(const PullOverPath & pull_over_path) cons return true; } -void PullOverModule::keepStoppedWithCurrentPath(PathWithLaneId & path) +void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) { constexpr double keep_stop_time = 2.0; constexpr double keep_current_idx_buffer_time = 2.0; @@ -1109,7 +1111,7 @@ void PullOverModule::keepStoppedWithCurrentPath(PathWithLaneId & path) } } -boost::optional PullOverModule::calcFeasibleDecelDistance( +boost::optional GoalPlannerModule::calcFeasibleDecelDistance( const double target_velocity) const { const auto v_now = planner_data_->self_odometry->twist.twist.linear.x; @@ -1133,7 +1135,7 @@ boost::optional PullOverModule::calcFeasibleDecelDistance( return min_stop_distance; } -double PullOverModule::calcSignedArcLengthFromEgo( +double GoalPlannerModule::calcSignedArcLengthFromEgo( const PathWithLaneId & path, const Pose & pose) const { const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -1146,7 +1148,7 @@ double PullOverModule::calcSignedArcLengthFromEgo( path.points, current_pose.position, ego_idx, pose.position, target_idx); } -void PullOverModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const +void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const { const double time = planner_data_->parameters.turn_signal_search_time; const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -1182,7 +1184,7 @@ void PullOverModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLan } } -void PullOverModule::decelerateBeforeSearchStart( +void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & search_start_pose, PathWithLaneId & path) const { const double pull_over_velocity = parameters_->pull_over_velocity; @@ -1198,7 +1200,7 @@ void PullOverModule::decelerateBeforeSearchStart( } } -bool PullOverModule::isCrossingPossible( +bool GoalPlannerModule::isCrossingPossible( const lanelet::ConstLanelet & start_lane, const lanelet::ConstLanelet & end_lane) const { if (start_lane.centerline().empty() || end_lane.centerline().empty()) { @@ -1275,7 +1277,7 @@ bool PullOverModule::isCrossingPossible( return false; } -bool PullOverModule::isCrossingPossible( +bool GoalPlannerModule::isCrossingPossible( const Pose & start_pose, const Pose & end_pose, const lanelet::ConstLanelets lanes) const { lanelet::ConstLanelet start_lane{}; @@ -1287,7 +1289,7 @@ bool PullOverModule::isCrossingPossible( return isCrossingPossible(start_lane, end_lane); } -bool PullOverModule::isCrossingPossible(const PullOverPath & pull_over_path) const +bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.lanes); const Pose & start_pose = pull_over_path.start_pose; @@ -1296,7 +1298,7 @@ bool PullOverModule::isCrossingPossible(const PullOverPath & pull_over_path) con return isCrossingPossible(start_pose, end_pose, lanes); } -void PullOverModule::setDebugData() +void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); @@ -1318,11 +1320,11 @@ void PullOverModule::setDebugData() const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green const double z = refined_goal_pose_.position.z; - add(pull_over_utils::createPullOverAreaMarkerArray( + add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - add(pull_over_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); + add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); } // Visualize path and related pose @@ -1380,7 +1382,7 @@ void PullOverModule::setDebugData() } } -void PullOverModule::printParkingPositionError() const +void GoalPlannerModule::printParkingPositionError() const { const auto current_pose = planner_data_->self_odometry->pose.pose; const double real_shoulder_to_map_shoulder = 0.0; @@ -1398,13 +1400,13 @@ void PullOverModule::printParkingPositionError() const distance_from_real_shoulder); } -bool PullOverModule::checkOriginalGoalIsInShoulder() const +bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const { const auto & route_handler = planner_data_->route_handler; const Pose & goal_pose = route_handler->getGoalPose(); const lanelet::ConstLanelets pull_over_lanes = - pull_over_utils::getPullOverLanes(*(route_handler), left_side_parking_); + goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); @@ -1412,12 +1414,12 @@ bool PullOverModule::checkOriginalGoalIsInShoulder() const lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); } -bool PullOverModule::needPathUpdate(const double path_update_duration) const +bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const { return !isOnGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); } -bool PullOverModule::hasEnoughTimePassedSincePathUpdate(const double duration) const +bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const { if (!last_path_update_time_) { return true; diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp similarity index 84% rename from planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp rename to planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 63903745eadd2..af96313aa0c19 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/pull_over/manager.hpp" +#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include @@ -23,14 +23,14 @@ namespace behavior_path_planner { -PullOverModuleManager::PullOverModuleManager( +GoalPlannerModuleManager::GoalPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters) + const std::shared_ptr & parameters) : SceneModuleManagerInterface(node, name, config, {""}), parameters_{parameters} { } -void PullOverModuleManager::updateModuleParams( +void GoalPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { using tier4_autoware_utils::updateParam; diff --git a/planning/behavior_path_planner/src/utils/pull_over/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp similarity index 95% rename from planning/behavior_path_planner/src/utils/pull_over/default_fixed_goal_planner.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index 3d6adefb87f3f..cd9c370ddb3f1 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/default_fixed_goal_planner.hpp" +#include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp similarity index 95% rename from planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index 783398bca524a..e8f2d6ca3fb8b 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/freespace_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include #include @@ -23,7 +23,7 @@ namespace behavior_path_planner { FreespacePullOver::FreespacePullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info) : PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity} { diff --git a/planning/behavior_path_planner/src/utils/pull_over/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp similarity index 88% rename from planning/behavior_path_planner/src/utils/pull_over/geometric_pull_over.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index ae03590bb11b6..2943a33607bba 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/geometric_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include #include @@ -26,7 +26,7 @@ namespace behavior_path_planner { GeometricPullOver::GeometricPullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const ParallelParkingParameters & parallel_parking_parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr occupancy_grid_map, @@ -45,7 +45,8 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) // prepare road nad shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_); - const auto shoulder_lanes = pull_over_utils::getPullOverLanes(*route_handler, left_side_parking_); + const auto shoulder_lanes = + goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); if (road_lanes.empty() || shoulder_lanes.empty()) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp similarity index 96% rename from planning/behavior_path_planner/src/utils/pull_over/goal_searcher.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 1253ef8480a10..d35842d09f056 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/goal_searcher.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include @@ -31,7 +31,7 @@ using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPose; GoalSearcher::GoalSearcher( - const PullOverParameters & parameters, const LinearRing2d & vehicle_footprint, + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map) : GoalSearcherBase{parameters}, vehicle_footprint_{vehicle_footprint}, @@ -53,7 +53,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; const auto pull_over_lanes = - pull_over_utils::getPullOverLanes(*route_handler, left_side_parking_); + goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); auto lanes = utils::getExtendedCurrentLanes(planner_data_); lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); @@ -142,11 +142,11 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const // check margin with pull over lane objects const auto pull_over_lanes = - pull_over_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); const auto [shoulder_lane_objects, others] = utils::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); constexpr bool filter_inside = true; - const auto target_objects = pull_over_utils::filterObjectsByLateralDistance( + const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, parameters_.object_recognition_collision_check_margin, filter_inside); if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { diff --git a/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 5fb58aa6cc443..562dd93aa2306 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/shift_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_over/util.hpp" #include #include @@ -26,7 +26,7 @@ namespace behavior_path_planner { ShiftPullOver::ShiftPullOver( - rclcpp::Node & node, const PullOverParameters & parameters, + rclcpp::Node & node, const GoalPlannerParameters & parameters, const LaneDepartureChecker & lane_departure_checker, const std::shared_ptr & occupancy_grid_map) : PullOverPlannerBase{node, parameters}, @@ -45,7 +45,8 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) // get road and shoulder lanes const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_); - const auto shoulder_lanes = pull_over_utils::getPullOverLanes(*route_handler, left_side_parking_); + const auto shoulder_lanes = + goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); if (road_lanes.empty() || shoulder_lanes.empty()) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/pull_over/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/pull_over/util.cpp rename to planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 0e501d0a81cfe..cc44e7f0e012d 100644 --- a/planning/behavior_path_planner/src/utils/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_over/util.hpp" +#include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" @@ -42,7 +42,7 @@ using tier4_autoware_utils::createPoint; namespace behavior_path_planner { -namespace pull_over_utils +namespace goal_planner_utils { PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) { @@ -180,5 +180,5 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -} // namespace pull_over_utils +} // namespace goal_planner_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 13a0560ddf303..c1ad5015ca1df 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -61,7 +61,7 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml", behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml", behavior_path_planner_dir + "/config/pull_out/pull_out.param.yaml", - behavior_path_planner_dir + "/config/pull_over/pull_over.param.yaml", + behavior_path_planner_dir + "/config/goal_planner/goal_planner.param.yaml", behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml", behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"}); diff --git a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml index 6dd6755da27d8..06ec042c30a7a 100644 --- a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml +++ b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml @@ -13,7 +13,7 @@ - "lane_change_right" - "avoidance_left" - "avoidance_right" - - "pull_over" + - "goal_planner" - "pull_out" - "intersection_occlusion" @@ -30,6 +30,6 @@ - "lane_change_right" - "avoidance_left" - "avoidance_right" - - "pull_over" + - "goal_planner" - "pull_out" - "intersection_occlusion" diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp index b942a6013e2e2..33970700991c7 100644 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp @@ -51,8 +51,8 @@ Module getModuleType(const std::string & module_name) module.type = Module::AVOIDANCE_LEFT; } else if (module_name == "avoidance_right") { module.type = Module::AVOIDANCE_RIGHT; - } else if (module_name == "pull_over") { - module.type = Module::PULL_OVER; + } else if (module_name == "goal_planner") { + module.type = Module::GOAL_PLANNER; } else if (module_name == "pull_out") { module.type = Module::PULL_OUT; } else if (module_name == "intersection_occlusion") { diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index fefa0b3812c20..a06a023671690 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -64,8 +64,8 @@ Module getModuleType(const std::string & module_name) module.type = Module::AVOIDANCE_LEFT; } else if (module_name == "avoidance_right") { module.type = Module::AVOIDANCE_RIGHT; - } else if (module_name == "pull_over") { - module.type = Module::PULL_OVER; + } else if (module_name == "goal_planner") { + module.type = Module::GOAL_PLANNER; } else if (module_name == "pull_out") { module.type = Module::PULL_OUT; } else if (module_name == "intersection_occlusion") { diff --git a/planning/rtc_replayer/src/rtc_replayer_node.cpp b/planning/rtc_replayer/src/rtc_replayer_node.cpp index 08f037fbfc390..ebc7171fa209b 100644 --- a/planning/rtc_replayer/src/rtc_replayer_node.cpp +++ b/planning/rtc_replayer/src/rtc_replayer_node.cpp @@ -47,8 +47,8 @@ std::string getModuleName(const uint8_t module_type) case Module::AVOIDANCE_RIGHT: { return "avoidance_right"; } - case Module::PULL_OVER: { - return "pull_over"; + case Module::GOAL_PLANNER: { + return "goal_planner"; } case Module::PULL_OUT: { return "pull_out"; diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index 57daa8f840428..d464197e03603 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -85,7 +85,7 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning std::vector steering_factor_topics = { "/planning/steering_factor/avoidance", "/planning/steering_factor/intersection", "/planning/steering_factor/lane_change", "/planning/steering_factor/pull_out", - "/planning/steering_factor/pull_over"}; + "/planning/steering_factor/goal_planner"}; sub_velocity_factors_ = init_factors(this, velocity_factors_, velocity_factor_topics); diff --git a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py index 194551d56dcd5..7bfa72101ade4 100755 --- a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py +++ b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py @@ -40,7 +40,7 @@ SteeringFactor.AVOIDANCE_PATH_RETURN: "avoidance return", SteeringFactor.STATION: "station", SteeringFactor.PULL_OUT: "pull out", - SteeringFactor.PULL_OVER: "pull over", + SteeringFactor.GOAL_PLANNER: "goal planner", SteeringFactor.EMERGENCY_OPERATION: "emergency operation", } From 2e7181cccb1812c02f863471ff66ce8d1d8611b6 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 26 Apr 2023 14:07:56 +0900 Subject: [PATCH 2/2] feat(behaivor_path_planner): output virtual wall (#3537) * feat(behavior_path_planner): add function to publish virtual wall Signed-off-by: satoshi-ota * feat(avoidance): use interface function to publish virtual wall Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner/planner_manager.hpp | 9 ++ .../scene_module/scene_module_interface.hpp | 90 ++++++++++++++++++- .../scene_module_manager_interface.hpp | 43 +++++++++ .../utils/avoidance/avoidance_module_data.hpp | 4 - .../src/behavior_path_planner_node.cpp | 1 + .../src/behavior_tree_manager.cpp | 1 + .../avoidance/avoidance_module.cpp | 44 ++------- 7 files changed, 149 insertions(+), 43 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 16af2770c600d..a7d19294586fd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -105,6 +105,15 @@ class PlannerManager manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishDebugMarker(); }); } + /** + * @brief publish all registered modules' virtual wall. + */ + void publishVirtualWall() const + { + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->publishVirtualWall(); }); + } + /** * @brief get manager pointers. * @return manager pointers. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 28b55aa3c196d..5bf18483ae5e6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -44,8 +44,13 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::SteeringFactor; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using motion_utils::createDeadLineVirtualWallMarker; +using motion_utils::createSlowDownVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; using rtc_interface::RTCInterface; using steering_factor_interface::SteeringFactorInterface; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using unique_identifier_msgs::msg::UUID; @@ -69,8 +74,15 @@ class SceneModuleInterface std::make_unique(&node, utils::convertToSnakeCase(name))) { #ifdef USE_OLD_ARCHITECTURE - const auto ns = std::string("~/debug/") + utils::convertToSnakeCase(name); - pub_debug_marker_ = node.create_publisher(ns, 20); + { + const auto ns = std::string("~/debug/") + utils::convertToSnakeCase(name); + pub_debug_marker_ = node.create_publisher(ns, 20); + } + + { + const auto ns = std::string("~/virtual_wall/") + utils::convertToSnakeCase(name); + pub_virtual_wall_ = node.create_publisher(ns, 20); + } #endif for (auto itr = rtc_interface_ptr_map_.begin(); itr != rtc_interface_ptr_map_.end(); ++itr) { @@ -259,6 +271,36 @@ class SceneModuleInterface #ifdef USE_OLD_ARCHITECTURE void publishDebugMarker() { pub_debug_marker_->publish(debug_marker_); } + + void publishVirtualWall() + { + MarkerArray markers{}; + + const auto opt_stop_pose = getStopPose(); + if (!!opt_stop_pose) { + const auto virtual_wall = createStopVirtualWallMarker( + opt_stop_pose.get(), utils::convertToSnakeCase(name()), rclcpp::Clock().now(), 0); + appendMarkerArray(virtual_wall, &markers); + } + + const auto opt_slow_pose = getSlowPose(); + if (!!opt_slow_pose) { + const auto virtual_wall = createSlowDownVirtualWallMarker( + opt_slow_pose.get(), utils::convertToSnakeCase(name()), rclcpp::Clock().now(), 0); + appendMarkerArray(virtual_wall, &markers); + } + + const auto opt_dead_pose = getDeadPose(); + if (!!opt_dead_pose) { + const auto virtual_wall = createDeadLineVirtualWallMarker( + opt_dead_pose.get(), utils::convertToSnakeCase(name()), rclcpp::Clock().now(), 0); + appendMarkerArray(virtual_wall, &markers); + } + + pub_virtual_wall_->publish(markers); + + resetWallPoses(); + } #endif bool isWaitingApproval() const { return is_waiting_approval_; } @@ -281,6 +323,43 @@ class SceneModuleInterface std::string name() const { return name_; } + boost::optional getStopPose() const + { + if (!stop_pose_) { + return {}; + } + + const auto & base_link2front = planner_data_->parameters.base_link2front; + return calcOffsetPose(stop_pose_.get(), base_link2front, 0.0, 0.0); + } + + boost::optional getSlowPose() const + { + if (!slow_pose_) { + return {}; + } + + const auto & base_link2front = planner_data_->parameters.base_link2front; + return calcOffsetPose(slow_pose_.get(), base_link2front, 0.0, 0.0); + } + + boost::optional getDeadPose() const + { + if (!dead_pose_) { + return {}; + } + + const auto & base_link2front = planner_data_->parameters.base_link2front; + return calcOffsetPose(dead_pose_.get(), base_link2front, 0.0, 0.0); + } + + void resetWallPoses() + { + stop_pose_ = boost::none; + slow_pose_ = boost::none; + dead_pose_ = boost::none; + } + rclcpp::Logger getLogger() const { return logger_; } private: @@ -290,6 +369,7 @@ class SceneModuleInterface #ifdef USE_OLD_ARCHITECTURE rclcpp::Publisher::SharedPtr pub_debug_marker_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; #endif BehaviorModuleOutput previous_module_output_; @@ -384,6 +464,12 @@ class SceneModuleInterface std::unique_ptr steering_factor_interface_ptr_; + mutable boost::optional stop_pose_{boost::none}; + + mutable boost::optional slow_pose_{boost::none}; + + mutable boost::optional dead_pose_{boost::none}; + mutable MarkerArray debug_marker_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index d0dfb84d674f6..665c9eaf7c802 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -32,6 +32,9 @@ namespace behavior_path_planner { +using motion_utils::createDeadLineVirtualWallMarker; +using motion_utils::createSlowDownVirtualWallMarker; +using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::toHexString; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; @@ -62,6 +65,7 @@ class SceneModuleManagerInterface } pub_debug_marker_ = node->create_publisher("~/debug/" + name, 20); + pub_virtual_wall_ = node->create_publisher("~/virtual_wall/" + name, 20); } virtual ~SceneModuleManagerInterface() = default; @@ -112,6 +116,43 @@ class SceneModuleManagerInterface pub_debug_marker_->publish(MarkerArray{}); } + void publishVirtualWall() const + { + using tier4_autoware_utils::appendMarkerArray; + + MarkerArray markers{}; + + const auto marker_offset = std::numeric_limits::max(); + + uint32_t marker_id = marker_offset; + for (const auto & m : registered_modules_) { + const auto opt_stop_pose = m->getStopPose(); + if (!!opt_stop_pose) { + const auto virtual_wall = createStopVirtualWallMarker( + opt_stop_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + appendMarkerArray(virtual_wall, &markers); + } + + const auto opt_slow_pose = m->getSlowPose(); + if (!!opt_slow_pose) { + const auto virtual_wall = createSlowDownVirtualWallMarker( + opt_slow_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + appendMarkerArray(virtual_wall, &markers); + } + + const auto opt_dead_pose = m->getDeadPose(); + if (!!opt_dead_pose) { + const auto virtual_wall = createDeadLineVirtualWallMarker( + opt_dead_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + appendMarkerArray(virtual_wall, &markers); + } + + m->resetWallPoses(); + } + + pub_virtual_wall_->publish(markers); + } + void publishDebugMarker() const { using tier4_autoware_utils::appendMarkerArray; @@ -190,6 +231,8 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_debug_marker_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; + std::string name_; std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 13c851f8ec92c..c1fc19db51658 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -498,10 +498,6 @@ struct DebugData std::vector total_shift; std::vector output_shift; - boost::optional stop_pose{boost::none}; - boost::optional slow_pose{boost::none}; - boost::optional feasible_bound{boost::none}; - bool exist_adjacent_objects{false}; // future pose diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 5a78759ea9b56..18c48c5e22975 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1127,6 +1127,7 @@ void BehaviorPathPlannerNode::run() #ifndef USE_OLD_ARCHITECTURE planner_manager_->print(); planner_manager_->publishDebugMarker(); + planner_manager_->publishVirtualWall(); lk_manager.unlock(); // release planner_manager_ #endif diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index a4214f816a86a..560e035c1697c 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -102,6 +102,7 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) { m->publishDebugMarker(); + m->publishVirtualWall(); if (!m->isExecutionRequested()) { m->onExit(); } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 244de59eafcaa..a5ab3f85182f4 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -53,7 +53,6 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcInterpolatedPose; using tier4_autoware_utils::calcLateralDeviation; using tier4_autoware_utils::calcLongitudinalDeviation; -using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; @@ -507,13 +506,11 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat getNominalPrepareDistance() + object_parameter.safety_buffer_longitudinal + base_link2front; const auto total_avoid_distance = variable + constant; - const auto opt_feasible_bound = calcLongitudinalOffsetPose( + dead_pose_ = calcLongitudinalOffsetPose( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); - if (opt_feasible_bound) { - debug.feasible_bound = opt_feasible_bound.get(); - } else { - debug.feasible_bound = getPose(data.reference_path.points.front()); + if (!dead_pose_) { + dead_pose_ = getPose(data.reference_path.points.front()); } } @@ -3252,14 +3249,9 @@ void AvoidanceModule::setDebugData( using marker_utils::avoidance_marker::createUnavoidableTargetObjectsMarkerArray; using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; - using motion_utils::createDeadLineVirtualWallMarker; - using motion_utils::createSlowDownVirtualWallMarker; - using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; - using tier4_autoware_utils::calcOffsetPose; debug_marker_.markers.clear(); - const auto & base_link2front = planner_data_->parameters.base_link2front; const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); const auto add = [this](const MarkerArray & added) { @@ -3286,24 +3278,6 @@ void AvoidanceModule::setDebugData( add(createPathMarkerArray(prev_linear_shift_path_.path, "prev_linear_shift", 0, 0.5, 0.4, 0.6)); add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3)); - if (debug.stop_pose) { - const auto p_front = calcOffsetPose(debug.stop_pose.get(), base_link2front, 0.0, 0.0); - appendMarkerArray( - createStopVirtualWallMarker(p_front, "avoidance stop", current_time, 0L), &debug_marker_); - } - - if (debug.slow_pose) { - const auto p_front = calcOffsetPose(debug.slow_pose.get(), base_link2front, 0.0, 0.0); - appendMarkerArray( - createSlowDownVirtualWallMarker(p_front, "avoidance slow", current_time, 0L), &debug_marker_); - } - - if (debug.feasible_bound) { - const auto p_front = calcOffsetPose(debug.feasible_bound.get(), base_link2front, 0.0, 0.0); - appendMarkerArray( - createDeadLineVirtualWallMarker(p_front, "feasible bound", current_time, 0L), &debug_marker_); - } - add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug)); std::vector avoidable_target_objects; @@ -3465,16 +3439,14 @@ void AvoidanceModule::insertWaitPoint( std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance); if (!use_constraints_for_decel) { - insertDecelPoint( - getEgoPosition(), start_longitudinal, 0.0, shifted_path.path, debug_data_.stop_pose); + insertDecelPoint(getEgoPosition(), start_longitudinal, 0.0, shifted_path.path, stop_pose_); return; } const auto stop_distance = getMildDecelDistance(0.0); if (stop_distance) { const auto insert_distance = std::max(start_longitudinal, *stop_distance); - insertDecelPoint( - getEgoPosition(), insert_distance, 0.0, shifted_path.path, debug_data_.stop_pose); + insertDecelPoint(getEgoPosition(), insert_distance, 0.0, shifted_path.path, stop_pose_); } } @@ -3494,8 +3466,7 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const const auto decel_distance = getMildDecelDistance(p->yield_velocity); if (decel_distance) { insertDecelPoint( - getEgoPosition(), *decel_distance, p->yield_velocity, shifted_path.path, - debug_data_.slow_pose); + getEgoPosition(), *decel_distance, p->yield_velocity, shifted_path.path, slow_pose_); } } @@ -3523,8 +3494,7 @@ void AvoidanceModule::insertPrepareVelocity(const bool avoidable, ShiftedPath & const auto decel_distance = getFeasibleDecelDistance(0.0); if (decel_distance) { - insertDecelPoint( - getEgoPosition(), *decel_distance, 0.0, shifted_path.path, debug_data_.slow_pose); + insertDecelPoint(getEgoPosition(), *decel_distance, 0.0, shifted_path.path, slow_pose_); } }