feat(planning_launch): add nearest search param (#437)

* feat(planning_launch): add nearest search param Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix 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>
autowarefoundation · Aug 17, 2022 · a21d333 · a21d333
1 parent 1e2127d
commit a21d333
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 0 deletions.
diff --git a/planning_launch/config/scenario_planning/common/nearest_search.param.yaml b/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
@@ -31,6 +31,16 @@
 
 
 def generate_launch_description():
+    nearest_search_param_path = os.path.join(
+        get_package_share_directory("planning_launch"),
+        "config",
+        "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(
         get_package_share_directory("planning_launch"),
@@ -138,6 +148,7 @@ def generate_launch_description():
             ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"),
         ],
         parameters=[
+            nearest_search_param,
             side_shift_param,
             avoidance_param,
             lane_change_param,
@@ -368,6 +379,7 @@ def generate_launch_description():
         parameters=[
             behavior_velocity_planner_param,
             common_param,
+            nearest_search_param,
             motion_velocity_smoother_param,
             smoother_type_param,
             blind_spot_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
@@ -40,6 +40,16 @@ def generate_launch_description():
     with open(common_param_path, "r") as f:
         common_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
+    nearest_search_param_path = os.path.join(
+        get_package_share_directory("planning_launch"),
+        "config",
+        "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(
         get_package_share_directory("planning_launch"),
@@ -63,6 +73,7 @@ def generate_launch_description():
             ("~/output/path", "obstacle_avoidance_planner/trajectory"),
         ],
         parameters=[
+            nearest_search_param,
             obstacle_avoidance_planner_param,
             {"is_showing_debug_info": False},
             {"is_stopping_if_outside_drivable_area": True},
@@ -103,6 +114,7 @@ def generate_launch_description():
             ("~/input/odometry", "/localization/kinematic_state"),
         ],
         parameters=[
+            nearest_search_param,
             surround_obstacle_checker_param,
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
@@ -136,6 +148,7 @@ def generate_launch_description():
         ],
         parameters=[
             common_param,
+            nearest_search_param,
             obstacle_cruise_planner_param,
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
@@ -188,6 +201,7 @@ def generate_launch_description():
         ],
         parameters=[
             common_param,
+            nearest_search_param,
             obstacle_stop_planner_param,
             obstacle_stop_planner_acc_param,
             {"enable_slow_down": False},

diff --git a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -1,6 +1,7 @@
 <launch>
   <!-- common param -->
   <arg name="common_param_path" default="$(find-pkg-share planning_launch)/config/scenario_planning/common/common.param.yaml"/>
+  <arg name="nearest_search_param_path" default="$(find-pkg-share planning_launch)/config/scenario_planning/common/nearest_search.param.yaml"/>
 
   <!-- pointcloud container -->
   <arg name="use_pointcloud_container"/>
@@ -33,6 +34,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="$(find-pkg-share planning_launch)/config/scenario_planning/common/motion_velocity_smoother/$(var smoother_type).param.yaml"/>
     </include>