diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml new file mode 100644 index 0000000000000..eb6709764ce3e --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # ego + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] 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 4d7ec68237e43..d51501849da4a 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 @@ -38,6 +38,16 @@ def launch_setup(context, *args, **kwargs): with open(vehicle_info_param_path, "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + # nearest search parameter + nearest_search_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "common", + "nearest_search.param.yaml", + ) + with open(nearest_search_param_path, "r") as f: + nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] + # behavior path planner side_shift_param_path = os.path.join( LaunchConfiguration("tier4_planning_launch_param_path").perform(context), @@ -138,6 +148,7 @@ def launch_setup(context, *args, **kwargs): ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"), ], parameters=[ + nearest_search_param, side_shift_param, avoidance_param, lane_change_param, @@ -353,6 +364,7 @@ def launch_setup(context, *args, **kwargs): ("~/output/traffic_signal", "debug/traffic_signal"), ], parameters=[ + nearest_search_param, behavior_velocity_planner_param, blind_spot_param, crosswalk_param, diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index 5991b97cc76b7..f86b9a8858bff 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -46,6 +46,16 @@ def launch_setup(context, *args, **kwargs): with open(common_param_path, "r") as f: common_param = yaml.safe_load(f)["/**"]["ros__parameters"] + # nearest search parameter + nearest_search_param_path = os.path.join( + LaunchConfiguration("tier4_planning_launch_param_path").perform(context), + "scenario_planning", + "common", + "nearest_search.param.yaml", + ) + with open(nearest_search_param_path, "r") as f: + nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] + # obstacle avoidance planner obstacle_avoidance_planner_param_path = os.path.join( LaunchConfiguration("tier4_planning_launch_param_path").perform(context), @@ -68,6 +78,7 @@ def launch_setup(context, *args, **kwargs): ("~/output/path", "obstacle_avoidance_planner/trajectory"), ], parameters=[ + nearest_search_param, obstacle_avoidance_planner_param, vehicle_info_param, {"is_showing_debug_info": False}, @@ -158,6 +169,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), ], parameters=[ + nearest_search_param, common_param, obstacle_stop_planner_param, obstacle_stop_planner_acc_param, @@ -193,6 +205,7 @@ def launch_setup(context, *args, **kwargs): ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), ], parameters=[ + nearest_search_param, common_param, obstacle_cruise_planner_param, ], diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index d49078612fe0f..1bcc85f6cf33b 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -2,6 +2,7 @@ + @@ -40,6 +41,7 @@ +