feat(tier4_planning_launch): add nearest search param (tier4#1582)

* feat(tier4_planning_launch): add nearest search param Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
boyali · Sep 28, 2022 · a7c736f · a7c736f
1 parent c592a29
commit a7c736f
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 0 deletions.
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
@@ -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/...aunch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/...aunch/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/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/...ng_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
@@ -2,6 +2,7 @@
   <!-- common param -->
   <arg name="tier4_planning_launch_param_path"/>
   <arg name="common_param_path" default="$(var tier4_planning_launch_param_path)/scenario_planning/common/common.param.yaml"/>
+  <arg name="nearest_search_param_path" default="$(var tier4_planning_launch_param_path)/scenario_planning/common/nearest_search.param.yaml"/>
   <arg name="vehicle_info_param_file"/>
 
   <!-- pointcloud container -->
@@ -40,6 +41,7 @@
       <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
         <arg name="smoother_type" value="$(var smoother_type)"/>
         <arg name="common_param_path" value="$(var common_param_path)"/>
+        <arg name="nearest_search_param_path" value="$(var nearest_search_param_path)"/>
         <arg name="param_path" value="$(var param_path)"/>
         <arg name="smoother_param_path" value="$(var tier4_planning_launch_param_path)/scenario_planning/common/motion_velocity_smoother/$(var smoother_type).param.yaml"/>
       </include>