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 5e795ae764092..ca5099112741f 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -66,8 +66,8 @@ std::string getModuleName(const uint8_t module_type) case Module::GOAL_PLANNER: { return "goal_planner"; } - case Module::PULL_OUT: { - return "pull_out"; + case Module::START_PLANNER: { + return "start_planner"; } case Module::TRAFFIC_LIGHT: { return "traffic_light"; @@ -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::GOAL_PLANNER || module_type == Module::PULL_OUT) { + module_type == Module::GOAL_PLANNER || module_type == Module::START_PLANNER) { 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 ce21d7fefc797..3e95afe247c86 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 @@ -232,8 +232,8 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: case SteeringFactor::STATION: label->setText("STATION"); break; - case SteeringFactor::PULL_OUT: - label->setText("PULL_OUT"); + case SteeringFactor::START_PLANNER: + label->setText("START_PLANNER"); break; case SteeringFactor::GOAL_PLANNER: label->setText("GOAL_PLANNER"); diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index df99552ac4fdc..bbe13c46eb552 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -24,7 +24,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 affe9ac6af3e0..e1c161a462d1e 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 @@ -55,8 +55,8 @@ def launch_setup(context, *args, **kwargs): lane_change_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("start_planner_param_path").perform(context), "r") as f: + start_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f: drivable_area_expansion_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("scene_module_manager_param_path").perform(context), "r") as f: @@ -96,7 +96,7 @@ def launch_setup(context, *args, **kwargs): dynamic_avoidance_param, lane_change_param, goal_planner_param, - pull_out_param, + start_planner_param, drivable_area_expansion_param, scene_module_manager_param, behavior_path_planner_param, diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 69deacf4dfb7f..4de0f91c53c6a 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -9,7 +9,7 @@ Zulfaqar Azmi Kosuke Takeuchi - + Kosuke Takeuchi Fumiya Watanabe diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz index 2685bf96eb881..9a390d10895f8 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz @@ -1225,7 +1225,7 @@ Visualization Manager: Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false - Name: PullOut + Name: StartPlanner Namespaces: {} Topic: @@ -1233,7 +1233,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullout + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz index a744e1ffdeb46..8ec93bb5b50d6 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz @@ -1226,7 +1226,7 @@ Visualization Manager: Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false - Name: PullOut + Name: StartPlanner Namespaces: {} Topic: @@ -1234,7 +1234,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullout + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 67aec896c9053..4463620dcc4a9 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -15,7 +15,7 @@ set(common_src src/scene_module/scene_module_visitor.cpp src/scene_module/avoidance/avoidance_module.cpp src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp - src/scene_module/pull_out/pull_out_module.cpp + src/scene_module/start_planner/start_planner_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 @@ -35,9 +35,9 @@ set(common_src 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 + src/utils/start_planner/util.cpp + src/utils/start_planner/shift_pull_out.cpp + src/utils/start_planner/geometric_pull_out.cpp src/utils/path_shifter/path_shifter.cpp src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp @@ -67,7 +67,7 @@ else() src/planner_manager.cpp src/scene_module/avoidance/manager.cpp src/scene_module/dynamic_avoidance/manager.cpp - src/scene_module/pull_out/manager.cpp + src/scene_module/start_planner/manager.cpp src/scene_module/goal_planner/manager.cpp src/scene_module/side_shift/manager.cpp src/scene_module/lane_change/manager.cpp diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 47ccc9e58674a..8bf46fb3d3ae7 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 | -| 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) | +| 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_start_planner_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) diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml index 2538633e63bf0..d2b2a6a79bf0c 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml @@ -8,8 +8,8 @@ - - + + @@ -57,10 +57,10 @@ desc - + - + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml index 65c600d246c59..8988dd41b50fb 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only.xml @@ -8,8 +8,8 @@ - - + + @@ -62,10 +62,10 @@ desc - + - + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml index 46b2492cca9fb..46b2c4a8a3f33 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_lane_change_only_old.xml @@ -9,9 +9,9 @@ - - - + + + @@ -73,11 +73,11 @@ - + - - + + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml index c39ef30770b16..97601698c4eb7 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_old.xml @@ -9,9 +9,9 @@ - - - + + + @@ -83,11 +83,11 @@ - + - - + + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_pull_out.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_pull_out.xml index 70d6b37cb61e0..32f30af69ac36 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_pull_out.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_pull_out.xml @@ -9,9 +9,9 @@ - - - + + + @@ -73,11 +73,11 @@ - + - - + + desc diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml index 80fe7ea8dac49..0410f775f7d17 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml +++ b/planning/behavior_path_planner/config/behavior_path_planner_tree_with_external_request_LC.xml @@ -8,8 +8,8 @@ - - + + @@ -57,10 +57,10 @@ desc - + - + desc 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 30626a4df30c4..3040566644ddb 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -31,7 +31,7 @@ priority: 5 max_module_size: 1 - pull_out: + start_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/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml similarity index 98% rename from planning/behavior_path_planner/config/pull_out/pull_out.param.yaml rename to planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index bd1ccd2065ee9..0d8782cb501b6 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - pull_out: + start_planner: th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_pull_out_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md similarity index 98% rename from planning/behavior_path_planner/docs/behavior_path_planner_pull_out_design.md rename to planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 027043af33b3e..1c3b91851fb14 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_pull_out_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -1,4 +1,4 @@ -# Pull Out design +# Start Planner design ## Purpose / Role @@ -8,7 +8,7 @@ Pull out from the shoulder lane without colliding with objects. ```plantuml @startuml -package pull_out{ +package start_planner{ abstract class PullOutPlannerBase { } @@ -19,7 +19,7 @@ package pull_out{ class GeometricPullOut { } - class PullOutModule { + class StartPlannerModule { } struct PullOutPath{} @@ -40,14 +40,14 @@ GeometricPullOut --|> PullOutPlannerBase PathShifter --o ShiftPullOut GeometricParallelParking --o GeometricPullOut -PullOutPlannerBase --o PullOutModule +PullOutPlannerBase --o StartPlannerModule PullOutPath --o PullOutPlannerBase @enduml ``` -## General parameters for pull_out +## General parameters for start_planner | Name | Unit | Type | Description | Default value | | :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | 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 a69e6dde1c364..c08a626d4375a 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 @@ -24,16 +24,16 @@ #include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" #include "behavior_path_planner/scene_module/goal_planner/goal_planner_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/side_shift/side_shift_module.hpp" +#include "behavior_path_planner/scene_module/start_planner/start_planner_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/dynamic_avoidance/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/side_shift/manager.hpp" +#include "behavior_path_planner/scene_module/start_planner/manager.hpp" #endif #include "behavior_path_planner/steering_factor_interface.hpp" @@ -41,8 +41,8 @@ #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/side_shift/side_shift_parameters.hpp" +#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include @@ -152,7 +152,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr dynamic_avoidance_param_ptr_; std::shared_ptr side_shift_param_ptr_; std::shared_ptr lane_change_param_ptr_; - std::shared_ptr pull_out_param_ptr_; + std::shared_ptr start_planner_param_ptr_; std::shared_ptr goal_planner_param_ptr_; BehaviorPathPlannerParameters getCommonParam(); @@ -166,7 +166,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node LaneChangeParameters getLaneChangeParam(); SideShiftParameters getSideShiftParam(); GoalPlannerParameters getGoalPlannerParam(); - PullOutParameters getPullOutParam(); + StartPlannerParameters getStartPlannerParam(); AvoidanceByLCParameters getAvoidanceByLCParam( const std::shared_ptr & avoidance_param, const std::shared_ptr & lane_change_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 625881e0d6e8e..6481f9e43d668 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -79,7 +79,7 @@ struct BehaviorPathPlannerParameters ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; ModuleConfigParameters config_dynamic_avoidance; - ModuleConfigParameters config_pull_out; + ModuleConfigParameters config_start_planner; ModuleConfigParameters config_goal_planner; ModuleConfigParameters config_side_shift; ModuleConfigParameters config_lane_change_left; 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 03473709446c3..03672aa59f6b9 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 @@ -33,7 +33,7 @@ class ExternalRequestLaneChangeModule; class LaneChangeInterface; class LaneChangeBTInterface; class LaneFollowingModule; -class PullOutModule; +class StartPlannerModule; class GoalPlannerModule; class SideShiftModule; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp similarity index 61% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/manager.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp index 5591da525bc36..09c201b0b38ce 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" +#include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" #include @@ -27,26 +27,26 @@ namespace behavior_path_planner { -class PullOutModuleManager : public SceneModuleManagerInterface +class StartPlannerModuleManager : public SceneModuleManagerInterface { public: - PullOutModuleManager( + StartPlannerModuleManager( 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_OUT__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp similarity index 78% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 115a389e9b44c..3f6ea8f5b3799 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_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/path_shifter/path_shifter.hpp" -#include "behavior_path_planner/utils/pull_out/geometric_pull_out.hpp" -#include "behavior_path_planner/utils/pull_out/pull_out_parameters.hpp" -#include "behavior_path_planner/utils/pull_out/pull_out_path.hpp" -#include "behavior_path_planner/utils/pull_out/shift_pull_out.hpp" +#include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" +#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" #include #include @@ -63,20 +63,20 @@ struct PullOutStatus PullOutStatus() {} }; -class PullOutModule : public SceneModuleInterface +class StartPlannerModule : public SceneModuleInterface { public: #ifdef USE_OLD_ARCHITECTURE - PullOutModule( + StartPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters); + const std::shared_ptr & parameters); #else - PullOutModule( + StartPlannerModule( 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; } @@ -93,7 +93,7 @@ class PullOutModule : public SceneModuleInterface CandidateOutput planCandidate() const override; void processOnExit() override; - void setParameters(const std::shared_ptr & parameters) + void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; } @@ -105,10 +105,10 @@ class PullOutModule : public SceneModuleInterface } private: - std::shared_ptr parameters_; + std::shared_ptr parameters_; vehicle_info_util::VehicleInfo vehicle_info_; - std::vector> pull_out_planners_; + std::vector> start_planner_planners_; PullOutStatus status_; std::deque odometry_buffer_; @@ -129,10 +129,9 @@ class PullOutModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; - void planWithPriorityOnEfficientPath( - const std::vector & start_pose_candidates, const Pose & goal_pose); - void planWithPriorityOnShortBackDistance( - const std::vector & start_pose_candidates, const Pose & goal_pose); + void planWithPriority( + const std::vector & start_pose_candidates, const Pose & goal_pose, + const std::string search_priority); PathWithLaneId generateStopPath() const; void updatePullOutStatus(); static bool isOverlappedWithLane( @@ -152,4 +151,4 @@ class PullOutModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp similarity index 70% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp index 0ab6bd465fb26..7ace770dc7ff1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.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_OUT__GEOMETRIC_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__GEOMETRIC_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__GEOMETRIC_PULL_OUT_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/pull_out/pull_out_path.hpp" -#include "behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" #include @@ -26,7 +26,7 @@ namespace behavior_path_planner class GeometricPullOut : public PullOutPlannerBase { public: - explicit GeometricPullOut(rclcpp::Node & node, const PullOutParameters & parameters); + explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; boost::optional plan(Pose start_pose, Pose goal_pose) override; @@ -36,4 +36,4 @@ class GeometricPullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__GEOMETRIC_PULL_OUT_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__GEOMETRIC_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp similarity index 81% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_path.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp index 1030c81ad6668..d68985ec27350 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.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_OUT__PULL_OUT_PATH_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PATH_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" @@ -31,4 +31,4 @@ struct PullOutPath Pose end_pose{}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PATH_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp similarity index 77% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 955e856061310..b95d247b6d23f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_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_out/pull_out_parameters.hpp" -#include "behavior_path_planner/utils/pull_out/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" #include #include @@ -44,7 +44,7 @@ enum class PlannerType { class PullOutPlannerBase { public: - explicit PullOutPlannerBase(rclcpp::Node & node, const PullOutParameters & parameters) + explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); vehicle_footprint_ = createVehicleFootprint(vehicle_info_); @@ -64,8 +64,8 @@ class PullOutPlannerBase std::shared_ptr planner_data_; vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; - PullOutParameters parameters_; + StartPlannerParameters parameters_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp similarity index 78% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/shift_pull_out.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index ac5aa89ea675b..7703c3edf4250 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.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_OUT__SHIFT_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__SHIFT_PULL_OUT_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__SHIFT_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__SHIFT_PULL_OUT_HPP_ -#include "behavior_path_planner/utils/pull_out/pull_out_path.hpp" -#include "behavior_path_planner/utils/pull_out/pull_out_planner_base.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" #include @@ -33,7 +33,7 @@ class ShiftPullOut : public PullOutPlannerBase { public: explicit ShiftPullOut( - rclcpp::Node & node, const PullOutParameters & parameters, + rclcpp::Node & node, const StartPlannerParameters & parameters, std::shared_ptr & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; @@ -43,7 +43,7 @@ class ShiftPullOut : public PullOutPlannerBase const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose, const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::PullOutParameters & parameter); + const behavior_path_planner::StartPlannerParameters & parameter); bool hasEnoughDistance( const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes, @@ -56,4 +56,4 @@ class ShiftPullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__SHIFT_PULL_OUT_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__SHIFT_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp similarity index 85% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index bc12812424561..e76cfcfd88081 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_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_OUT__PULL_OUT_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" @@ -23,7 +23,7 @@ namespace behavior_path_planner { -struct PullOutParameters +struct StartPlannerParameters { double th_arrived_distance; double th_stopped_velocity; @@ -55,4 +55,4 @@ struct PullOutParameters } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__PULL_OUT_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp similarity index 85% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/util.hpp rename to planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 314a4c9426cab..708433fbfe239 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.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_OUT__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ #include "behavior_path_planner/data_manager.hpp" @@ -30,7 +30,7 @@ #include #include -namespace behavior_path_planner::pull_out_utils +namespace behavior_path_planner::start_planner_utils { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; @@ -46,6 +46,6 @@ PathWithLaneId getBackwardPath( lanelet::ConstLanelets getPullOutLanes(const std::shared_ptr & planner_data); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); -} // namespace behavior_path_planner::pull_out_utils +} // namespace behavior_path_planner::start_planner_utils -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OUT__UTIL_HPP_ +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 87bced439ce46..baff842954be6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -19,7 +19,7 @@ #include "behavior_path_planner/marker_util/debug_utilities.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_path.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "motion_utils/motion_utils.hpp" #include "perception_utils/predicted_path_utils.hpp" diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index b5585151869df..cd7311f3308bb 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -9,7 +9,7 @@ Zulfaqar Azmi Kosuke Takeuchi - + Kosuke Takeuchi Fumiya Watanabe 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 c4bf8387b859f..51587e7efa286 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -125,7 +125,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod dynamic_avoidance_param_ptr_ = std::make_shared(getDynamicAvoidanceParam()); lane_change_param_ptr_ = std::make_shared(getLaneChangeParam()); - pull_out_param_ptr_ = std::make_shared(getPullOutParam()); + start_planner_param_ptr_ = std::make_shared(getStartPlannerParam()); goal_planner_param_ptr_ = std::make_shared(getGoalPlannerParam()); side_shift_param_ptr_ = std::make_shared(getSideShiftParam()); avoidance_by_lc_param_ptr_ = std::make_shared( @@ -186,10 +186,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "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_); + auto start_planner_module = + std::make_shared("StartPlanner", *this, start_planner_param_ptr_); path_candidate_publishers_.emplace( - "PullOut", create_publisher(path_candidate_name_space + "pull_out", 1)); - bt_manager_->registerSceneModule(pull_out_module); + "StartPlanner", create_publisher(path_candidate_name_space + "start_planner", 1)); + bt_manager_->registerSceneModule(start_planner_module); bt_manager_->createBehaviorTree(); } @@ -214,14 +215,14 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod module_name, create_publisher(path_reference_name_space + module_name, 1)); }; - if (p.config_pull_out.enable_module) { - auto manager = std::make_shared( - this, "pull_out", p.config_pull_out, pull_out_param_ptr_); + if (p.config_start_planner.enable_module) { + auto manager = std::make_shared( + this, "start_planner", p.config_start_planner, start_planner_param_ptr_); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( - "pull_out", create_publisher(path_candidate_name_space + "pull_out", 1)); + "start_planner", create_publisher(path_candidate_name_space + "start_planner", 1)); path_reference_publishers_.emplace( - "pull_out", create_publisher(path_reference_name_space + "pull_out", 1)); + "start_planner", create_publisher(path_reference_name_space + "start_planner", 1)); } if (p.config_goal_planner.enable_module) { @@ -348,7 +349,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() return config; }; - p.config_pull_out = get_scene_module_manager_param("pull_out."); + p.config_start_planner = get_scene_module_manager_param("start_planner."); 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."); @@ -1010,11 +1011,11 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() return p; } -PullOutParameters BehaviorPathPlannerNode::getPullOutParam() +StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam() { - PullOutParameters p; + StartPlannerParameters p; - std::string ns = "pull_out."; + std::string ns = "start_planner."; p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index b4d7537ce7c80..cd7ca2540352b 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -417,7 +417,7 @@ ModuleStatus GoalPlannerModule::updateState() current_state_ = ModuleStatus::SUCCESS; } - // pull_out module will be run when setting new goal, so not need finishing pull_over module. + // start_planner 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_; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp similarity index 83% rename from planning/behavior_path_planner/src/scene_module/pull_out/manager.cpp rename to planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 9084f461d0125..ab33c78c945ab 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_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_out/manager.hpp" +#include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include @@ -23,14 +23,14 @@ namespace behavior_path_planner { -PullOutModuleManager::PullOutModuleManager( +StartPlannerModuleManager::StartPlannerModuleManager( 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 PullOutModuleManager::updateModuleParams( +void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { using tier4_autoware_utils::updateParam; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp similarity index 80% rename from planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp rename to planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 169189a10f330..3727bbd27c6f3 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp" +#include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_out/util.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -36,9 +36,9 @@ using tier4_autoware_utils::inverseTransformPoint; namespace behavior_path_planner { #ifdef USE_OLD_ARCHITECTURE -PullOutModule::PullOutModule( +StartPlannerModule::StartPlannerModule( 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()} @@ -48,21 +48,21 @@ PullOutModule::PullOutModule( // set enabled planner if (parameters_->enable_shift_pull_out) { - pull_out_planners_.push_back( + start_planner_planners_.push_back( std::make_shared(node, *parameters, lane_departure_checker_)); } if (parameters_->enable_geometric_pull_out) { - pull_out_planners_.push_back(std::make_shared(node, *parameters)); + start_planner_planners_.push_back(std::make_shared(node, *parameters)); } - if (pull_out_planners_.empty()) { + if (start_planner_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } } #else -PullOutModule::PullOutModule( +StartPlannerModule::StartPlannerModule( const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::unordered_map > & rtc_interface_ptr_map) + const std::shared_ptr & parameters, + const std::unordered_map> & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} @@ -72,19 +72,19 @@ PullOutModule::PullOutModule( // set enabled planner if (parameters_->enable_shift_pull_out) { - pull_out_planners_.push_back( + start_planner_planners_.push_back( std::make_shared(node, *parameters, lane_departure_checker_)); } if (parameters_->enable_geometric_pull_out) { - pull_out_planners_.push_back(std::make_shared(node, *parameters)); + start_planner_planners_.push_back(std::make_shared(node, *parameters)); } - if (pull_out_planners_.empty()) { + if (start_planner_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } } #endif -BehaviorModuleOutput PullOutModule::run() +BehaviorModuleOutput StartPlannerModule::run() { current_state_ = ModuleStatus::RUNNING; @@ -97,13 +97,13 @@ BehaviorModuleOutput PullOutModule::run() return plan(); } -void PullOutModule::processOnExit() +void StartPlannerModule::processOnExit() { resetPathCandidate(); resetPathReference(); } -bool PullOutModule::isExecutionRequested() const +bool StartPlannerModule::isExecutionRequested() const { has_received_new_route_ = !planner_data_->prev_route_id || @@ -143,7 +143,7 @@ bool PullOutModule::isExecutionRequested() const // Check if ego is not out of lanes const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_); - const auto pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_); + const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_); auto lanes = current_lanes; lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { @@ -159,15 +159,15 @@ bool PullOutModule::isExecutionRequested() const return true; } -bool PullOutModule::isExecutionReady() const +bool StartPlannerModule::isExecutionReady() const { return true; } // this runs only when RUNNING -ModuleStatus PullOutModule::updateState() +ModuleStatus StartPlannerModule::updateState() { - RCLCPP_DEBUG(getLogger(), "PULL_OUT updateState"); + RCLCPP_DEBUG(getLogger(), "START_PLANNER updateState"); if (hasFinishedPullOut()) { current_state_ = ModuleStatus::SUCCESS; @@ -179,7 +179,7 @@ ModuleStatus PullOutModule::updateState() return current_state_; } -BehaviorModuleOutput PullOutModule::plan() +BehaviorModuleOutput StartPlannerModule::plan() { if (isWaitingApproval()) { clearWaitingApproval(); @@ -254,7 +254,7 @@ BehaviorModuleOutput PullOutModule::plan() // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, SteeringFactor::PULL_OUT, steering_factor_direction, + {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); } else { const double distance = motion_utils::calcSignedArcLength( @@ -264,7 +264,7 @@ BehaviorModuleOutput PullOutModule::plan() // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - SteeringFactor::PULL_OUT, steering_factor_direction, SteeringFactor::TURNING, ""); + SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); } setDebugData(); @@ -272,14 +272,14 @@ BehaviorModuleOutput PullOutModule::plan() return output; } -CandidateOutput PullOutModule::planCandidate() const +CandidateOutput StartPlannerModule::planCandidate() const { return CandidateOutput{}; } -std::shared_ptr PullOutModule::getCurrentPlanner() const +std::shared_ptr StartPlannerModule::getCurrentPlanner() const { - for (const auto & planner : pull_out_planners_) { + for (const auto & planner : start_planner_planners_) { if (status_.planner_type == planner->getPlannerType()) { return planner; } @@ -287,7 +287,7 @@ std::shared_ptr PullOutModule::getCurrentPlanner() const return nullptr; } -PathWithLaneId PullOutModule::getFullPath() const +PathWithLaneId StartPlannerModule::getFullPath() const { const auto pull_out_planner = getCurrentPlanner(); if (pull_out_planner == nullptr) { @@ -313,7 +313,7 @@ PathWithLaneId PullOutModule::getFullPath() const return full_path; } -BehaviorModuleOutput PullOutModule::planWaitingApproval() +BehaviorModuleOutput StartPlannerModule::planWaitingApproval() { updatePullOutStatus(); waitApproval(); @@ -332,7 +332,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() } const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_); - const auto pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_); + const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_); auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; const auto drivable_lanes = utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_out_lanes); @@ -377,7 +377,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() updateRTCStatus(start_distance, finish_distance); steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, SteeringFactor::PULL_OUT, steering_factor_direction, + {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); } else { const double distance = motion_utils::calcSignedArcLength( @@ -386,7 +386,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() updateRTCStatus(0.0, distance); steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - SteeringFactor::PULL_OUT, steering_factor_direction, SteeringFactor::APPROACHING, ""); + SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); } setDebugData(); @@ -394,25 +394,26 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() return output; } -void PullOutModule::resetStatus() +void StartPlannerModule::resetStatus() { PullOutStatus initial_status; status_ = initial_status; } -void PullOutModule::incrementPathIndex() +void StartPlannerModule::incrementPathIndex() { status_.current_path_idx = std::min(status_.current_path_idx + 1, status_.pull_out_path.partial_paths.size() - 1); } -PathWithLaneId PullOutModule::getCurrentPath() const +PathWithLaneId StartPlannerModule::getCurrentPath() const { return status_.pull_out_path.partial_paths.at(status_.current_path_idx); } -void PullOutModule::planWithPriorityOnEfficientPath( - const std::vector & start_pose_candidates, const Pose & goal_pose) +void StartPlannerModule::planWithPriority( + const std::vector & start_pose_candidates, const Pose & goal_pose, + const std::string search_priority) { status_.is_safe = false; status_.planner_type = PlannerType::NONE; @@ -422,101 +423,88 @@ void PullOutModule::planWithPriorityOnEfficientPath( return; } - // plan with each planner - for (const auto & planner : pull_out_planners_) { - for (size_t i = 0; i < start_pose_candidates.size(); i++) { - status_.back_finished = i == 0; - const auto & pull_out_start_pose = start_pose_candidates.at(i); - planner->setPlannerData(planner_data_); - const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); - // not found safe path - if (!pull_out_path) { - continue; - } - // use current path if back is not needed - if (status_.back_finished) { - status_.is_safe = true; - status_.pull_out_path = *pull_out_path; - status_.pull_out_start_pose = pull_out_start_pose; - status_.planner_type = planner->getPlannerType(); - break; - } + const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { + // Set back_finished flag based on the current index + status_.back_finished = i == 0; - if (i == start_pose_candidates.size() - 1) continue; + // Get the pull_out_start_pose for the current index + const auto & pull_out_start_pose = start_pose_candidates.at(i); - // check next path if back is needed - const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); - const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); - // not found safe path - if (!pull_out_path_next) { - continue; - } + planner->setPlannerData(planner_data_); + const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + // not found safe path + if (!pull_out_path) { + return false; + } + // use current path if back is not needed + if (status_.back_finished) { status_.is_safe = true; - status_.pull_out_path = *pull_out_path_next; - status_.pull_out_start_pose = pull_out_start_pose_next; + status_.pull_out_path = *pull_out_path; + status_.pull_out_start_pose = pull_out_start_pose; status_.planner_type = planner->getPlannerType(); - break; - } - if (status_.is_safe) { - break; + return true; } - } -} -// todo: common processing with planWithPriorityOnEfficientPath -void PullOutModule::planWithPriorityOnShortBackDistance( - const std::vector & start_pose_candidates, const Pose & goal_pose) -{ - status_.is_safe = false; - status_.planner_type = PlannerType::NONE; - - // check if start pose candidates are valid - if (start_pose_candidates.empty()) { - return; - } + // If this is the last start pose candidate, return false + if (i == start_pose_candidates.size() - 1) return false; - for (size_t i = 0; i < start_pose_candidates.size(); i++) { - status_.back_finished = i == 0; - const auto & pull_out_start_pose = start_pose_candidates.at(i); - // plan with each planner - for (const auto & planner : pull_out_planners_) { - planner->setPlannerData(planner_data_); - const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); - // not found safe path - if (!pull_out_path) { - continue; - } - // use current path if back is not needed - if (status_.back_finished) { - status_.is_safe = true; - status_.pull_out_path = *pull_out_path; - status_.pull_out_start_pose = pull_out_start_pose; - status_.planner_type = planner->getPlannerType(); - break; - } + // check next path if back is needed + const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); + const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); + // not found safe path + if (!pull_out_path_next) { + return false; + } - if (i == start_pose_candidates.size() - 1) continue; + // Update status variables with the next path information + status_.is_safe = true; + status_.pull_out_path = *pull_out_path_next; + status_.pull_out_start_pose = pull_out_start_pose_next; + status_.planner_type = planner->getPlannerType(); + return true; + }; - // check next path if back is needed - const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); - const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); - // not found safe path - if (!pull_out_path_next) { - continue; + using PriorityOrder = std::vector>>; + const auto make_loop_order_planner_first = [&]() { + PriorityOrder order_priority; + for (const auto & planner : start_planner_planners_) { + for (size_t i = 0; i < start_pose_candidates.size(); i++) { + order_priority.emplace_back(i, planner); } - status_.is_safe = true; - status_.pull_out_path = *pull_out_path_next; - status_.pull_out_start_pose = pull_out_start_pose_next; - status_.planner_type = planner->getPlannerType(); - break; } - if (status_.is_safe) { - break; + return order_priority; + }; + + const auto make_loop_order_pose_first = [&]() { + PriorityOrder order_priority; + for (size_t i = 0; i < start_pose_candidates.size(); i++) { + for (const auto & planner : start_planner_planners_) { + order_priority.emplace_back(i, planner); + } } + return order_priority; + }; + + // Choose loop order based on priority_on_efficient_path + PriorityOrder order_priority; + if (search_priority == "efficient_path") { + order_priority = make_loop_order_planner_first(); + } else if (search_priority == "short_back_distance") { + order_priority = make_loop_order_pose_first(); + } else { + RCLCPP_ERROR( + getLogger(), + "search_priority should be efficient_path or short_back_distance, but %s is given.", + search_priority.c_str()); + throw std::domain_error("[start_planner] invalid search_priority"); + } + + for (const auto & p : order_priority) { + if (is_safe_with_pose_planner(p.first, p.second)) break; } } -PathWithLaneId PullOutModule::generateStopPath() const +PathWithLaneId StartPlannerModule::generateStopPath() const { const auto & current_pose = planner_data_->self_odometry->pose.pose; constexpr double dummy_path_distance = 1.0; @@ -548,13 +536,14 @@ PathWithLaneId PullOutModule::generateStopPath() const return path; } -void PullOutModule::updatePullOutStatus() +void StartPlannerModule::updatePullOutStatus() { if (has_received_new_route_) { status_ = PullOutStatus(); } - // skip updating if enough time has not passed for preventing chattering between back and pull_out + // skip updating if enough time has not passed for preventing chattering between back and + // start_planner if (!has_received_new_route_ && !last_pull_out_start_update_time_ && !status_.back_finished) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); @@ -571,7 +560,7 @@ void PullOutModule::updatePullOutStatus() const auto & goal_pose = planner_data_->route_handler->getGoalPose(); status_.current_lanes = utils::getExtendedCurrentLanes(planner_data_); - status_.pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_); + status_.pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_); // combine road and shoulder lanes status_.lanes = @@ -579,18 +568,7 @@ void PullOutModule::updatePullOutStatus() // search pull out start candidates backward std::vector start_pose_candidates = searchPullOutStartPoses(); - - if (parameters_->search_priority == "efficient_path") { - planWithPriorityOnEfficientPath(start_pose_candidates, goal_pose); - } else if (parameters_->search_priority == "short_back_distance") { - planWithPriorityOnShortBackDistance(start_pose_candidates, goal_pose); - } else { - RCLCPP_ERROR( - getLogger(), - "search_priority should be efficient_path or short_back_distance, but %s is given.", - parameters_->search_priority.c_str()); - throw std::domain_error("[pull_out] invalid search_priority"); - } + planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( @@ -604,7 +582,7 @@ void PullOutModule::updatePullOutStatus() checkBackFinished(); if (!status_.back_finished) { - status_.backward_path = pull_out_utils::getBackwardPath( + status_.backward_path = start_planner_utils::getBackwardPath( *route_handler, status_.pull_out_lanes, current_pose, status_.pull_out_start_pose, parameters_->backward_velocity); } @@ -615,7 +593,7 @@ void PullOutModule::updatePullOutStatus() } // make this class? -std::vector PullOutModule::searchPullOutStartPoses() +std::vector StartPlannerModule::searchPullOutStartPoses() { std::vector pull_out_start_pose{}; @@ -681,7 +659,7 @@ std::vector PullOutModule::searchPullOutStartPoses() return pull_out_start_pose; } -bool PullOutModule::isOverlappedWithLane( +bool StartPlannerModule::isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, const tier4_autoware_utils::LinearRing2d & vehicle_footprint) { @@ -694,7 +672,7 @@ bool PullOutModule::isOverlappedWithLane( return false; } -bool PullOutModule::hasFinishedPullOut() const +bool StartPlannerModule::hasFinishedPullOut() const { if (!status_.back_finished) { return false; @@ -732,7 +710,7 @@ bool PullOutModule::hasFinishedPullOut() const return has_finished; } -void PullOutModule::checkBackFinished() +void StartPlannerModule::checkBackFinished() { // check ego car is close enough to pull out start pose const auto current_pose = planner_data_->self_odometry->pose.pose; @@ -747,7 +725,7 @@ void PullOutModule::checkBackFinished() RCLCPP_INFO(getLogger(), "back finished"); status_.back_finished = true; - // request pull_out approval + // request start_planner approval waitApproval(); removeRTCStatus(); for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { @@ -757,7 +735,7 @@ void PullOutModule::checkBackFinished() } } -bool PullOutModule::isStopped() +bool StartPlannerModule::isStopped() { odometry_buffer_.push_back(planner_data_->self_odometry); // Delete old data in buffer @@ -780,7 +758,7 @@ bool PullOutModule::isStopped() return is_stopped; } -bool PullOutModule::hasFinishedCurrentPath() +bool StartPlannerModule::hasFinishedCurrentPath() { const auto current_path = getCurrentPath(); const auto current_path_end = current_path.points.back(); @@ -791,7 +769,7 @@ bool PullOutModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -TurnSignalInfo PullOutModule::calcTurnSignalInfo() const +TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const { TurnSignalInfo turn_signal{}; // output @@ -835,7 +813,7 @@ TurnSignalInfo PullOutModule::calcTurnSignalInfo() const return turn_signal; } -void PullOutModule::setDebugData() const +void StartPlannerModule::setDebugData() const { using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; diff --git a/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp similarity index 89% rename from planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp rename to planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 21111f5b06ee6..f51f2ddd7c5ab 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/pull_out/geometric_pull_out.hpp" +#include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" -#include "behavior_path_planner/utils/pull_out/util.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -25,10 +25,10 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using pull_out_utils::combineReferencePath; -using pull_out_utils::getPullOutLanes; +using start_planner_utils::combineReferencePath; +using start_planner_utils::getPullOutLanes; -GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const PullOutParameters & parameters) +GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) : PullOutPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters} { diff --git a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp rename to planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 11c7671a34d73..42f419d6a641f 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.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_out/shift_pull_out.hpp" +#include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/pull_out/util.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -29,11 +29,11 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; namespace behavior_path_planner { -using pull_out_utils::combineReferencePath; -using pull_out_utils::getPullOutLanes; +using start_planner_utils::combineReferencePath; +using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( - rclcpp::Node & node, const PullOutParameters & parameters, + rclcpp::Node & node, const StartPlannerParameters & parameters, std::shared_ptr & lane_departure_checker) : PullOutPlannerBase{node, parameters}, lane_departure_checker_{lane_departure_checker} { @@ -126,7 +126,7 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose, - const BehaviorPathPlannerParameters & common_parameter, const PullOutParameters & parameter) + const BehaviorPathPlannerParameters & common_parameter, const StartPlannerParameters & parameter) { std::vector candidate_paths{}; diff --git a/planning/behavior_path_planner/src/utils/pull_out/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp similarity index 95% rename from planning/behavior_path_planner/src/utils/pull_out/util.cpp rename to planning/behavior_path_planner/src/utils/start_planner/util.cpp index d46b980327a29..54562873d19e5 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_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_out/util.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" @@ -35,7 +35,7 @@ #include #include -namespace behavior_path_planner::pull_out_utils +namespace behavior_path_planner::start_planner_utils { PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) { @@ -114,4 +114,4 @@ lanelet::ConstLanelets getPullOutLanes(const std::shared_ptr // pull out from road lane return utils::getExtendedCurrentLanes(planner_data); } -} // namespace behavior_path_planner::pull_out_utils +} // namespace behavior_path_planner::start_planner_utils 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 a24086a3df8d8..885590f92d420 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 @@ -62,7 +62,7 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml", behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_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/start_planner/start_planner.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 06ec042c30a7a..1d86f18ead09f 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 @@ -14,7 +14,7 @@ - "avoidance_left" - "avoidance_right" - "goal_planner" - - "pull_out" + - "start_planner" - "intersection_occlusion" default_enable_list: @@ -31,5 +31,5 @@ - "avoidance_left" - "avoidance_right" - "goal_planner" - - "pull_out" + - "start_planner" - "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 33970700991c7..31a3dde1ec8e7 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 @@ -53,8 +53,8 @@ Module getModuleType(const std::string & module_name) module.type = Module::AVOIDANCE_RIGHT; } 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 == "start_planner") { + module.type = Module::START_PLANNER; } else if (module_name == "intersection_occlusion") { module.type = Module::INTERSECTION_OCCLUSION; } else { diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index a06a023671690..7a63d2606c8de 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -66,8 +66,8 @@ Module getModuleType(const std::string & module_name) module.type = Module::AVOIDANCE_RIGHT; } 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 == "start_planner") { + module.type = Module::START_PLANNER; } else if (module_name == "intersection_occlusion") { module.type = Module::INTERSECTION_OCCLUSION; } diff --git a/planning/rtc_replayer/src/rtc_replayer_node.cpp b/planning/rtc_replayer/src/rtc_replayer_node.cpp index ebc7171fa209b..a4ac04c4b2063 100644 --- a/planning/rtc_replayer/src/rtc_replayer_node.cpp +++ b/planning/rtc_replayer/src/rtc_replayer_node.cpp @@ -50,8 +50,8 @@ std::string getModuleName(const uint8_t module_type) case Module::GOAL_PLANNER: { return "goal_planner"; } - case Module::PULL_OUT: { - return "pull_out"; + case Module::START_PLANNER: { + return "start_planner"; } case Module::TRAFFIC_LIGHT: { return "traffic_light"; diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp index d464197e03603..46c8ea4125c9a 100644 --- a/system/default_ad_api/src/planning.cpp +++ b/system/default_ad_api/src/planning.cpp @@ -84,7 +84,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/lane_change", "/planning/steering_factor/start_planner", "/planning/steering_factor/goal_planner"}; sub_velocity_factors_ = 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 7bfa72101ade4..b60fa5365f2a2 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 @@ -39,7 +39,7 @@ SteeringFactor.AVOIDANCE_PATH_CHANGE: "avoidance change", SteeringFactor.AVOIDANCE_PATH_RETURN: "avoidance return", SteeringFactor.STATION: "station", - SteeringFactor.PULL_OUT: "pull out", + SteeringFactor.START_PLANNER: "start planner", SteeringFactor.GOAL_PLANNER: "goal planner", SteeringFactor.EMERGENCY_OPERATION: "emergency operation", }