fix pointcloud preprocess for multi_lidar_ogm

tier4 · Mar 11, 2024 · 5fcffbe · 5fcffbe
1 parent 3a32bfd
commit 5fcffbe
Show file tree

Hide file tree

Showing 2 changed files with 56 additions and 10 deletions.
diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py
@@ -38,22 +38,23 @@ def launch_setup(context, *args, **kwargs):
         parameters=[
             {
                 "input_topics": [
-                    "/sensing/lidar/top/pointcloud",
-                    "/sensing/lidar/side_left/pointcloud",
-                    "/sensing/lidar/side_right/pointcloud",
-                    "/sensing/lidar/front_left/pointcloud",
-                    "/sensing/lidar/front_right/pointcloud",
+                    "/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",
                 ],
                 "output_frame": LaunchConfiguration("base_frame"),
                 "input_offset": [
                     0.035,
-                    0.025,
-                    0.025,
-                    0.025,
-                    0.025,
+                    # 0.025,
+                    # 0.025,
+                    # 0.025,
+                    # 0.025,
                 ],  # each sensor will wait 60, 70, 70, 70ms
                 "timeout_sec": 0.095,  # set shorter than 100ms
                 "input_twist_topic_type": "twist",
+                "publish_synchronized_pointcloud": True,
             }
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],

diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py
@@ -115,6 +115,8 @@ def create_parameter_dict(*args):
                         "cloud_min_angle",
                         "cloud_max_angle",
                         "dual_return_distance_threshold",
+                        "setup_sensor",
+                        "retry_hw",
                     ),
                 },
             ],
@@ -126,6 +128,36 @@ def create_parameter_dict(*args):
         )
     )
 
+    nodes.append(
+        ComposableNode(
+            package="nebula_ros",
+            plugin=sensor_make + "HwMonitorRosWrapper",
+            name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node",
+            parameters=[
+                {
+                    "sensor_model": sensor_model,
+                    **create_parameter_dict(
+                        "return_mode",
+                        "frame_id",
+                        "scan_phase",
+                        "sensor_ip",
+                        "host_ip",
+                        "data_port",
+                        "gnss_port",
+                        "packet_mtu_size",
+                        "rotation_speed",
+                        "cloud_min_angle",
+                        "cloud_max_angle",
+                        "diag_span",
+                        "dual_return_distance_threshold",
+                        "delay_monitor_ms",
+                    ),
+                },
+            ],
+            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        )
+    )
+
     cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
     cropbox_parameters["negative"] = True
 
@@ -188,15 +220,23 @@ def create_parameter_dict(*args):
         )
     )
 
+    # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
+    if LaunchConfiguration("output_as_sensor_frame").perform(context):
+        ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")}
+    else:
+        ring_outlier_filter_parameters = {
+            "output_frame": ""
+        }  # keep the output frame as the input frame
     nodes.append(
         ComposableNode(
             package="pointcloud_preprocessor",
             plugin="pointcloud_preprocessor::RingOutlierFilterComponent",
             name="ring_outlier_filter",
             remappings=[
                 ("input", "rectified/pointcloud_ex"),
-                ("output", "pointcloud"),
+                ("output", "pointcloud_before_sync"),
             ],
+            parameters=[ring_outlier_filter_parameters],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
     )
@@ -236,6 +276,7 @@ def create_parameter_dict(*args):
                     "setup_sensor",
                     "ptp_profile",
                     "ptp_transport_type",
+                    "retry_hw",
                 ),
             }
         ],
@@ -263,6 +304,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("config_file", "", description="sensor configuration file")
     add_launch_arg("launch_driver", "True", "do launch driver")
     add_launch_arg("setup_sensor", "True", "configure sensor")
+    add_launch_arg("retry_hw", "false", "retry hw")
     add_launch_arg("sensor_ip", "192.168.1.201", "device ip address")
     add_launch_arg("host_ip", "255.255.255.255", "host ip address")
     add_launch_arg("scan_phase", "0.0")
@@ -287,6 +329,9 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("container_name", "nebula_node_container")
     add_launch_arg("ptp_profile", "1588v2")
     add_launch_arg("ptp_transport_type", "L2")
+    add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
+    add_launch_arg("diag_span", "1000", "")
+    add_launch_arg("delay_monitor_ms", "2000", "")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",