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 1e6161079730..ff448e694eb1 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 ce21d7fefc79..3e95afe247c8 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 6cc3b10d68c5..e406eb5653b0 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -24,22 +24,13 @@ - + - - - - - - - - - - - + + 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 affe9ac6af3e..6dc0b2bea85b 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, @@ -131,34 +131,22 @@ def launch_setup(context, *args, **kwargs): behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] # behavior velocity planner - with open(LaunchConfiguration("blind_spot_param_path").perform(context), "r") as f: - blind_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("crosswalk_param_path").perform(context), "r") as f: - crosswalk_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("detection_area_param_path").perform(context), "r") as f: - detection_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("intersection_param_path").perform(context), "r") as f: - intersection_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("stop_line_param_path").perform(context), "r") as f: - stop_line_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("traffic_light_param_path").perform(context), "r") as f: - traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("virtual_traffic_light_param_path").perform(context), "r") as f: - virtual_traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("occlusion_spot_param_path").perform(context), "r") as f: - occlusion_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("no_stopping_area_param_path").perform(context), "r") as f: - no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("run_out_param_path").perform(context), "r") as f: - run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("speed_bump_param_path").perform(context), "r") as f: - speed_bump_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("out_of_lane_param_path").perform(context), "r") as f: - out_of_lane_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open( - LaunchConfiguration("behavior_velocity_planner_param_path").perform(context), "r" - ) as f: - behavior_velocity_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] + behavior_velocity_planner_common_param_path = LaunchConfiguration( + "behavior_velocity_planner_common_param_path" + ).perform(context) + behavior_velocity_planner_module_param_paths = LaunchConfiguration( + "behavior_velocity_planner_module_param_paths" + ).perform(context) + + behavior_velocity_planner_params_paths = [ + behavior_velocity_planner_common_param_path, + *yaml.safe_load(behavior_velocity_planner_module_param_paths), + ] + + behavior_velocity_planner_params = {} + for path in behavior_velocity_planner_params_paths: + with open(path) as f: + behavior_velocity_planner_params.update(yaml.safe_load(f)["/**"]["ros__parameters"]) behavior_velocity_planner_component = ComposableNode( package="behavior_velocity_planner", @@ -210,20 +198,8 @@ def launch_setup(context, *args, **kwargs): ], parameters=[ nearest_search_param, - behavior_velocity_planner_param, - blind_spot_param, - crosswalk_param, - detection_area_param, - intersection_param, - stop_line_param, - traffic_light_param, - virtual_traffic_light_param, - occlusion_spot_param, - no_stopping_area_param, + behavior_velocity_planner_params, vehicle_param, - run_out_param, - speed_bump_param, - out_of_lane_param, common_param, motion_velocity_smoother_param, behavior_velocity_smoother_type_param, @@ -244,17 +220,10 @@ def launch_setup(context, *args, **kwargs): ) # This condition is true if run_out module is enabled and its detection method is Points - launch_run_out_with_points_method = PythonExpression( - [ - LaunchConfiguration( - "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"] - ), - " and ", - "'", - run_out_param["run_out"]["detection_method"], - "' == 'Points'", - ] - ) + run_out_module = "behavior_velocity_planner::RunOutModulePlugin" + run_out_method = behavior_velocity_planner_params.get("run_out", {}).get("detection_method") + launch_run_out = run_out_module in behavior_velocity_planner_params["launch_modules"] + launch_run_out_with_points_method = launch_run_out and run_out_method == "Points" # load compare map for run_out module load_compare_map = IncludeLaunchDescription( @@ -270,7 +239,7 @@ def launch_setup(context, *args, **kwargs): "use_multithread": "true", }.items(), # launch compare map only when run_out module is enabled and detection method is Points - condition=IfCondition(launch_run_out_with_points_method), + condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))), ) load_vector_map_inside_area_filter = IncludeLaunchDescription( @@ -287,7 +256,7 @@ def launch_setup(context, *args, **kwargs): "polygon_type": "no_obstacle_segmentation_area_for_run_out", }.items(), # launch vector map filter only when run_out module is enabled and detection method is Points - condition=IfCondition(launch_run_out_with_points_method), + condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))), ) group = GroupAction( diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 69deacf4dfb7..4de0f91c53c6 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 2685bf96eb88..9a390d10895f 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 a744e1ffdeb4..8ec93bb5b50d 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 81fb08bbb59d..f521407053e2 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 47ccc9e58674..8bf46fb3d3ae 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/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index a2218c1fdead..5162332db426 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -17,6 +17,7 @@ enable_force_avoidance_for_stopped_vehicle: false enable_safety_check: true enable_yield_maneuver: true + enable_yield_maneuver_during_shifting: false disable_path_update: false use_hatched_road_markings: false 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 2538633e63bf..d2b2a6a79bf0 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 65c600d246c5..8988dd41b50f 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 46b2492cca9f..46b2c4a8a3f3 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 c39ef30770b1..97601698c4eb 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 70d6b37cb61e..32f30af69ac3 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 80fe7ea8dac4..0410f775f7d1 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 30626a4df30c..3040566644dd 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 bd1ccd2065ee..0d8782cb501b 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 027043af33b3..1c3b91851fb1 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 9f8f98c9cbea..60152bdf42bb 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 @@ -155,7 +155,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(); @@ -169,7 +169,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 aed5f0470202..c4401b2ec80b 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/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 07a80c1ccfab..d716d07deaac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -221,6 +221,14 @@ class AvoidanceModule : public SceneModuleInterface */ void insertWaitPoint(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + /** + * @brief insert stop point to yield. (stop in the lane if possible, even if the shift has + * initiated.) + * @param flag. if it is true, the ego decelerates within accel/jerk constraints. + * @param target path. + */ + void insertStopPoint(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + /** * @brief insert stop point in output path. * @param flag. if it is true, the ego decelerates within accel/jerk constraints. @@ -235,6 +243,12 @@ class AvoidanceModule : public SceneModuleInterface */ void insertYieldVelocity(ShiftedPath & shifted_path) const; + /** + * @brief calculate stop distance based on object's overhang. + * @param stop distance. + */ + double calcDistanceToStopLine(const ObjectData & object) const; + // avoidance data preparation /** @@ -274,6 +288,13 @@ class AvoidanceModule : public SceneModuleInterface */ void fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const; + /** + * @brief fill ego status based on the candidate path safety check result. + * @param avoidance data. + * @param debug data. + */ + void fillEgoStatus(AvoidancePlanningData & data, DebugData & debug) const; + /** * @brief fill debug data. * @param avoidance data. @@ -293,6 +314,12 @@ class AvoidanceModule : public SceneModuleInterface */ void updateRegisteredRawShiftLines(); + /** + * @brief check whether the ego can transit yield maneuver. + * @param avoidance data. + */ + bool canYieldManeuver(const AvoidancePlanningData & data) const; + // shift line generation /** @@ -652,6 +679,8 @@ class AvoidanceModule : public SceneModuleInterface bool is_avoidance_maneuver_starts; + bool arrived_path_end_{false}; + std::shared_ptr parameters_; std::shared_ptr ego_velocity_starting_avoidance_ptr_; @@ -674,6 +703,8 @@ class AvoidanceModule : public SceneModuleInterface ObjectDataArray registered_objects_; + mutable ObjectDataArray ego_stopped_objects_; + mutable ObjectDataArray stopped_objects_; mutable DebugData debug_data_; 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 03473709446c..03672aa59f6b 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 5591da525bc3..09c201b0b38c 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 79% 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 fa549c657590..3f6ea8f5b379 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_; @@ -151,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/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 9f65ae5cb9da..07bf525bd71e 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 @@ -91,6 +91,9 @@ struct AvoidanceParameters // enable yield maneuver. bool enable_yield_maneuver{false}; + // enable yield maneuver. + bool enable_yield_maneuver_during_shifting{false}; + // disable path update bool disable_path_update{false}; @@ -446,6 +449,8 @@ struct AvoidancePlanningData bool yield_required{false}; bool found_avoidance_path{false}; + + double to_stop_line{std::numeric_limits::max()}; }; /* diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 67d632c8c1d1..0cd3627cad53 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -131,10 +131,6 @@ PathWithLaneId getTargetSegment( const double target_lane_length, const double lane_changing_length, const double lane_changing_velocity, const double total_required_min_dist); -bool isEgoWithinOriginalLane( - const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, - const BehaviorPathPlannerParameters & common_param); - void get_turn_signal_info( const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info); 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 0ab6bd465fb2..7ace770dc7ff 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 1030c81ad666..d68985ec2735 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 955e85606131..b95d247b6d23 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 ac5aa89ea675..7703c3edf425 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 bc1281242456..e76cfcfd8808 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 314a4c9426ca..708433fbfe23 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 f9b1b25c5c42..0f5e0a34f849 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" @@ -304,6 +304,10 @@ bool isEgoOutOfRoute( const Pose & self_pose, const std::optional & modified_goal, const std::shared_ptr & route_handler); +bool isEgoWithinOriginalLane( + const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param); + // path management // TODO(Horibe) There is a similar function in route_handler. Check. diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index b5585151869d..cd7311f3308b 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 27e7a277ac76..e8d352b86e5e 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) { @@ -368,7 +369,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."); @@ -563,6 +564,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() declare_parameter(ns + "enable_force_avoidance_for_stopped_vehicle"); p.enable_safety_check = declare_parameter(ns + "enable_safety_check"); p.enable_yield_maneuver = declare_parameter(ns + "enable_yield_maneuver"); + p.enable_yield_maneuver_during_shifting = + declare_parameter(ns + "enable_yield_maneuver_during_shifting"); p.disable_path_update = declare_parameter(ns + "disable_path_update"); p.use_hatched_road_markings = declare_parameter(ns + "use_hatched_road_markings"); p.publish_debug_marker = declare_parameter(ns + "publish_debug_marker"); @@ -1049,11 +1052,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/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 0c783fc8e5e5..ffd553d9c980 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 @@ -171,14 +171,28 @@ bool AvoidanceModule::isExecutionReady() const ModuleStatus AvoidanceModule::updateState() { + const auto & data = avoidance_data_; const auto is_plan_running = isAvoidancePlanRunning(); - const bool has_avoidance_target = !avoidance_data_.target_objects.empty(); + const bool has_avoidance_target = !data.target_objects.empty(); - if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), avoidance_data_.current_lanelets)) { + if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated."); return ModuleStatus::SUCCESS; } + const auto idx = planner_data_->findEgoIndex(data.reference_path.points); + if (idx == data.reference_path.points.size() - 1) { + arrived_path_end_ = true; + } + + constexpr double THRESHOLD = 1.0; + if ( + calcDistance2d(getEgoPose(), getPose(data.reference_path.points.back())) > THRESHOLD && + arrived_path_end_) { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "reach path end point. exit avoidance module."); + return ModuleStatus::SUCCESS; + } + DEBUG_PRINT( "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target); @@ -193,7 +207,7 @@ ModuleStatus AvoidanceModule::updateState() return ModuleStatus::SUCCESS; } - helper_.setPreviousDrivingLanes(avoidance_data_.current_lanelets); + helper_.setPreviousDrivingLanes(data.current_lanelets); return ModuleStatus::RUNNING; } @@ -393,11 +407,45 @@ ObjectData AvoidanceModule::createObjectData( return object_data; } +bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const +{ + // transit yield maneuver only when the avoidance maneuver is not initiated. + if (data.avoiding_now) { + return false; + } + + if (!data.stop_target_object) { + return true; + } + + constexpr double TH_STOP_SPEED = 0.01; + constexpr double TH_STOP_POSITION = 0.5; + + const auto stopped_for_the_object = + getEgoSpeed() < TH_STOP_SPEED && std::abs(data.to_stop_line) < TH_STOP_POSITION; + + const auto id = data.stop_target_object.get().object.object_id; + const auto same_id_obj = std::find_if( + ego_stopped_objects_.begin(), ego_stopped_objects_.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + + // if the ego already started avoiding for the object, never transit yield maneuver again. + if (same_id_obj != ego_stopped_objects_.end()) { + return stopped_for_the_object; + } + + // registered objects whom the ego stopped for at the moment of stopping. + if (stopped_for_the_object) { + ego_stopped_objects_.push_back(data.stop_target_object.get()); + } + + return true; +} + void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const { constexpr double AVOIDING_SHIFT_THR = 0.1; data.avoiding_now = std::abs(helper_.getEgoShift()) > AVOIDING_SHIFT_THR; - data.candidate_path = utils::avoidance::toShiftedPath(data.reference_path); auto path_shifter = path_shifter_; @@ -439,20 +487,19 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * STEP 5 * Generate avoidance path. */ - auto candidate_path = generateAvoidancePath(path_shifter); + data.candidate_path = generateAvoidancePath(path_shifter); /** * STEP 6 * Check avoidance path safety. For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ - data.safe = isSafePath(path_shifter, candidate_path, debug); - - if (data.safe) { - data.safe_new_sl = data.unapproved_new_sl; - data.candidate_path = candidate_path; - } + data.safe = isSafePath(path_shifter, data.candidate_path, debug); +} +void AvoidanceModule::fillEgoStatus( + AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const +{ /** * Find the nearest object that should be avoid. When the ego follows reference path, * if the both of following two conditions are satisfied, the module surely avoid the object. @@ -466,29 +513,30 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de if (o.avoid_required && enough_space) { data.avoid_required = true; data.stop_target_object = o; + data.to_stop_line = calcDistanceToStopLine(o); break; } } + const auto can_yield_maneuver = canYieldManeuver(data); + /** - * If the avoidance path is not safe in situation where the ego should avoid object, the ego - * stops in front of the front object with the necessary distance to avoid the object. + * If the avoidance path is safe, use unapproved_new_sl for avoidance path generation. */ - if (!data.safe && data.avoid_required) { - data.yield_required = data.found_avoidance_path && data.avoid_required; - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "not found safe avoidance path. transit yield maneuver..."); + if (data.safe) { + data.yield_required = false; + data.safe_new_sl = data.unapproved_new_sl; + return; } /** - * Even if data.avoid_required is false, the module cancels registered shift point when the - * approved avoidance path is not safe. + * If the yield maneuver is disabled, use unapproved_new_sl for avoidance path generation even if + * the shift line is unsafe. */ - if (!data.safe && registered) { - data.yield_required = true; - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "found safe avoidance path, but it is not safe. canceling avoidance path..."); + if (!parameters_->enable_yield_maneuver) { + data.yield_required = false; + data.safe_new_sl = data.unapproved_new_sl; + return; } /** @@ -496,15 +544,38 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * Even if it is determined that a yield is necessary, the yield maneuver is not executed * if the avoidance has already been initiated. */ - if (!data.safe && data.avoiding_now) { + if (!can_yield_maneuver) { data.yield_required = false; - data.safe = true; // OVERWRITE SAFETY JUDGE data.safe_new_sl = data.unapproved_new_sl; - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "avoiding now. could not transit yield maneuver!!!"); + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. but could not transit yield status."); + return; + } + + /** + * Transit yield maneuver. Clear shift lines and output yield path. + */ + { + data.yield_required = true; + data.safe_new_sl.clear(); + } + + /** + * Even if data.avoid_required is false, the module cancels registered shift point when the + * approved avoidance path is not safe. + */ + const auto approved_path_exist = !path_shifter_.getShiftLines().empty(); + if (approved_path_exist) { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe. canceling approved path..."); + return; } - fillDebugData(data, debug); + /** + * If the avoidance path is not safe in situation where the ego should avoid object, the ego + * stops in front of the front object with the necessary distance to avoid the object. + */ + { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe. transit yield maneuver..."); + } } void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugData & debug) const @@ -550,7 +621,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & data) const { - if (data.yield_required && parameters_->enable_yield_maneuver) { + if (data.yield_required) { return AvoidanceState::YIELD; } @@ -598,6 +669,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif break; } case AvoidanceState::AVOID_EXECUTE: { + insertStopPoint(true, path); break; } default: @@ -755,13 +827,8 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) } AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( - AvoidancePlanningData & data, DebugData & debug) const + AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - { - debug_avoidance_initializer_for_shift_line_.clear(); - debug.unavoidable_objects.clear(); - } - const auto prepare_distance = helper_.getNominalPrepareDistance(); // To be consistent with changes in the ego position, the current shift length is considered. @@ -770,26 +837,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto & base_link2rear = planner_data_->parameters.base_link2rear; AvoidLineArray avoid_lines; - std::vector avoidance_debug_msg_array; - avoidance_debug_msg_array.reserve(data.target_objects.size()); for (auto & o : data.target_objects) { - AvoidanceDebugMsg avoidance_debug_msg; - const auto avoidance_debug_array_false_and_push_back = - [&avoidance_debug_msg, &avoidance_debug_msg_array](const std::string & failed_reason) { - avoidance_debug_msg.allow_avoidance = false; - avoidance_debug_msg.failed_reason = failed_reason; - avoidance_debug_msg_array.push_back(avoidance_debug_msg); - }; - - avoidance_debug_msg.object_id = toHexString(o.object.object_id); - avoidance_debug_msg.longitudinal_distance = o.longitudinal; - avoidance_debug_msg.lateral_distance_from_centerline = o.lateral; - avoidance_debug_msg.to_furthest_linestring_distance = o.to_road_shoulder_distance; - if (!o.avoid_margin) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN); o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; - debug.unavoidable_objects.push_back(o); if (o.avoid_required) { break; } else { @@ -800,9 +850,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto is_object_on_right = utils::avoidance::isOnRight(o); const auto shift_length = helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.get()); if (utils::avoidance::isSameDirectionShift(is_object_on_right, shift_length)) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::SAME_DIRECTION_SHIFT); o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; - debug.unavoidable_objects.push_back(o); if (o.avoid_required) { break; } else { @@ -838,11 +886,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( // TODO(Horibe) Even if there is no enough distance for avoidance shift, the // return-to-center shift must be considered for each object if the current_shift // is not zero. - avoidance_debug_array_false_and_push_back( - AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO); if (!data.avoiding_now) { o.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; - debug.unavoidable_objects.push_back(o); if (o.avoid_required) { break; } else { @@ -854,13 +899,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( // This is the case of exceeding the jerk limit. Use the sharp avoidance ego speed. const auto required_jerk = path_shifter_.calcJerkFromLatLonDistance( avoiding_shift, remaining_distance, helper_.getSharpAvoidanceEgoSpeed()); - avoidance_debug_msg.required_jerk = required_jerk; - avoidance_debug_msg.maximum_jerk = parameters_->max_lateral_jerk; if (required_jerk > parameters_->max_lateral_jerk) { - avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_LARGE_JERK); if (!data.avoiding_now) { o.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - debug.unavoidable_objects.push_back(o); if (o.avoid_required) { break; } else { @@ -913,14 +954,32 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( avoiding_shift, return_shift, al_avoid.start_longitudinal, al_avoid.end_longitudinal, al_return.end_longitudinal, nominal_avoid_distance, avoiding_distance, o.avoid_margin.get(), nominal_return_distance); - avoidance_debug_msg.allow_avoidance = true; - avoidance_debug_msg_array.push_back(avoidance_debug_msg); o.is_avoidable = true; } - debug_avoidance_initializer_for_shift_line_ = std::move(avoidance_debug_msg_array); - debug_avoidance_initializer_for_shift_line_time_ = clock_->now(); + // debug + { + std::vector debug_info_array; + const auto append = [&](const auto & o) { + AvoidanceDebugMsg debug_info; + debug_info.object_id = toHexString(o.object.object_id); + debug_info.longitudinal_distance = o.longitudinal; + debug_info.lateral_distance_from_centerline = o.lateral; + debug_info.allow_avoidance = o.reason == ""; + debug_info.failed_reason = o.reason; + debug_info_array.push_back(debug_info); + }; + + for (const auto & o : data.target_objects) { + append(o); + } + + debug_avoidance_initializer_for_shift_line_.clear(); + debug_avoidance_initializer_for_shift_line_ = std::move(debug_info_array); + debug_avoidance_initializer_for_shift_line_time_ = clock_->now(); + } + fillAdditionalInfoFromLongitudinal(avoid_lines); return avoid_lines; @@ -2765,7 +2824,8 @@ CandidateOutput AvoidanceModule::planCandidate() const CandidateOutput output; - auto shifted_path = data.candidate_path; + auto shifted_path = data.yield_required ? utils::avoidance::toShiftedPath(data.reference_path) + : data.candidate_path; if (!data.safe_new_sl.empty()) { // clip from shift start index for visualize utils::clipPathLength( @@ -3099,6 +3159,8 @@ void AvoidanceModule::updateData() #endif fillShiftLine(avoidance_data_, debug_data_); + fillEgoStatus(avoidance_data_, debug_data_); + fillDebugData(avoidance_data_, debug_data_); } void AvoidanceModule::processOnEntry() @@ -3126,6 +3188,7 @@ void AvoidanceModule::initVariables() current_raw_shift_lines_ = {}; original_unique_id = 0; is_avoidance_maneuver_starts = false; + arrived_path_end_ = false; } void AvoidanceModule::initRTCStatus() @@ -3376,22 +3439,12 @@ void AvoidanceModule::updateAvoidanceDebugData( } } -void AvoidanceModule::insertWaitPoint( - const bool use_constraints_for_decel, ShiftedPath & shifted_path) const +double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const { const auto & p = parameters_; - const auto & data = avoidance_data_; const auto & base_link2front = planner_data_->parameters.base_link2front; const auto & vehicle_width = planner_data_->parameters.vehicle_width; - if (!data.stop_target_object) { - return; - } - - if (data.avoiding_now) { - return; - } - // D5 // |<---->| D4 // |<----------------------------------------------------------------------->| @@ -3407,34 +3460,98 @@ void AvoidanceModule::insertWaitPoint( // D4: o_front.longitudinal // D5: base_link2front - const auto o_front = data.stop_target_object.get(); - const auto t = utils::getHighestProbLabel(o_front.object.classification); + const auto t = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(t); const auto avoid_margin = object_parameter.safety_buffer_lateral + p->lateral_collision_margin + 0.5 * vehicle_width; const auto variable = helper_.getMinimumAvoidanceDistance( - helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), avoid_margin)); + helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + base_link2front; - const auto start_longitudinal = - o_front.longitudinal - - std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance); + + return object.longitudinal - + std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance); +} + +void AvoidanceModule::insertWaitPoint( + const bool use_constraints_for_decel, ShiftedPath & shifted_path) const +{ + const auto & data = avoidance_data_; + + if (!data.stop_target_object) { + return; + } + + if (data.avoiding_now) { + return; + } if (!use_constraints_for_decel) { utils::avoidance::insertDecelPoint( - getEgoPosition(), start_longitudinal, 0.0, shifted_path.path, stop_pose_); + getEgoPosition(), data.to_stop_line, 0.0, shifted_path.path, stop_pose_); return; } const auto stop_distance = helper_.getMildDecelDistance(0.0); if (stop_distance) { - const auto insert_distance = std::max(start_longitudinal, *stop_distance); + const auto insert_distance = std::max(data.to_stop_line, *stop_distance); utils::avoidance::insertDecelPoint( getEgoPosition(), insert_distance, 0.0, shifted_path.path, stop_pose_); } } +void AvoidanceModule::insertStopPoint( + const bool use_constraints_for_decel, ShiftedPath & shifted_path) const +{ + const auto & data = avoidance_data_; + + if (data.safe) { + return; + } + + if (!parameters_->enable_yield_maneuver_during_shifting) { + return; + } + + const auto stop_idx = [&]() { + const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); + for (size_t idx = ego_idx; idx < shifted_path.path.points.size(); ++idx) { + const auto & estimated_pose = shifted_path.path.points.at(idx).point.pose; + if (!utils::isEgoWithinOriginalLane( + data.current_lanelets, estimated_pose, planner_data_->parameters)) { + return idx - 1; + } + } + + return shifted_path.path.points.size() - 1; + }(); + + const auto stop_distance = + calcSignedArcLength(shifted_path.path.points, getEgoPosition(), stop_idx); + + // Insert deceleration point at stop distance without deceleration if use_constraints_for_decel is + // false + if (!use_constraints_for_decel) { + utils::avoidance::insertDecelPoint( + getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_); + return; + } + + // Otherwise, consider deceleration constraints before inserting deceleration point + const auto decel_distance = helper_.getMildDecelDistance(0.0); + if (!decel_distance) { + return; + } + if (stop_distance < decel_distance) { + return; + } + + constexpr double MARGIN = 1.0; + utils::avoidance::insertDecelPoint( + getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_); +} + void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const { const auto & p = parameters_; 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 0060a3cc1b80..52a494ef659c 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() return 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/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 59723f116ebd..2a07d1fa3387 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -300,7 +300,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; - return utils::lane_change::isEgoWithinOriginalLane( + return utils::isEgoWithinOriginalLane( status_.current_lanes, estimated_pose, planner_data_->parameters); } } @@ -335,7 +335,7 @@ bool NormalLaneChange::isAbleToStopSafely() const dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; - return utils::lane_change::isEgoWithinOriginalLane( + return utils::isEgoWithinOriginalLane( status_.current_lanes, estimated_pose, planner_data_->parameters); } } @@ -947,7 +947,7 @@ NormalLaneChangeBT::NormalLaneChangeBT( PathWithLaneId NormalLaneChangeBT::getReferencePath() const { PathWithLaneId reference_path; - if (!utils::lane_change::isEgoWithinOriginalLane( + if (!utils::isEgoWithinOriginalLane( status_.current_lanes, getEgoPose(), planner_data_->parameters)) { return reference_path; } 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 9084f461d012..ab33c78c945a 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 89% 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 f5934cdc5049..e9579b8cd9c7 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,20 +48,20 @@ 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::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, @@ -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()) { return ModuleStatus::SUCCESS; @@ -178,7 +178,7 @@ ModuleStatus PullOutModule::updateState() return current_state_; } -BehaviorModuleOutput PullOutModule::plan() +BehaviorModuleOutput StartPlannerModule::plan() { if (isWaitingApproval()) { clearWaitingApproval(); @@ -253,7 +253,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( @@ -263,7 +263,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(); @@ -271,14 +271,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; } @@ -286,7 +286,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) { @@ -312,7 +312,7 @@ PathWithLaneId PullOutModule::getFullPath() const return full_path; } -BehaviorModuleOutput PullOutModule::planWaitingApproval() +BehaviorModuleOutput StartPlannerModule::planWaitingApproval() { updatePullOutStatus(); waitApproval(); @@ -331,7 +331,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); @@ -376,7 +376,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( @@ -385,7 +385,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(); @@ -393,19 +393,19 @@ 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 { if (status_.pull_out_path.partial_paths.size() <= status_.current_path_idx) { return PathWithLaneId{}; @@ -413,7 +413,7 @@ PathWithLaneId PullOutModule::getCurrentPath() const return status_.pull_out_path.partial_paths.at(status_.current_path_idx); } -void PullOutModule::planWithPriority( +void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & goal_pose, const std::string search_priority) { @@ -469,7 +469,7 @@ void PullOutModule::planWithPriority( using PriorityOrder = std::vector>>; const auto make_loop_order_planner_first = [&]() { PriorityOrder order_priority; - for (const auto & planner : pull_out_planners_) { + for (const auto & planner : start_planner_planners_) { for (size_t i = 0; i < start_pose_candidates.size(); i++) { order_priority.emplace_back(i, planner); } @@ -480,7 +480,7 @@ void PullOutModule::planWithPriority( 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 : pull_out_planners_) { + for (const auto & planner : start_planner_planners_) { order_priority.emplace_back(i, planner); } } @@ -498,7 +498,7 @@ void PullOutModule::planWithPriority( getLogger(), "search_priority should be efficient_path or short_back_distance, but %s is given.", search_priority.c_str()); - throw std::domain_error("[pull_out] invalid search_priority"); + throw std::domain_error("[start_planner] invalid search_priority"); } for (const auto & p : order_priority) { @@ -506,7 +506,7 @@ void PullOutModule::planWithPriority( } } -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; @@ -538,13 +538,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()); @@ -561,7 +562,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 = @@ -583,7 +584,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); } @@ -594,7 +595,7 @@ void PullOutModule::updatePullOutStatus() } // make this class? -std::vector PullOutModule::searchPullOutStartPoses() +std::vector StartPlannerModule::searchPullOutStartPoses() { std::vector pull_out_start_pose{}; @@ -660,7 +661,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) { @@ -673,7 +674,7 @@ bool PullOutModule::isOverlappedWithLane( return false; } -bool PullOutModule::hasFinishedPullOut() const +bool StartPlannerModule::hasFinishedPullOut() const { if (!status_.back_finished) { return false; @@ -685,13 +686,13 @@ bool PullOutModule::hasFinishedPullOut() const // are also running at the same time. const double lateral_offset_to_path = motion_utils::calcLateralOffset(getCurrentPath().points, current_pose.position); - constexpr double lateral_offset_threshold = 0.5; + constexpr double lateral_offset_threshold = 0.2; if (std::abs(lateral_offset_to_path) > lateral_offset_threshold) { return false; } const double yaw_deviation = motion_utils::calcYawDeviation(getCurrentPath().points, current_pose); - constexpr double yaw_deviation_threshold = 0.5; + constexpr double yaw_deviation_threshold = 0.087; // 5deg if (std::abs(yaw_deviation) > yaw_deviation_threshold) { return false; } @@ -711,7 +712,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; @@ -726,7 +727,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) { @@ -736,7 +737,7 @@ void PullOutModule::checkBackFinished() } } -bool PullOutModule::isStopped() +bool StartPlannerModule::isStopped() { odometry_buffer_.push_back(planner_data_->self_odometry); // Delete old data in buffer @@ -759,7 +760,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(); @@ -770,7 +771,7 @@ bool PullOutModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -TurnSignalInfo PullOutModule::calcTurnSignalInfo() const +TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const { TurnSignalInfo turn_signal{}; // output @@ -814,7 +815,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/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index a29cc5e34f45..aeaee91159fa 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -590,20 +590,6 @@ ShiftLine getLaneChangingShiftLine( return shift_line; } -bool isEgoWithinOriginalLane( - const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, - const BehaviorPathPlannerParameters & common_param) -{ - const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); - const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); - const auto base_link2front = common_param.base_link2front; - const auto base_link2rear = common_param.base_link2rear; - const auto vehicle_width = common_param.vehicle_width; - const auto vehicle_poly = - tier4_autoware_utils::toFootprint(current_pose, base_link2front, base_link2rear, vehicle_width); - return boost::geometry::within(vehicle_poly, lanelet::utils::to2D(lane_poly).basicPolygon()); -} - void get_turn_signal_info( const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info) { 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 21111f5b06ee..f51f2ddd7c5a 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 11c7671a34d7..42f419d6a641 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 d46b980327a2..54562873d19e 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/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index c015a2659193..bfb795f42035 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1112,6 +1112,23 @@ bool isEgoOutOfRoute( return false; } +bool isEgoWithinOriginalLane( + const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param) +{ + const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); + const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); + const auto base_link2front = common_param.base_link2front; + const auto base_link2rear = common_param.base_link2rear; + const auto vehicle_width = common_param.vehicle_width; + const auto vehicle_poly = + tier4_autoware_utils::toFootprint(current_pose, base_link2front, base_link2rear, vehicle_width); + + // Check if the ego vehicle is entirely within the lane by checking if the vehicle's polygon + // is within the lane's polygon + return boost::geometry::within(vehicle_poly, lanelet::utils::to2D(lane_poly).basicPolygon()); +} + lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes) { lanelet::ConstLanelets lanes; 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 a24086a3df8d..885590f92d42 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/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt index da4a9de4a2a8..682da3ae58d7 100644 --- a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt @@ -11,4 +11,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/blind_spot.param.yaml rename to planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index 174488c81499..55b67b38ac35 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -13,4 +13,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/crosswalk.param.yaml rename to planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml diff --git a/planning/behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_detection_area_module/CMakeLists.txt index 5413517d9fb4..2a16ed95ba65 100644 --- a/planning/behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_detection_area_module/CMakeLists.txt @@ -11,4 +11,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/detection_area.param.yaml b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/detection_area.param.yaml rename to planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 063430ad5d29..3e6d5bc81ff9 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -13,4 +13,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/intersection.param.yaml rename to planning/behavior_velocity_intersection_module/config/intersection.param.yaml diff --git a/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt new file mode 100644 index 000000000000..ef3073df56c2 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_no_drivable_lane_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp + src/util.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_no_drivable_lane_module/config/no_drivable_lane.param.yaml b/planning/behavior_velocity_no_drivable_lane_module/config/no_drivable_lane.param.yaml new file mode 100644 index 000000000000..d75acfc0b653 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/config/no_drivable_lane.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + no_drivable_lane: + stop_margin: 1.5 # Stop margin before the no drivable lane [m] + print_debug_info: false diff --git a/planning/behavior_velocity_no_drivable_lane_module/docs/no-drivable-lane-design.md b/planning/behavior_velocity_no_drivable_lane_module/docs/no-drivable-lane-design.md new file mode 100644 index 000000000000..57bff0f44625 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/docs/no-drivable-lane-design.md @@ -0,0 +1,61 @@ +## No Drivable Lane + +### Role + +This module plans the velocity of the related part of the path in case there is a no drivable lane referring to it. + +A no drivable lane is a lanelet or more that are out of operation design domain (ODD), i.e., the vehicle **must not** drive autonomously in this lanelet. +A lanelet can be no drivable (out of ODD) due to many reasons, either technical limitations of the SW and/or HW, business requirements, safety considerations, .... etc, or even a combination of those. + +Some examples of No Drivable Lanes + +- Closed road intentionally, due to construction work for example +- Underpass road that goes under a railway, for safety reasons +- Road with slope/inclination that the vehicle is not be able to drive autonomously due to technical limitations. And lots of other examples. + +![no-drivable-lane-design.svg](no_drivable_lane_design.svg) + +A lanelet becomes invalid by adding a new tag under the relevant lanelet in the map file ``. + +The target of this module is to stop the vehicle before entering the no drivable lane (with configurable stop margin) or keep the vehicle stationary if autonomous mode started inside a no drivable lane. Then ask the human driver to take the responsibility of the driving task (Takeover Request / Request to Intervene) + +### Activation Timing + +This function is activated when the lane id of the target path has an no drivable lane label (i.e. the `no_drivable_lane` attribute is `yes`). + +### Module Parameters + +| Parameter | Type | Description | +| ------------------ | ------ | ---------------------------------------------------- | +| `stop_margin` | double | [m] margin for ego vehicle to stop before speed_bump | +| `print_debug_info` | bool | whether debug info will be printed or not | + +### Inner-workings / Algorithms + +- Get no_drivable_lane attribute on the path from lanelet2 map +- The no drivable lane state machine starts in `INIT` state +- Get the intersection points between path and no drivable lane polygon +- Assign the state to `APPROACHING` toward a no drivable lane if: + - the distance from front of the ego vehicle till the first intersection point between the ego path and the no drivable lane polygon is more than the `stop_margin` +- Assign the state to `INSIDE_NO_DRIVABLE_LANE` if: + - the first point of the ego path is inside the no drivable lane polygon, or + - the distance from front of the ego vehicle till the first intersection point between the ego path and the no drivable lane polygon is less than the `stop_margin` +- Assign the state to `STOPPED` when the vehicle is completely stopped + +![no_drivable_lane_scenarios.svg](no_drivable_lane_scenarios.svg) + +- At each state, RTC settings are assigned according to the following table + +#### RTC Settings During Different States + +| State | RTC Activation | Safe State | Distance | +| ------------------------- | -------------- | ---------- | ----------------------------------------------------------- | +| `INIT` | `false` | `true` | distance from ego front to first intersection point OR zero | +| `APPROACHING` | `false` | `true` | distance from ego front to first intersection | +| `INSIDE_NO_DRIVABLE_LANE` | `false` | `false` | zero | +| `STOPPED` | `true` | `false` | zero | + +### Future Work + +- As [Request to Intervene API](https://github.com/autowarefoundation/autoware/issues/3487) is not implemented yet, this will be handled to notify the driver to takeover the driving task responsibility after the vehicle stops due to `no_drivable_lane` +- Handle the case when the vehicle stops before a no drivable lane but part of its footprint intersects with the no drivable lane polygon. diff --git a/planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_design.svg b/planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_design.svg new file mode 100644 index 000000000000..7fc316be74ac --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_design.svg @@ -0,0 +1,163 @@ + + + + + + + + + + + + + + +
+
+
+ No drivable lane +
+
+
+ + No drivable lane + + + + + + + + + + + + +
+
+
+ First intersection point +
+
+
+ + First intersection point + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + +
+
+
+ Ego path +
+
+
+ + Ego path + + + + + + + + + + + +
+
+
+
Stop margin
+
+
+
+ + Stop margin + + + + + + + + +
+
+
+ Direction of motion +
+
+
+ + Direction of motion + + + + + + + +
+
+
+ Second intersection point +
+
+
+ + Second intersection point + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_scenarios.svg b/planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_scenarios.svg new file mode 100644 index 000000000000..4e74b197ccf9 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/docs/no_drivable_lane_scenarios.svg @@ -0,0 +1,181 @@ + + + + + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + +
+
+
+ EGO +
+
+
+ + EGO + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_no_drivable_lane_module/package.xml new file mode 100644 index 000000000000..ad21ed5328e4 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/package.xml @@ -0,0 +1,37 @@ + + + + behavior_velocity_no_drivable_lane_module + 0.1.0 + The behavior_velocity_no_drivable_lane_module package + + Ahmed Ebrahim + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + sensor_msgs + tf2 + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml new file mode 100644 index 000000000000..33b0d5d6e746 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp new file mode 100644 index 000000000000..a4f693dfaa88 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 TIER IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene.hpp" + +#include +#include +#include + +#include + +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using visualization_msgs::msg::Marker; + +namespace +{ +visualization_msgs::msg::MarkerArray createNoDrivableLaneMarkers( + const NoDrivableLaneModule::DebugData & debug_data, const rclcpp::Time & now, + const int64_t module_id) +{ + visualization_msgs::msg::MarkerArray msg; + const int32_t uid = planning_utils::bitShift(module_id); + + // No Drivable Lane Polygon + if (!debug_data.no_drivable_lane_polygon.empty()) { + auto marker = createDefaultMarker( + "map", now, "no_drivable_lane polygon", uid, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + for (const auto & p : debug_data.no_drivable_lane_polygon) { + marker.points.push_back(createPoint(p.x, p.y, p.z)); + } + marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + } + + // Path - polygon intersection points + { + auto marker = createDefaultMarker( + "map", now, "path_polygon intersection points", uid, Marker::POINTS, + createMarkerScale(0.25, 0.25, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + const auto & p_first = debug_data.path_polygon_intersection.first_intersection_point; + if (p_first) { + marker.points.push_back(createPoint(p_first->x, p_first->y, p_first->z)); + } + const auto & p_second = debug_data.path_polygon_intersection.second_intersection_point; + if (p_second) { + marker.points.push_back(createPoint(p_second->x, p_second->y, p_second->z)); + } + if (!marker.points.empty()) msg.markers.push_back(marker); + } + + return msg; +} +} // namespace + +motion_utils::VirtualWalls NoDrivableLaneModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls; + + const auto now = this->clock_->now(); + + if ( + (state_ == State::APPROACHING) || (state_ == State::INSIDE_NO_DRIVABLE_LANE) || + (state_ == State::STOPPED)) { + motion_utils::VirtualWall wall; + wall.text = "no_drivable_lane"; + wall.style = motion_utils::VirtualWallType::stop; + wall.ns = std::to_string(module_id_) + "_"; + wall.pose = debug_data_.stop_pose; + virtual_walls.push_back(wall); + } + + return virtual_walls; +} + +visualization_msgs::msg::MarkerArray NoDrivableLaneModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + const auto now = this->clock_->now(); + + appendMarkerArray( + createNoDrivableLaneMarkers(debug_data_, this->clock_->now(), module_id_), &debug_marker_array); + + return debug_marker_array; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp new file mode 100644 index 000000000000..418f05baeaf2 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -0,0 +1,69 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "manager.hpp" + +#include + +namespace behavior_velocity_planner +{ + +NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin", 1.5); + planner_param_.print_debug_info = node.declare_parameter(ns + ".print_debug_info", false); +} + +void NoDrivableLaneModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & ll : planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->current_odometry->pose)) { + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + if (isModuleRegistered(module_id)) { + continue; + } + + const std::string no_drivable_lane_attribute = ll.attributeOr("no_drivable_lane", "no"); + if (no_drivable_lane_attribute != "yes") { + continue; + } + + registerModule(std::make_shared( + module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_)); + } +} + +std::function &)> +NoDrivableLaneModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_id_set = planning_utils::getLaneIdSetOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [lane_id_set](const std::shared_ptr & scene_module) { + return lane_id_set.count(scene_module->getModuleId()) == 0; + }; +} + +} // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::NoDrivableLaneModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp new file mode 100644 index 000000000000..2b3b80510c9e --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class NoDrivableLaneModuleManager : public SceneModuleManagerInterface +{ +public: + explicit NoDrivableLaneModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "no_drivable_lane"; } + +private: + NoDrivableLaneModule::PlannerParam planner_param_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class NoDrivableLaneModulePlugin : public PluginWrapper +{ +}; + +} // namespace behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp new file mode 100644 index 000000000000..f08154cda336 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -0,0 +1,286 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene.hpp" + +#include "motion_utils/motion_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "util.hpp" + +#include + +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::createPoint; + +NoDrivableLaneModule::NoDrivableLaneModule( + const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), + planner_param_(planner_param), + state_(State::INIT) +{ + velocity_factor_.init(VelocityFactor::NO_DRIVABLE_LANE); +} + +bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + if (path->points.empty()) { + return false; + } + + *stop_reason = planning_utils::initializeStopReason(StopReason::NO_DRIVABLE_LANE); + + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const auto & lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & no_drivable_lane = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto & no_drivable_lane_polygon = + lanelet::utils::to2D(no_drivable_lane).polygon2d().basicPolygon(); + + path_no_drivable_lane_polygon_intersection = + getPathIntersectionWithNoDrivableLanePolygon(*path, no_drivable_lane_polygon, ego_pos, 2); + + distance_ego_first_intersection = 0.0; + + if (path_no_drivable_lane_polygon_intersection.first_intersection_point) { + first_intersection_point = + path_no_drivable_lane_polygon_intersection.first_intersection_point.get(); + distance_ego_first_intersection = motion_utils::calcSignedArcLength( + path->points, planner_data_->current_odometry->pose.position, first_intersection_point); + distance_ego_first_intersection -= planner_data_->vehicle_info_.max_longitudinal_offset_m; + } + + initialize_debug_data(no_drivable_lane, ego_pos); + + switch (state_) { + case State::INIT: { + if (planner_param_.print_debug_info) { + RCLCPP_INFO(logger_, "Init"); + } + + handle_init_state(); + + break; + } + + case State::APPROACHING: { + if (planner_param_.print_debug_info) { + RCLCPP_INFO(logger_, "Approaching "); + } + + handle_approaching_state(path, stop_reason); + + break; + } + + case State::INSIDE_NO_DRIVABLE_LANE: { + if (planner_param_.print_debug_info) { + RCLCPP_INFO(logger_, "INSIDE_NO_DRIVABLE_LANE"); + } + + handle_inside_no_drivable_lane_state(path, stop_reason); + + break; + } + + case State::STOPPED: { + if (planner_param_.print_debug_info) { + RCLCPP_INFO(logger_, "STOPPED"); + } + + handle_stopped_state(path, stop_reason); + + break; + } + + default: { + RCLCPP_ERROR(logger_, "ERROR. Undefined case"); + return false; + } + } + return true; +} + +void NoDrivableLaneModule::handle_init_state() +{ + if ( + (path_no_drivable_lane_polygon_intersection.is_first_path_point_inside_polygon) || + ((path_no_drivable_lane_polygon_intersection.first_intersection_point) && + (distance_ego_first_intersection <= planner_param_.stop_margin))) { + state_ = State::INSIDE_NO_DRIVABLE_LANE; + } else if ( + (path_no_drivable_lane_polygon_intersection.first_intersection_point) && + (distance_ego_first_intersection > planner_param_.stop_margin)) { + state_ = State::APPROACHING; + } else { + state_ = State::INIT; + } +} + +void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopReason * stop_reason) +{ + const double longitudinal_offset = + -1.0 * (planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m); + + const auto op_target_point = motion_utils::calcLongitudinalOffsetPoint( + path->points, first_intersection_point, longitudinal_offset); + + geometry_msgs::msg::Point target_point; + + if (op_target_point) { + target_point = op_target_point.get(); + } + + const auto target_segment_idx = motion_utils::findNearestSegmentIndex(path->points, target_point); + + const auto & op_target_point_idx = + motion_utils::insertTargetPoint(target_segment_idx, target_point, path->points, 5e-2); + size_t target_point_idx; + if (op_target_point_idx) { + target_point_idx = op_target_point_idx.get(); + } + + geometry_msgs::msg::Point stop_point = + tier4_autoware_utils::getPoint(path->points.at(target_point_idx).point); + + const auto & op_stop_pose = + planning_utils::insertStopPoint(stop_point, target_segment_idx, *path); + + // Get stop point and stop factor + { + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto & stop_pose = op_stop_pose.get(); + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points.push_back(stop_point); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + + const auto virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( + path->points, stop_pose.position, debug_data_.base_link2front); + + debug_data_.stop_pose = virtual_wall_pose.get(); + } + + const size_t current_seg_idx = findEgoSegmentIndex(path->points); + const auto intersection_segment_idx = + motion_utils::findNearestSegmentIndex(path->points, first_intersection_point); + const double signed_arc_dist_to_intersection_point = + motion_utils::calcSignedArcLength( + path->points, planner_data_->current_odometry->pose.position, current_seg_idx, + first_intersection_point, intersection_segment_idx) - + planner_data_->vehicle_info_.max_longitudinal_offset_m; + + // Move to stopped state if stopped + if ( + (signed_arc_dist_to_intersection_point <= planner_param_.stop_margin) && + (planner_data_->isVehicleStopped())) { + if (planner_param_.print_debug_info) { + RCLCPP_INFO(logger_, "APPROACHING -> STOPPED"); + RCLCPP_INFO_STREAM( + logger_, "signed_arc_dist_to_stop_point = " << signed_arc_dist_to_intersection_point); + } + + if (signed_arc_dist_to_intersection_point < 0.0) { + RCLCPP_ERROR( + logger_, "Failed to stop before no drivable lane but ego stopped. Change state to STOPPED"); + } + + state_ = State::STOPPED; + } +} + +void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( + PathWithLaneId * path, StopReason * stop_reason) +{ + const auto & current_point = planner_data_->current_odometry->pose.position; + const size_t current_seg_idx = findEgoSegmentIndex(path->points); + + // Insert stop point + planning_utils::insertStopPoint(current_point, current_seg_idx, *path); + + // Get stop point and stop factor + { + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto & stop_pose = tier4_autoware_utils::getPose(path->points.at(0)); + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points.push_back(current_point); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_odometry->pose, stop_pose, + VelocityFactor::NO_DRIVABLE_LANE); + + const auto & virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( + path->points, stop_pose.position, debug_data_.base_link2front); + + debug_data_.stop_pose = virtual_wall_pose.get(); + } + + // Move to stopped state if stopped + if (planner_data_->isVehicleStopped()) { + if (planner_param_.print_debug_info) { + RCLCPP_INFO(logger_, "APPROACHING -> STOPPED"); + } + state_ = State::STOPPED; + } +} + +void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReason * stop_reason) +{ + const auto & stopped_pose = motion_utils::calcLongitudinalOffsetPose( + path->points, planner_data_->current_odometry->pose.position, 0.0); + + if (!stopped_pose) { + state_ = State::INIT; + return; + } + + SegmentIndexWithPose ego_pos_on_path; + ego_pos_on_path.pose = stopped_pose.get(); + ego_pos_on_path.index = findEgoSegmentIndex(path->points); + + // Insert stop pose + planning_utils::insertStopPoint(ego_pos_on_path.pose.position, ego_pos_on_path.index, *path); + + // Get stop point and stop factor + { + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto & stop_pose = ego_pos_on_path.pose; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points.push_back(stop_pose.position); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); + + const auto virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( + path->points, stop_pose.position, debug_data_.base_link2front); + + debug_data_.stop_pose = virtual_wall_pose.get(); + } +} + +void NoDrivableLaneModule::initialize_debug_data( + const lanelet::Lanelet & no_drivable_lane, const geometry_msgs::msg::Point & ego_pos) +{ + debug_data_ = DebugData(); + debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + debug_data_.path_polygon_intersection = path_no_drivable_lane_polygon_intersection; + + for (const auto & p : no_drivable_lane.polygon2d().basicPolygon()) { + debug_data_.no_drivable_lane_polygon.push_back(createPoint(p.x(), p.y(), ego_pos.z)); + } +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp new file mode 100644 index 000000000000..51d333933905 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -0,0 +1,90 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENE_HPP_ +#define SCENE_HPP_ + +#include "util.hpp" + +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +using autoware_auto_planning_msgs::msg::PathWithLaneId; + +class NoDrivableLaneModule : public SceneModuleInterface +{ +public: + enum class State { INIT, APPROACHING, INSIDE_NO_DRIVABLE_LANE, STOPPED }; + + struct SegmentIndexWithPose + { + size_t index; + geometry_msgs::msg::Pose pose; + }; + + struct DebugData + { + double base_link2front; + PathWithNoDrivableLanePolygonIntersection path_polygon_intersection; + std::vector no_drivable_lane_polygon; + geometry_msgs::msg::Pose stop_pose; + }; + + struct PlannerParam + { + double stop_margin; + bool print_debug_info; + }; + + NoDrivableLaneModule( + const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + motion_utils::VirtualWalls createVirtualWalls() override; + +private: + const int64_t lane_id_; + + // Parameter + PlannerParam planner_param_; + + // Debug + DebugData debug_data_; + + // State machine + State state_; + + PathWithNoDrivableLanePolygonIntersection path_no_drivable_lane_polygon_intersection; + geometry_msgs::msg::Point first_intersection_point; + double distance_ego_first_intersection; + + void handle_init_state(); + void handle_approaching_state(PathWithLaneId * path, StopReason * stop_reason); + void handle_inside_no_drivable_lane_state(PathWithLaneId * path, StopReason * stop_reason); + void handle_stopped_state(PathWithLaneId * path, StopReason * stop_reason); + void initialize_debug_data( + const lanelet::Lanelet & no_drivable_lane, const geometry_msgs::msg::Point & ego_pos); +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp new file mode 100644 index 000000000000..9b50a73a725d --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -0,0 +1,109 @@ +// Copyright 2023 TIER IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "util.hpp" + +#include "motion_utils/motion_utils.hpp" + +#include + +#include + +namespace behavior_velocity_planner +{ + +namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Polygon = bg::model::polygon; +using Line = bg::model::linestring; + +using motion_utils::calcSignedArcLength; +using tier4_autoware_utils::createPoint; + +PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLanePolygon( + const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, + const geometry_msgs::msg::Point & ego_pos, const size_t max_num) +{ + PathWithNoDrivableLanePolygonIntersection path_no_drivable_lane_polygon_intersection; + std::vector intersects{}; + + bool found_max_num = false; + for (size_t i = 0; i < ego_path.points.size() - 1; ++i) { + const auto & p_back = ego_path.points.at(i).point.pose.position; + const auto & p_front = ego_path.points.at(i + 1).point.pose.position; + const Line segment{{p_back.x, p_back.y}, {p_front.x, p_front.y}}; + + std::vector tmp_intersects{}; + bg::intersection(segment, polygon, tmp_intersects); + + for (const auto & p : tmp_intersects) { + intersects.push_back(p); + if (intersects.size() == max_num) { + found_max_num = true; + break; + } + } + + if (found_max_num) { + break; + } + } + + const auto compare = [&](const Point & p1, const Point & p2) { + const auto dist_l1 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z)); + + const auto dist_l2 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z)); + + return dist_l1 < dist_l2; + }; + + std::sort(intersects.begin(), intersects.end(), compare); + + const auto & p_last = ego_path.points.back().point.pose.position; + const auto & p_first = ego_path.points.front().point.pose.position; + const Point & last_path_point{p_last.x, p_last.y}; + const Point & first_path_point{p_first.x, p_first.y}; + + path_no_drivable_lane_polygon_intersection.is_first_path_point_inside_polygon = + bg::within(first_path_point, polygon); + auto const & is_last_path_point_inside_polygon = bg::within(last_path_point, polygon); + + if ( + intersects.empty() && + path_no_drivable_lane_polygon_intersection.is_first_path_point_inside_polygon && + is_last_path_point_inside_polygon) { + path_no_drivable_lane_polygon_intersection.is_path_inside_of_polygon = true; + } else { + // classify first and second intersection points + for (size_t i = 0; i < intersects.size(); ++i) { + const auto & p = intersects.at(i); + if ( + (intersects.size() == 2 && i == 0) || + (intersects.size() == 1 && is_last_path_point_inside_polygon)) { + path_no_drivable_lane_polygon_intersection.first_intersection_point = + createPoint(p.x(), p.y(), ego_pos.z); + } else if ( + (intersects.size() == 2 && i == 1) || + (intersects.size() == 1 && + path_no_drivable_lane_polygon_intersection.is_first_path_point_inside_polygon)) { + path_no_drivable_lane_polygon_intersection.second_intersection_point = + createPoint(p.x(), p.y(), ego_pos.z); + } + } + } + return path_no_drivable_lane_polygon_intersection; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp new file mode 100644 index 000000000000..1cd527d93ef7 --- /dev/null +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp @@ -0,0 +1,56 @@ +// Copyright 2023 TIER IV, Inc., Leo Drive Teknoloji A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTIL_HPP_ +#define UTIL_HPP_ + +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ + +namespace bg = boost::geometry; + +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; + +// the status of intersection between path and no drivable lane polygon +struct PathWithNoDrivableLanePolygonIntersection +{ + bool is_path_inside_of_polygon = false; // true if path is completely inside the no drivable lane + // polygon (no intersection point) + bool is_first_path_point_inside_polygon = + false; // true if first path point is inside the no drivable lane polygon + boost::optional first_intersection_point; + boost::optional second_intersection_point; +}; + +PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLanePolygon( + const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, + const geometry_msgs::msg::Point & ego_pos, const size_t max_num); + +} // namespace behavior_velocity_planner + +#endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt index 0340710792fd..4d14228c4685 100644 --- a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -11,4 +11,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene_no_stopping_area.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/no_stopping_area.param.yaml b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/no_stopping_area.param.yaml rename to planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml diff --git a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt index 06e8e8cf3e90..7a062ee85442 100644 --- a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -27,4 +27,4 @@ if(BUILD_TESTING) target_include_directories(test_${PROJECT_NAME} PRIVATE src) endif() -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/occlusion_spot.param.yaml rename to planning/behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 40173ad76d93..633d5fdd613a 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -15,4 +15,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene_out_of_lane.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/out_of_lane.param.yaml rename to planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index d1114c0b3ddb..52288163a981 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -20,6 +20,7 @@ It loads modules as plugins. Please refer to the links listed below for detail o - [Run Out](../behavior_velocity_run_out_module/docs/run-out-design.md) - [Speed Bump](../behavior_velocity_speed_bump_module/docs/speed-bump-design.md) - [Out of Lane](../behavior_velocity_out_of_lane_module/docs/out-of-lane-design.md) +- [No Drivable Lane](../behavior_velocity_no_drivable_lane_module/docs/no-drivable-lane-design.md) When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose. So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position. diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index 6f1341cb0264..e8377f841aa7 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -1,17 +1,5 @@ /**: ros__parameters: - launch_stop_line: true - launch_crosswalk: true - launch_traffic_light: true - launch_intersection: true - launch_blind_spot: true - launch_detection_area: true - launch_virtual_traffic_light: false # disabled by default to not confuse newcomers - launch_occlusion_spot: false - launch_no_stopping_area: true - launch_run_out: false - launch_speed_bump: false - launch_out_of_lane: true forward_path_length: 1000.0 backward_path_length: 5.0 stop_line_extend_length: 5.0 @@ -20,3 +8,19 @@ system_delay: 0.5 delay_response_time: 0.5 is_publish_debug_path: false # publish all debug path with lane id in each module + launch_modules: + - behavior_velocity_planner::CrosswalkModulePlugin + - behavior_velocity_planner::WalkwayModulePlugin + - behavior_velocity_planner::TrafficLightModulePlugin + - behavior_velocity_planner::IntersectionModulePlugin # Intersection module should be before merge from private to declare intersection parameters. + - behavior_velocity_planner::MergeFromPrivateModulePlugin + - behavior_velocity_planner::BlindSpotModulePlugin + - behavior_velocity_planner::DetectionAreaModulePlugin + # behavior_velocity_planner::VirtualTrafficLightModulePlugin + - behavior_velocity_planner::NoStoppingAreaModulePlugin # No stopping area module requires all the stop line. Therefore this modules should be placed at the bottom. + - behavior_velocity_planner::StopLineModulePlugin # Permanent stop line module should be after no stopping area + # behavior_velocity_planner::OcclusionSpotModulePlugin + # behavior_velocity_planner::RunOutModulePlugin + # behavior_velocity_planner::SpeedBumpModulePlugin + - behavior_velocity_planner::OutOfLaneModulePlugin + - behavior_velocity_planner::NoDrivableLaneModulePlugin diff --git a/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg b/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg index 08f1df2e968f..f48e1ad630e5 100644 --- a/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg +++ b/planning/behavior_velocity_planner/docs/BehaviorVelocityPlanner-Architecture.drawio.svg @@ -1,25 +1,27 @@ + + + - - + + - +
-
+
@@ -28,7 +30,7 @@
- /per... + /per... @@ -40,34 +42,34 @@ - +
-
+
updateSceneModuleInstance -
+
- updateSceneModuleInstance + updateSceneModuleInstance - +
-
+
@@ -76,18 +78,18 @@
- add/delete modules + add/delete modules - +
-
+
@@ -102,12 +104,12 @@ - +
-
+
scene_modules
@@ -120,12 +122,12 @@ - +
-
+
@@ -134,18 +136,18 @@
- Turn RIght + Turn RIght - +
-
+
@@ -166,12 +168,12 @@ - +
-
+
@@ -180,18 +182,18 @@
- Turn Left + Turn Left - +
-
+
@@ -216,34 +218,34 @@ - +
-
+
updateSceneModuleInstance -
+
- updateSceneModuleInstance + updateSceneModuleInstance - +
-
+
@@ -252,18 +254,18 @@
- add/delete modules + add/delete modules - +
-
+
@@ -278,12 +280,12 @@ - +
-
+
scene_modules
@@ -296,12 +298,12 @@ - +
-
+
@@ -316,12 +318,12 @@ - +
-
+
@@ -342,12 +344,12 @@ - +
-
+
@@ -362,12 +364,12 @@ - +
-
+
@@ -394,34 +396,34 @@ - +
-
+
updateSceneModuleInstance -
+
- updateSceneModuleInstance + updateSceneModuleInstance - +
-
+
@@ -430,18 +432,18 @@
- add/delete modules + add/delete modules - +
-
+
@@ -456,12 +458,12 @@ - +
-
+
scene_modules
@@ -474,12 +476,12 @@ - +
-
+
@@ -494,12 +496,12 @@ - +
-
+
@@ -520,32 +522,32 @@ - +
-
+
- Stoplin 2 + Stopline 2
- Stoplin 2 + Stopline 2 - +
-
+
@@ -569,370 +571,1034 @@ + + + + + Detection Area + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Detection Area 1 +
+
+
+ + Detection Area 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Detection Area 2 +
+
+
+ + Detection Area 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + Run out + + + + + +
+
+
+ updateSceneModuleInstance +
+
+
+
+ + updateSceneModuleInstance + + + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + + + + +
+
+
+ modifyPathVelocity +
+
+
+ + modifyPathVelocity + + + + + + +
+
+
+ scene_modules +
+
+
+ + scene_modules + + + + + + +
+
+
+ Run out 1 +
+
+
+ + Run out 1 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + + + + +
+
+
+ Run out 2 +
+
+
+ + Run out 2 + + + + + + +
+
+
+ Update +
+ Velocity +
+
+
+ + Update... + + + + + + + +
+
+
+ /perception/object_recognition/objects +
+
+
+ + /per... + + + + + + + +
+
+
+ /perception/prediction/objects +
+
+
+ + /per... + + + + + + + +
+
+
+ /vehicle/status/twist +
+
+
+ + /veh... + + - +
-
+
+
+ /planning/behavior_planning/path_with_lane_id +
+
+
+ + /pla... + + + + + + + Behavior Velocity Planner + + + + + +
+
+
+ UpdateModuleInstance +
+
+
+ + UpdateModuleInstance + + + + + + +
+
+
+ PlanVelocity +
+
+
+ + PlanVelocity + + + + + + +
+
+
+ RefinePath +
+
+
+ + RefinePath + + + + + + + +
+
+
+ path_with_lane_id +
+
+
+ + path_w... + + + + + + + +
+
+
+ path_with_lane_id +
+
+
+ + path_w... + + + + + + + +
+
+
+ path_with_lane_id +
+
+
+ + path_w... + + + + + + + +
+
+
+ Updated +
+ path_with_lane_id +
+
+
+ + Update... + + + + + + + +
+
+
+ Updated +
+ path_with_lane_id +
+
+
+ + Update... + + + + + + + +
+
+
+ Data +
+
+
+ + Data + + + + + + +
+
- /planning/behavior_planning/path_with_lane_id + Planner Data
- /pla... + Planner Data - - - - - Behavior Velocity Planner + + + + + SceneManagers - + + + + + Crosswalk + + - +
-
+
- UpdateModuleInstance + updateSceneModuleInstance +
- UpdateModuleInstance + updateSceneModuleInstance - + + + + + +
+
+
+ add/delete modules +
+
+
+ + add/delete modules + + + - +
-
+
- PlanVelocity + modifyPathVelocity
- PlanVelocity + modifyPathVelocity - + - +
-
-
- RefinePath +
+
+ scene_modules
- RefinePath + scene_modules - - + - -
-
+ +
+
- path_with_lane_id + Crosswalk 1
- path_w... + Crosswalk 1 - - + - -
-
+ +
+
- path_with_lane_id + Update +
+ Velocity
- path_w... + Update... - - + + + + + - -
-
+ +
+
- path_with_lane_id + Crosswalk2
- path_w... + Crosswalk2 - - + - -
-
+ +
+
- Updated -
- path_with_lane_id + Update +
+ Velocity
- Update... + Update... - - + + + + + + + + + + + + + + + Intersection + + - -
-
+ +
+
- Updated -
- path_with_lane_id + updateSceneModuleInstance +
- Update... + updateSceneModuleInstance - - + + - +
-
+
- Data + add/delete modules
- Data + add/delete modules - - + - +
-
+
- /tf + modifyPathVelocity
- /tf + modifyPathVelocity - - + - +
-
-
- /perception/object_recognition/objects +
+
+ scene_modules
- /per... + scene_modules - - + - +
-
+
- /perception/prediction/objects + Intersection 1
- /per... + Intersection 1 - - + - +
-
+
- /vehicle/status/twist + Update +
+ Velocity
- /veh... + Update... - - + + + + + - +
-
+
- /map/semantic + Intersection 2
- /map... + Intersection 2 - + - +
-
+
- Planner Data + Update +
+ Velocity
- Planner Data + Update... - - - - - SceneManagers - - - - + + + - Crosswalk + Virtual Traffic Light - + - +
-
+
updateSceneModuleInstance -
+
- updateSceneModuleInstance + updateSceneModuleInstance - - + + - -
-
+ +
+
@@ -941,18 +1607,18 @@
- add/delete modules + add/delete modules - + - +
-
+
@@ -961,56 +1627,56 @@
- modifyPathVelocity + modifyPathVelocity - + - +
-
+
scene_modules
- scene_modules + scene_modules - + - +
-
+
- Crosswalk 1 + Virtual Traffic Light 1
- Crosswalk 1 + Virtual Traffic Light 1 - + - +
-
+
@@ -1021,42 +1687,42 @@
- Update... + Update... - - - - - - + + + + + + - +
-
+
- Crosswalk2 + Virtual Traffic Light 2
- Crosswalk2 + Virtual Traffic Light 2 - + - +
-
+
@@ -1067,56 +1733,46 @@
- Update... + Update... - - - - - - - - - - - - - + + + - Intersection + Traffic LIght - + - +
-
+
updateSceneModuleInstance -
+
- updateSceneModuleInstance + updateSceneModuleInstance - - + + - +
-
+
@@ -1125,18 +1781,18 @@
- add/delete modules + add/delete modules - + - +
-
+
@@ -1145,56 +1801,56 @@
- modifyPathVelocity + modifyPathVelocity - + - +
-
+
scene_modules
- scene_modules + scene_modules - + - +
-
+
- Intersection 1 + Traffic LIght 1
- Intersection 1 + Traffic LIght 1 - + - +
-
+
@@ -1205,42 +1861,42 @@
- Update... + Update... - - - - - + + + + + - +
-
+
- Intersection 2 + Traffic Light 2
- Intersection 2 + Traffic Light 2 - + - +
-
+
@@ -1251,46 +1907,52 @@
- Update... + Update... - - - + + + + + + + + + - Virtual Traffic Light + No Stopping Area - + - +
-
+
updateSceneModuleInstance -
+
- updateSceneModuleInstance + updateSceneModuleInstance - - + + - +
-
+
@@ -1299,18 +1961,18 @@
- add/delete modules + add/delete modules - + - +
-
+
@@ -1319,56 +1981,56 @@
- modifyPathVelocity + modifyPathVelocity - + - +
-
+
scene_modules
- scene_modules + scene_modules - + - +
-
+
- Virtual Traffic Light 1 + No Stopping Area1
- Virtual Traffic Light 1 + No Stopping Area1 - + - +
-
+
@@ -1379,42 +2041,42 @@
- Update... + Update... - - - - - + + + + + - +
-
+
- Virtual Traffic Light 2 + No Stopping Area2
- Virtual Traffic Light 2 + No Stopping Area2 - + - +
-
+
@@ -1425,46 +2087,46 @@
- Update... + Update... - - - + + + - Traffic LIght + Speed Bump - + - +
-
+
updateSceneModuleInstance -
+
- updateSceneModuleInstance + updateSceneModuleInstance - - + + - +
-
+
@@ -1473,18 +2135,18 @@
- add/delete modules + add/delete modules - + - +
-
+
@@ -1493,56 +2155,56 @@
- modifyPathVelocity + modifyPathVelocity - + - +
-
+
scene_modules
- scene_modules + scene_modules - + - +
-
+
- Traffic LIght 1 + Speed Bump 1
- Traffic LIght 1 + Speed Bump 1 - + - +
-
+
@@ -1553,42 +2215,42 @@
- Update... + Update... - - - - - + + + + + - +
-
+
- Traffic Ligh 2 + Speed Bump 2
- Traffic Ligh 2 + Speed Bump 2 - + - +
-
+
@@ -1599,52 +2261,48 @@
- Update... + Update... - - - - - - - - - + + + + + - No Stopping Area + No Drivable Lane - + - +
-
+
updateSceneModuleInstance -
+
- updateSceneModuleInstance + updateSceneModuleInstance - - + + - +
-
+
@@ -1653,18 +2311,18 @@
- add/delete modules + add/delete modules - + - +
-
+
@@ -1673,56 +2331,56 @@
- modifyPathVelocity + modifyPathVelocity - + - +
-
+
scene_modules
- scene_modules + scene_modules - + - +
-
+
- No Stopping Area1 + No Drivable Lanelet 1
- No Stopping Area1 + No Drivable Lanelet 1 - + - +
-
+
@@ -1733,42 +2391,42 @@
- Update... + Update... - - - - - + + + + + - +
-
+
- No Stopping Area2 + No Drivable Lanelet 2
- No Stopping Area2 + No Drivable Lanelet 2 - + - +
-
+
@@ -1779,23 +2437,71 @@
- Update... + Update... + + + + + + + + + + + + + +
+
+
+ /map/semantic +
+
+
+ + /map... + + + + + + + +
+
+
+ /tf +
+
+
+ + /tf - +
-
+
- /planning/behavior_planning/path + /planning/behavior_planning/path
@@ -1805,13 +2511,13 @@ - - + + - - Viewer does not support full SVG 1.1 + + Text is not SVG - cannot display diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index c08cfcbfa32f..04dc3111fde4 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -18,6 +18,7 @@ + @@ -37,6 +38,7 @@ + @@ -63,6 +65,7 @@ + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 0f131eebcd40..018c21c1fcc9 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -63,6 +63,19 @@ ament_cmake_ros ament_lint_auto autoware_lint_common + behavior_velocity_blind_spot_module + behavior_velocity_crosswalk_module + behavior_velocity_detection_area_module + behavior_velocity_intersection_module + behavior_velocity_no_drivable_lane_module + behavior_velocity_no_stopping_area_module + behavior_velocity_occlusion_spot_module + behavior_velocity_out_of_lane_module + behavior_velocity_run_out_module + behavior_velocity_speed_bump_module + behavior_velocity_stop_line_module + behavior_velocity_traffic_light_module + behavior_velocity_virtual_traffic_light_module ament_cmake diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 3645ffca3345..12503f743b0f 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -33,6 +33,7 @@ #include #include +#include namespace { @@ -136,54 +137,8 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio this->declare_parameter("ego_nearest_yaw_threshold"); // Initialize PlannerManager - if (this->declare_parameter("launch_crosswalk")) { - planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::CrosswalkModulePlugin"); - planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::WalkwayModulePlugin"); - } - if (this->declare_parameter("launch_traffic_light")) { - planner_manager_.launchScenePlugin( - *this, "behavior_velocity_planner::TrafficLightModulePlugin"); - } - if (this->declare_parameter("launch_intersection")) { - // intersection module should be before merge from private to declare intersection parameters - planner_manager_.launchScenePlugin( - *this, "behavior_velocity_planner::IntersectionModulePlugin"); - planner_manager_.launchScenePlugin( - *this, "behavior_velocity_planner::MergeFromPrivateModulePlugin"); - } - if (this->declare_parameter("launch_blind_spot")) { - planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::BlindSpotModulePlugin"); - } - if (this->declare_parameter("launch_detection_area")) { - planner_manager_.launchScenePlugin( - *this, "behavior_velocity_planner::DetectionAreaModulePlugin"); - } - if (this->declare_parameter("launch_virtual_traffic_light")) { - planner_manager_.launchScenePlugin( - *this, "behavior_velocity_planner::VirtualTrafficLightModulePlugin"); - } - // this module requires all the stop line.Therefore this modules should be placed at the bottom. - if (this->declare_parameter("launch_no_stopping_area")) { - planner_manager_.launchScenePlugin( - *this, "behavior_velocity_planner::NoStoppingAreaModulePlugin"); - } - // permanent stop line module should be after no stopping area - if (this->declare_parameter("launch_stop_line")) { - planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::StopLineModulePlugin"); - } - // to calculate ttc it's better to be after stop line - if (this->declare_parameter("launch_occlusion_spot")) { - planner_manager_.launchScenePlugin( - *this, "behavior_velocity_planner::OcclusionSpotModulePlugin"); - } - if (this->declare_parameter("launch_run_out")) { - planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::RunOutModulePlugin"); - } - if (this->declare_parameter("launch_speed_bump")) { - planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::SpeedBumpModulePlugin"); - } - if (this->declare_parameter("launch_out_of_lane")) { - planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::OutOfLaneModulePlugin"); + for (const auto & name : declare_parameter>("launch_modules")) { + planner_manager_.launchScenePlugin(*this, name); } } diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 061a46f01f21..ccb950fe47d6 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -53,6 +53,12 @@ std::shared_ptr generateNode() const auto motion_velocity_smoother_dir = ament_index_cpp::get_package_share_directory("motion_velocity_smoother"); + const auto get_behavior_velocity_module_config = [](const std::string & module) { + const auto package_name = "behavior_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + test_utils::updateNodeOptions( node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", @@ -61,35 +67,24 @@ std::shared_ptr generateNode() motion_velocity_smoother_dir + "/config/default_motion_velocity_smoother.param.yaml", motion_velocity_smoother_dir + "/config/Analytical.param.yaml", behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - behavior_velocity_planner_dir + "/config/blind_spot.param.yaml", - behavior_velocity_planner_dir + "/config/crosswalk.param.yaml", - behavior_velocity_planner_dir + "/config/detection_area.param.yaml", - behavior_velocity_planner_dir + "/config/intersection.param.yaml", - behavior_velocity_planner_dir + "/config/no_stopping_area.param.yaml", - behavior_velocity_planner_dir + "/config/occlusion_spot.param.yaml", - behavior_velocity_planner_dir + "/config/run_out.param.yaml", - behavior_velocity_planner_dir + "/config/speed_bump.param.yaml", - behavior_velocity_planner_dir + "/config/stop_line.param.yaml", - behavior_velocity_planner_dir + "/config/traffic_light.param.yaml", - behavior_velocity_planner_dir + "/config/virtual_traffic_light.param.yaml", - behavior_velocity_planner_dir + "/config/out_of_lane.param.yaml"}); - - node_options.append_parameter_override("launch_stop_line", true); - node_options.append_parameter_override("launch_crosswalk", true); - node_options.append_parameter_override("launch_traffic_light", true); - node_options.append_parameter_override("launch_intersection", true); - node_options.append_parameter_override("launch_blind_spot", true); - node_options.append_parameter_override("launch_detection_area", true); - node_options.append_parameter_override( - "launch_virtual_traffic_light", false); // TODO(Kyoichi Sugahara) set to true - node_options.append_parameter_override( - "launch_occlusion_spot", false); // TODO(Kyoichi Sugahara) set to true - node_options.append_parameter_override("launch_no_stopping_area", true); - node_options.append_parameter_override( - "launch_run_out", false); // TODO(Kyoichi Sugahara) set to true - node_options.append_parameter_override( - "launch_speed_bump", false); // TODO(Kyoichi Sugahara) set to true - node_options.append_parameter_override("launch_out_of_lane", true); + get_behavior_velocity_module_config("blind_spot"), + get_behavior_velocity_module_config("crosswalk"), + get_behavior_velocity_module_config("detection_area"), + get_behavior_velocity_module_config("intersection"), + get_behavior_velocity_module_config("no_stopping_area"), + get_behavior_velocity_module_config("occlusion_spot"), + get_behavior_velocity_module_config("run_out"), + get_behavior_velocity_module_config("speed_bump"), + get_behavior_velocity_module_config("stop_line"), + get_behavior_velocity_module_config("traffic_light"), + get_behavior_velocity_module_config("virtual_traffic_light"), + get_behavior_velocity_module_config("out_of_lane")}); + + // TODO(Takagi, Isamu): set launch_modules + // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light + // TODO(Kyoichi Sugahara) set to true launch_occlusion_spot + // TODO(Kyoichi Sugahara) set to true launch_run_out + // TODO(Kyoichi Sugahara) set to true launch_speed_bump return std::make_shared(node_options); } diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt index 806097cdff86..89745a18065a 100644 --- a/planning/behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt @@ -14,4 +14,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/run_out.param.yaml rename to planning/behavior_velocity_run_out_module/config/run_out.param.yaml diff --git a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt index 45931b98e502..a96eb3398e5c 100644 --- a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt +++ b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt @@ -12,4 +12,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/speed_bump.param.yaml b/planning/behavior_velocity_speed_bump_module/config/speed_bump.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/speed_bump.param.yaml rename to planning/behavior_velocity_speed_bump_module/config/speed_bump.param.yaml diff --git a/planning/behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_stop_line_module/CMakeLists.txt index e6205fa66bf9..3c8cc6a4f962 100644 --- a/planning/behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_stop_line_module/CMakeLists.txt @@ -11,4 +11,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/stop_line.param.yaml rename to planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml diff --git a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt index 0433c0d0a6c5..cf9392fa0568 100644 --- a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt @@ -11,4 +11,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/traffic_light.param.yaml rename to planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml diff --git a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt index 5e909ebd2617..48ffa35cbae5 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt @@ -11,4 +11,4 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp ) -ament_auto_package() +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml b/planning/behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml similarity index 100% rename from planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml rename to planning/behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index b047de1b8ff6..ddc304c99ea6 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -101,7 +101,7 @@ using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; enum class ModuleName { UNKNOWN = 0, - PULL_OUT, + START_PLANNER, }; class PlanningInterfaceTestManager diff --git a/planning/planning_test_utils/src/planning_interface_test_manager.cpp b/planning/planning_test_utils/src/planning_interface_test_manager.cpp index 0e897e65394d..6489f32dc4cd 100644 --- a/planning/planning_test_utils/src/planning_interface_test_manager.cpp +++ b/planning/planning_test_utils/src/planning_interface_test_manager.cpp @@ -110,7 +110,7 @@ void PlanningInterfaceTestManager::publishInitialPose( rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift, ModuleName module_name) { - if (module_name == ModuleName::PULL_OUT) { + if (module_name == ModuleName::START_PLANNER) { test_utils::publishToTargetNode( test_node_, target_node, topic_name, initial_pose_pub_, test_utils::makeInitialPoseFromLaneId(10291)); @@ -251,7 +251,7 @@ void PlanningInterfaceTestManager::publishNominalRoute( void PlanningInterfaceTestManager::publishBehaviorNominalRoute( rclcpp::Node::SharedPtr target_node, std::string topic_name, ModuleName module_name) { - if (module_name == ModuleName::PULL_OUT) { + if (module_name == ModuleName::START_PLANNER) { test_utils::publishToTargetNode( test_node_, target_node, topic_name, behavior_normal_route_pub_, test_utils::makeBehaviorRouteFromLaneId(10291, 10333), 5); 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 2e53b5d246a9..0aa3cbd49e8e 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 @@ -12,7 +12,7 @@ - "avoidance_left" - "avoidance_right" - "goal_planner" - - "pull_out" + - "start_planner" - "intersection_occlusion" default_enable_list: @@ -27,5 +27,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 d7ccfdff777f..e28957cee161 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 a06a02367169..7a63d2606c8d 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 333beafc1239..8bed9ec41e9e 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 d464197e0360..46c8ea4125c9 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 7bfa72101ade..b60fa5365f2a 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", }