feat: load global parameter (#26)

* feat: load global parameter Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * ci: run pre-commit Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * refactor: address review Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * chore: cosmetic change Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>
tier4 · Feb 24, 2022 · ea5525a · ea5525a
1 parent b63cf79
commit ea5525a
Show file tree

Hide file tree

Showing 15 changed files with 36 additions and 80 deletions.
diff --git a/README.md b/README.md
@@ -64,7 +64,6 @@ When you create your own sensor kit packages, please refer to `aip_*_launch` as
 <!-- LiDAR Driver -->
 <include file="$(find-pkg-share aip_xxx_launch)/launch/lidar.launch.xml">
     <arg name="launch_driver" value="$(var launch_driver)" />
-    <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
 </include>
 

diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml
@@ -4,7 +4,6 @@
   <arg name="use_concat_filter" default="true" />
   <arg name="use_radius_search" default="false" />
   <arg name="use_dji_driver" default="false" />
-  <arg name="vehicle_param_file" />
   <arg name="vehicle_mirror_param_file" />
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
@@ -20,15 +19,13 @@
         <arg name="port" value="2368"/>
         <arg name="scan_phase" value="180.0" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
 
     <include file="$(find-pkg-share common_sensor_launch)/launch/livox_horizon.launch.py" if="$(var use_dji_driver)">
       <arg name="multi_topic" value="1" />
       <arg name="launch_driver" value="$(var launch_driver)" />
-      <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
       <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
     </include>
 

diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml
@@ -1,7 +1,6 @@
 <launch>
 
   <arg name="launch_driver" default="true" description="do launch driver"/>
-  <arg name="vehicle_param_file" description="path to the file of vehicle info yaml"/>
   <arg name="vehicle_mirror_param_file" description="path to the file of vehicle mirror position yaml"/>
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
@@ -11,7 +10,6 @@
     <!-- LiDAR Driver -->
     <include file="$(find-pkg-share aip_x1_launch)/launch/lidar.launch.xml">
       <arg name="launch_driver" value="$(var launch_driver)" />
-      <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
       <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>

diff --git a/aip_x1_launch/launch/velodyne_VLP16.launch.xml b/aip_x1_launch/launch/velodyne_VLP16.launch.xml
@@ -14,7 +14,6 @@
   <arg name="scan_phase" default="0.0"/>
   <arg name="view_direction" default="0.0" />
   <arg name="view_width" default="6.28" />
-  <arg name="vehicle_param_file" />
   <arg name="vehicle_mirror_param_file" />
 
   <include file="$(find-pkg-share aip_x1_launch)/launch/velodyne_node_container.launch.py">
@@ -30,7 +29,6 @@
     <arg name="scan_phase" value="$(var scan_phase)"/>
     <arg name="view_direction" value="$(var view_direction)" />
     <arg name="view_width" value="$(var view_width)" />
-    <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
     <arg name="use_intra_process" value="true" />
     <arg name="use_multithread" value="false" />

diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py
@@ -22,21 +22,19 @@
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
-import yaml
 
 
 def get_vehicle_info(context):
-    path = LaunchConfiguration("vehicle_param_file").perform(context)
-    with open(path, "r") as f:
-        p = yaml.safe_load(f)["/**"]["ros__parameters"]
-    p["vehicle_length"] = p["front_overhang"] + p["wheel_base"] + p["rear_overhang"]
-    p["vehicle_width"] = p["wheel_tread"] + p["left_overhang"] + p["right_overhang"]
-    p["min_longitudinal_offset"] = -p["rear_overhang"]
-    p["max_longitudinal_offset"] = p["front_overhang"] + p["wheel_base"]
-    p["min_lateral_offset"] = -(p["wheel_tread"] / 2.0 + p["right_overhang"])
-    p["max_lateral_offset"] = p["wheel_tread"] / 2.0 + p["left_overhang"]
+    gp = context.launch_configurations.get("ros_params", {})
+    p = {}
+    p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"]
+    p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"]
+    p["min_longitudinal_offset"] = -gp["rear_overhang"]
+    p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"]
+    p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"])
+    p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"]
     p["min_height_offset"] = 0.0
-    p["max_height_offset"] = p["vehicle_height"]
+    p["max_height_offset"] = gp["vehicle_height"]
     return p
 
 
@@ -210,7 +208,6 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("view_width", description="lidar angle: 0~6.28 [rad]")
     add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox")
     add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox")
-    add_launch_arg("vehicle_param_file", description="path to the file of vehicle info yaml")
     add_launch_arg(
         "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
     )

diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml
@@ -3,7 +3,6 @@
   <arg name="launch_driver" default="true" />
   <arg name="use_concat_filter" default="true" />
   <arg name="vehicle_id" default="$(env VEHICLE_ID default)" />
-  <arg name="vehicle_param_file" />
   <arg name="vehicle_mirror_param_file" />
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
@@ -24,7 +23,6 @@
         <arg name="return_mode" value="Strongest" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -42,7 +40,6 @@
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -61,7 +58,6 @@
         <arg name="return_mode" value="Dual" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -80,7 +76,6 @@
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -99,7 +94,6 @@
         <arg name="return_mode" value="Strongest" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -118,7 +112,6 @@
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -137,7 +130,6 @@
         <arg name="return_mode" value="Strongest" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -156,7 +148,6 @@
         <arg name="return_mode" value="First" />
         <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>

diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py
@@ -28,17 +28,16 @@
 
 
 def get_vehicle_info(context):
-    path = LaunchConfiguration("vehicle_param_file").perform(context)
-    with open(path, "r") as f:
-        p = yaml.safe_load(f)["/**"]["ros__parameters"]
-    p["vehicle_length"] = p["front_overhang"] + p["wheel_base"] + p["rear_overhang"]
-    p["vehicle_width"] = p["wheel_tread"] + p["left_overhang"] + p["right_overhang"]
-    p["min_longitudinal_offset"] = -p["rear_overhang"]
-    p["max_longitudinal_offset"] = p["front_overhang"] + p["wheel_base"]
-    p["min_lateral_offset"] = -(p["wheel_tread"] / 2.0 + p["right_overhang"])
-    p["max_lateral_offset"] = p["wheel_tread"] / 2.0 + p["left_overhang"]
+    gp = context.launch_configurations.get("ros_params", {})
+    p = {}
+    p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"]
+    p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"]
+    p["min_longitudinal_offset"] = -gp["rear_overhang"]
+    p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"]
+    p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"])
+    p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"]
     p["min_height_offset"] = 0.0
-    p["max_height_offset"] = p["vehicle_height"]
+    p["max_height_offset"] = gp["vehicle_height"]
     return p
 
 
@@ -255,7 +254,6 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("frame_id", "pandar")
     add_launch_arg("input_frame", LaunchConfiguration("base_frame"))
     add_launch_arg("output_frame", LaunchConfiguration("base_frame"))
-    add_launch_arg("vehicle_param_file")
     add_launch_arg("vehicle_mirror_param_file")
     add_launch_arg("use_multithread", "true")
     add_launch_arg("use_intra_process", "true")

diff --git a/aip_x2_launch/launch/sensing.launch.xml b/aip_x2_launch/launch/sensing.launch.xml
@@ -1,7 +1,6 @@
 <launch>
 
   <arg name="launch_driver" default="true" description="do launch driver"/>
-  <arg name="vehicle_param_file" description="path to the file of vehicle info yaml"/>
   <arg name="vehicle_mirror_param_file" description="path to the file of vehicle mirror position yaml"/>
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
@@ -11,7 +10,6 @@
     <!-- LiDAR Driver -->
     <include file="$(find-pkg-share aip_x2_launch)/launch/lidar.launch.xml">
       <arg name="launch_driver" value="$(var launch_driver)" />
-      <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
       <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>

diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml
@@ -3,7 +3,6 @@
   <arg name="launch_driver" default="true" />
   <arg name="use_concat_filter" default="true" />
   <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
-  <arg name="vehicle_param_file" />
   <arg name="vehicle_mirror_param_file" />
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
@@ -21,7 +20,6 @@
         <!-- TODO(fred-apex-ai) sensor_timestamp not supported anymore in ros2 driver but default values changed here -->
         <!-- <arg name="sensor_timestamp" value="false" /> -->
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -36,7 +34,6 @@
         <!-- TODO(fred-apex-ai) sensor_timestamp not supported anymore in ros2 driver but default values changed here -->
         <!-- <arg name="sensor_timestamp" value="false" /> -->
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -51,7 +48,6 @@
         <!-- TODO(fred-apex-ai) sensor_timestamp not supported anymore in ros2 driver but default values changed here -->
         <!-- <arg name="sensor_timestamp" value="false" /> -->
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -66,7 +62,6 @@
         <!-- TODO(fred-apex-ai) sensor_timestamp not supported anymore in ros2 driver but default values changed here -->
         <!-- <arg name="sensor_timestamp" value="false" /> -->
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -77,7 +72,6 @@
         <arg name="sensor_frame" value="livox_front_left" />
         <arg name="bd_code_param_path" value="$(find-pkg-share individual_params)/config/$(env VEHICLE_ID default)/aip_xx1/livox_front_left_bd_code.param.yaml" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group>
@@ -88,7 +82,6 @@
         <arg name="sensor_frame" value="livox_front_right" />
         <arg name="bd_code_param_path" value="$(find-pkg-share individual_params)/config/$(env VEHICLE_ID default)/aip_xx1/livox_front_right_bd_code.param.yaml" />
         <arg name="launch_driver" value="$(var launch_driver)" />
-        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       </include>
     </group> -->

diff --git a/aip_xx1_launch/launch/sensing.launch.xml b/aip_xx1_launch/launch/sensing.launch.xml
@@ -1,7 +1,6 @@
 <launch>
 
   <arg name="launch_driver" default="true" description="do launch driver"/>
-  <arg name="vehicle_param_file" description="path to the file of vehicle info yaml"/>
   <arg name="vehicle_mirror_param_file" description="path to the file of vehicle mirror position yaml"/>
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
@@ -11,7 +10,6 @@
     <!-- LiDAR Driver -->
     <include file="$(find-pkg-share aip_xx1_launch)/launch/lidar.launch.xml">
       <arg name="launch_driver" value="$(var launch_driver)" />
-      <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
       <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>

diff --git a/common_sensor_launch/launch/livox_horizon.launch.py b/common_sensor_launch/launch/livox_horizon.launch.py
@@ -24,21 +24,19 @@
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
-import yaml
 
 
 def get_vehicle_info(context):
-    path = LaunchConfiguration("vehicle_param_file").perform(context)
-    with open(path, "r") as f:
-        p = yaml.safe_load(f)["/**"]["ros__parameters"]
-    p["vehicle_length"] = p["front_overhang"] + p["wheel_base"] + p["rear_overhang"]
-    p["vehicle_width"] = p["wheel_tread"] + p["left_overhang"] + p["right_overhang"]
-    p["min_longitudinal_offset"] = -p["rear_overhang"]
-    p["max_longitudinal_offset"] = p["front_overhang"] + p["wheel_base"]
-    p["min_lateral_offset"] = -(p["wheel_tread"] / 2.0 + p["right_overhang"])
-    p["max_lateral_offset"] = p["wheel_tread"] / 2.0 + p["left_overhang"]
+    gp = context.launch_configurations.get("ros_params", {})
+    p = {}
+    p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"]
+    p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"]
+    p["min_longitudinal_offset"] = -gp["rear_overhang"]
+    p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"]
+    p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"])
+    p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"]
     p["min_height_offset"] = 0.0
-    p["max_height_offset"] = p["vehicle_height"]
+    p["max_height_offset"] = gp["vehicle_height"]
     return p
 
 
@@ -180,7 +178,6 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("base_frame", "base_link")
     add_launch_arg("sensor_frame", "livox_frame")
     add_launch_arg("use_tag_filter", "true")
-    add_launch_arg("vehicle_param_file")
     add_launch_arg("vehicle_mirror_param_file")
 
     return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])
diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml
@@ -14,7 +14,6 @@
   <arg name="scan_phase" default="0.0"/>
   <arg name="view_direction" default="0.0" />
   <arg name="view_width" default="6.28" />
-  <arg name="vehicle_param_file" />
   <arg name="vehicle_mirror_param_file" />
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_node_container.launch.py">
@@ -30,7 +29,6 @@
     <arg name="scan_phase" value="$(var scan_phase)"/>
     <arg name="view_direction" value="$(var view_direction)" />
     <arg name="view_width" value="$(var view_width)" />
-    <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
     <arg name="use_intra_process" value="true" />
     <arg name="use_multithread" value="false" />

diff --git a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml
@@ -14,7 +14,6 @@
   <arg name="scan_phase" default="0.0"/>
   <arg name="view_direction" default="0.0" />
   <arg name="view_width" default="6.28" />
-  <arg name="vehicle_param_file" />
   <arg name="vehicle_mirror_param_file" />
 
   <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_node_container.launch.py">
@@ -30,7 +29,6 @@
     <arg name="scan_phase" value="$(var scan_phase)"/>
     <arg name="view_direction" value="$(var view_direction)" />
     <arg name="view_width" value="$(var view_width)" />
-    <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
     <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
     <arg name="use_intra_process" value="false" />
     <arg name="use_multithread" value="false" />