add nearest search param

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

launch/tier4_control_launch/config/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]

launch/tier4_control_launch/launch/control.launch.py

@@ -44,6 +44,14 @@ def launch_setup(context, *args, **kwargs):
     with open(lat_controller_param_path, "r") as f:
         lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
+    nearest_search_param_path = os.path.join(
+        LaunchConfiguration("tier4_control_launch_param_path").perform(context),
+        "common",
+        "nearest_search.param.yaml",
+    )
+    with open(nearest_search_param_path, "r") as f:
+        nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
     lon_controller_param_path = os.path.join(
         LaunchConfiguration("tier4_control_launch_param_path").perform(context),
         "trajectory_follower",
@@ -94,6 +102,7 @@ def launch_setup(context, *args, **kwargs):
                 "ctrl_period": 0.03,
                 "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"),
             },
+            nearest_search_param,
             lon_controller_param,
             lat_controller_param,
             vehicle_info_param,
@@ -117,7 +126,7 @@ def launch_setup(context, *args, **kwargs):
                 "/control/trajectory_follower/lateral/predicted_trajectory",
             ),
         ],
-        parameters=[lane_departure_checker_param, vehicle_info_param],
+        parameters=[nearest_search_param_path, lane_departure_checker_param, vehicle_info_param],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -203,6 +212,7 @@ def launch_setup(context, *args, **kwargs):
             ("control_mode_request", "/control/control_mode_request"),
         ],
         parameters=[
+            nearest_search_param_path,
             operation_mode_transition_manager_param,
             vehicle_info_param,
         ],