add pythonic lidar launch; merge radar / imu launch with logic branch

Signed-off-by: Yuxuan Liu <[email protected]>

aip_xx1_launch/config/lidar_gen2.yaml

@@ -0,0 +1,69 @@
+launches:
+  - sensor_type: hesai_OT128
+    namespace: top
+    parameters:
+      max_range: 300.0
+      sensor_frame: hesai_top
+      sensor_ip: "192.168.1.201"
+      data_port: 2368
+      scan_phase: 160
+      vertical_bins: 128
+  - sensor_type: hesai_XT32
+    namespace: front_left
+    parameters:
+      max_range: 300.0
+      sensor_frame: hesai_front_left
+      sensor_ip: "192.168.1.21"
+      data_port: 2369
+      scan_phase: 50.0
+      cloud_min_angle: 50
+      cloud_max_angle: 320
+      vertical_bins: 16
+      horizontal_ring_id: 0
+  - sensor_type: hesai_XT32
+    namespace: front_right
+    parameters:
+      max_range: 300.0
+      sensor_frame: hesai_front_right
+      sensor_ip: "192.168.1.22"
+      data_port: 2370
+      scan_phase: 310.0
+      cloud_min_angle: 40
+      cloud_max_angle: 310
+  - sensor_type: hesai_XT32
+    namespace: side_left
+    parameters:
+      max_range: 10.0
+      sensor_frame: hesai_side_left
+      sensor_ip: "192.168.1.23"
+      data_port: 2371
+      scan_phase: 90.0
+      cloud_min_angle: 90
+      cloud_max_angle: 270
+  - sensor_type: hesai_XT32
+    namespace: side_right
+    parameters:
+      max_range: 10.0
+      sensor_frame: hesai_side_right
+      sensor_ip: "192.168.1.24"
+      data_port: 2372
+      scan_phase: 270.0
+      cloud_min_angle: 90
+      cloud_max_angle: 270
+
+preprocessor:
+  input_topics:
+    - "/sensing/lidar/top/pointcloud_before_sync"
+    - "/sensing/lidar/side_left/pointcloud_before_sync"
+    - "/sensing/lidar/side_right/pointcloud_before_sync"
+    - "/sensing/lidar/front_left/pointcloud_before_sync"
+    - "/sensing/lidar/front_right/pointcloud_before_sync"
+  input_offset:
+    - 0.035
+    - 0.025
+    - 0.025
+    - 0.025
+    - 0.025
+  timeout_sec: 0.095
+  input_twist_topic_type: "twist"
+  publish_synchronized_pointcloud: true

aip_xx1_launch/config/lidar_launch.yaml

@@ -0,0 +1,70 @@
+launches:
+  - sensor_type: velodyne_VLS128
+    namespace: top
+    parameters:
+      max_range: 250.0
+      sensor_frame: velodyne_top
+      sensor_ip: "192.168.1.201"
+      data_port: 2368
+      scan_phase: 300.0
+      vertical_bins: 128
+      horizontal_ring_id: 64
+      horizontal_resolution: 0.4
+      is_channel_order_top2down: false
+  - sensor_type: velodyne_VLP16
+    namespace: left
+    parameters:
+      max_range: 5.0
+      sensor_frame: velodyne_left
+      sensor_ip: "192.168.1.202"
+      data_port: 2369
+      scan_phase: 180.0
+      cloud_min_angle: 300
+      cloud_max_angle: 60
+      vertical_bins: 16
+      horizontal_ring_id: 0
+      horizontal_resolution: 0.4
+      is_channel_order_top2down: false
+  - sensor_type: velodyne_VLP16
+    namespace: right
+    parameters:
+      max_range: 5.0
+      sensor_frame: velodyne_right
+      sensor_ip: "192.168.1.203"
+      data_port: 2370
+      scan_phase: 180.0
+      cloud_min_angle: 300
+      cloud_max_angle: 60
+      vertical_bins: 16
+      horizontal_ring_id: 0
+      horizontal_resolution: 0.4
+      is_channel_order_top2down: false
+  - sensor_type: velodyne_VLP16
+    namespace: rear
+    parameters:
+      max_range: 1.5
+      sensor_frame: velodyne_rear
+      sensor_ip: "192.168.1.204"
+      data_port: 2371
+      scan_phase: 180.0
+      cloud_min_angle: 300
+      cloud_max_angle: 60
+      vertical_bins: 16
+      horizontal_ring_id: 0
+      horizontal_resolution: 0.4
+      is_channel_order_top2down: false
+
+preprocessor:
+  input_topics:
+    - "/sensing/lidar/top/pointcloud"
+    - "/sensing/lidar/left/pointcloud"
+    - "/sensing/lidar/right/pointcloud"
+    - "/sensing/lidar/rear/ointcloud"
+  input_offset:
+    - 0.035
+    - 0.025
+    - 0.025
+    - 0.025
+  timeout_sec: 0.095
+  input_twist_topic_type: "twist"
+  publish_synchronized_pointcloud: false

aip_xx1_launch/launch/imu.launch.xml

@@ -1,14 +1,27 @@
 <launch>
 
+  <arg name="launch_driver" default="true" />
+  <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
+  <arg name="imu_connection" default="$(env IMU_CONNECTION serial)" description="serial or can" />
+
   <group>
     <push-ros-namespace namespace="imu"/>
 
-    <arg name="launch_driver" default="true" />
-    <arg name="interface" default="canImu"/>
-    <arg name="receiver_interval_sec" default="0.01"/>
-    <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
+    <!-- Use Serial Driver to connect to IMU  -->
+    <group if="$(eval &quot;'$(var imu_connection)'=='serial'&quot;)">
+      <push-ros-namespace namespace="tamagawa"/>
+      <node pkg="tamagawa_imu_driver" name="tag_serial_driver" exec="tag_serial_driver" if="$(var launch_driver)" respawn="true" respawn_delay="10.0">
+        <remap from="imu/data_raw" to="imu_raw" />
+        <param name="port" value="/dev/imu" />
+        <param name="imu_frame_id" value="tamagawa/imu_link" />
+      </node>
+    </group>
 
-    <group>
+    <!-- Use CAN to connect to IMU  -->
+    <group if="$(eval &quot;'$(var imu_connection)'=='can'&quot;)">
+      <arg name="interface" default="canImu"/>
+      <arg name="receiver_interval_sec" default="0.01"/>
+      <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
       <push-ros-namespace namespace="tamagawa"/>
       <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_receiver.launch.py">
         <arg name="interface" value="$(var interface)"/>
@@ -21,6 +34,7 @@
       </node>
     </group>
 
+    <!-- IMU Correction launch-->
     <arg name="imu_raw_name" default="tamagawa/imu_raw"/>
     <arg name="imu_corrector_param_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_xx1/imu_corrector.param.yaml"/>
     <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
@@ -29,6 +43,7 @@
       <arg name="param_file" value="$(var imu_corrector_param_file)"/>
     </include>
 
+    <!-- Gyro Bias Estimator-->
     <include file="$(find-pkg-share imu_corrector)/launch/gyro_bias_estimator.launch.xml">
       <arg name="input_imu_raw" value="$(var imu_raw_name)"/>
       <arg name="input_odom" value="/localization/kinematic_state"/>

aip_xx1_launch/launch/lidar.launch.xml.py

@@ -0,0 +1,172 @@
+# Copyright 2024 Tier IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+from launch import LaunchDescription
+from copy import deepcopy
+from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration, EnvironmentVariable
+from launch_ros.actions import PushRosNamespace
+from ament_index_python.packages import get_package_share_directory
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import os
+import yaml
+
+
+def generate_launch_dictionary():
+    path_dictionary = {
+        "hesai_OT128": AnyLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("common_sensor_launch"),
+                "launch",
+                "hesai_OT128.launch.xml",
+            )
+        ),
+        "hesai_XT32": AnyLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("common_sensor_launch"),
+                "launch",
+                "hesai_XT32.launch.xml",
+            )
+        ),
+        "velodyne_VLS128": AnyLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("common_sensor_launch"),
+                "launch",
+                "velodyne_VLS128.launch.xml",
+            )
+        ),
+        "velodyne_VLP16": AnyLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("common_sensor_launch"),
+                "launch",
+                "velodyne_VLP16.launch.xml",
+            )
+        ),
+        "livox_horizon": AnyLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("common_sensor_launch"),
+                "launch",
+                "livox_horizon.launch.py",
+            )
+        ),
+    }
+    return path_dictionary
+
+
+def load_sub_launches_from_yaml(context, *args, **kwargs):
+    def load_yaml(yaml_file_path):
+        with open(LaunchConfiguration(yaml_file_path).perform(context), "r") as f:
+            return yaml.safe_load(f)
+
+    config = load_yaml("config_file")
+
+    path_dictionary = generate_launch_dictionary()
+
+    base_parameters = {}
+    base_parameters["host_ip"] = LaunchConfiguration("host_ip").perform(context)
+    base_parameters["vehicle_mirror_param_file"] = LaunchConfiguration(
+        "vehicle_mirror_param_file"
+    ).perform(context)
+    base_parameters["launch_driver"] = LaunchConfiguration("launch_driver").perform(context)
+    base_parameters["vehicle_id"] = LaunchConfiguration("vehicle_id").perform(context)
+    base_parameters["pointcloud_container_name"] = LaunchConfiguration(
+        "pointcloud_container_name"
+    ).perform(context)
+    base_parameters["enable_blockage_diag"] = LaunchConfiguration(
+        "enable_blockage_diag"
+    ).perform(context)
+
+    sub_launch_actions = []
+    for launch in config["launches"]:
+        launch_parameters = deepcopy(base_parameters)
+        launch_parameters.update(launch["parameters"]) # dict
+        launch_parameter_list_tuple = [(str(k),str(v)) for k,v in launch_parameters.items()]
+        sub_launch_action = GroupAction(
+            [
+                PushRosNamespace(launch["namespace"]),
+                IncludeLaunchDescription(
+                    deepcopy(path_dictionary[launch["sensor_type"]]),
+                    launch_arguments=launch_parameter_list_tuple,
+                ),
+            ]
+        )
+        sub_launch_actions.append(sub_launch_action)
+
+    sub_launch_actions.append(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(
+                    get_package_share_directory("aip_xx1_launch"),
+                    "launch",
+                    "pointcloud_preprocessor.launch.py",
+                )
+            ),
+            launch_arguments=[
+                ("base_frame", "base_link"),
+                ("use_multithread", "true"),
+                ("use_intra_process", "true"),
+                ("use_pointcloud_container", LaunchConfiguration(
+                    "use_pointcloud_container"
+                )),
+                ("pointcloud_container_name", LaunchConfiguration(
+                    "pointcloud_container_name"
+                ))
+            ]
+        )
+    )
+    return [
+        GroupAction([PushRosNamespace("lidar"), *sub_launch_actions]),
+    ]
+
+
+def generate_launch_description():
+    # Define launch arguments
+    launch_arguments = []
+    config_file_arg = DeclareLaunchArgument(
+        "config_file",
+        default_value=os.path.join(
+            get_package_share_directory("aip_xx1_launch"), "config", "lidar_launch.yaml"
+        ),
+        description="Path to the configuration file",
+    )
+    launch_arguments.append(config_file_arg)
+
+    def add_launch_arg(name: str, default_value=None, **kwargs):
+        launch_arguments.append(
+            DeclareLaunchArgument(name, default_value=default_value, **kwargs)
+        )
+
+    add_launch_arg("launch_driver", "true")
+    add_launch_arg("host_ip", "192.168.1.10")
+    add_launch_arg("use_concat_filter", "true")
+    add_launch_arg(
+        "vehicle_id",
+        default_value=EnvironmentVariable("VEHICLE_ID", default_value="default"),
+    )
+    add_launch_arg("vehicle_mirror_param_file")
+    add_launch_arg(
+        "use_pointcloud_container", "false", description="launch pointcloud container"
+    )
+    add_launch_arg("pointcloud_container_name", "pointcloud_container")
+    add_launch_arg("enable_blockage_diag", "false")
+
+    # Create launch description with the config_file argument
+    ld = LaunchDescription(launch_arguments)
+    # Add sub-launch files dynamically based on the YAML configuration
+    ld.add_action(OpaqueFunction(function=load_sub_launches_from_yaml))
+
+    return ld

aip_xx1_launch/launch/pointcloud_preprocessor.launch.py

@@ -23,7 +23,7 @@
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
-
+from launch.launch_description_sources import AnyLaunchDescriptionSource
 
 def launch_setup(context, *args, **kwargs):
     # set concat filter as a component