feat: ring outlier filter and distortion correction node load from pa…

…ram file (tier4#6)

Signed-off-by: A. Sena Yılmaz <[email protected]>

awsim_labs_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py

@@ -39,13 +39,14 @@ def launch_setup(context, *args, **kwargs):
         parameters=[
             {
                 "input_topics": [
-                    "/sensing/lidar/top/pointcloud",
-                    "/sensing/lidar/left/pointcloud",
-                    "/sensing/lidar/right/pointcloud",
+                    "/sensing/lidar/top/pointcloud_before_sync",
+                    "/sensing/lidar/left/pointcloud_before_sync",
+                    "/sensing/lidar/right/pointcloud_before_sync",
                 ],
                 "output_frame": LaunchConfiguration("base_frame"),
                 "timeout_sec": 0.01,
                 "input_twist_topic_type": "twist",
+                "publish_synchronized_pointcloud": True,
             }
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],

common_awsim_labs_sensor_launch/config/distortion_corrector_node.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    base_frame: base_link
+    use_imu: true
+    use_3d_distortion_correction: false

common_awsim_labs_sensor_launch/config/ring_outlier_filter_node.param.yaml

@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    distance_ratio: 1.03
+    object_length_threshold: 0.1
+    num_points_threshold: 4
+    max_rings_num: 128
+    max_points_num_per_ring: 4000
+    publish_outlier_pointcloud: false
+    min_azimuth_deg: 0.0
+    max_azimuth_deg: 360.0
+    max_distance: 12.0
+    vertical_bins: 128
+    horizontal_bins: 36
+    noise_threshold: 2

common_awsim_labs_sensor_launch/launch/velodyne_node_container.launch.py

@@ -11,7 +11,8 @@
 # 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.
-
+import os
+from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import OpaqueFunction
@@ -22,6 +23,7 @@
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterFile
 import yaml
 
 
@@ -57,8 +59,25 @@ def create_parameter_dict(*args):
             result[x] = LaunchConfiguration(x)
         return result
 
+    # Pointcloud preprocessor parameters
+    distortion_corrector_node_param = ParameterFile(
+        param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context),
+        allow_substs=True,
+    )
+    ring_outlier_filter_node_param = ParameterFile(
+        param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context),
+        allow_substs=True,
+    )
+
     nodes = []
 
+    nodes.append(
+        ComposableNode(
+            package="glog_component",
+            plugin="GlogComponent",
+            name="glog_component",
+        )
+    )
     cropbox_parameters = create_parameter_dict("input_frame", "output_frame")
     cropbox_parameters["negative"] = True
 
@@ -105,16 +124,37 @@ def create_parameter_dict(*args):
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
     )
+    nodes.append(
+        ComposableNode(
+            package="autoware_pointcloud_preprocessor",
+            plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent",
+            name="distortion_corrector_node",
+            remappings=[
+                ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
+                ("~/input/imu", "/sensing/imu/imu_data"),
+                ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"),
+                ("~/output/pointcloud", "rectified/pointcloud_ex"),
+            ],
+            parameters=[distortion_corrector_node_param],
+            extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+        )
+    )
 
+    # Ring Outlier Filter is the last component in the pipeline, so control the output frame here
+    # if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
+    #     ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
+    # else:
+    #     ring_outlier_output_frame = {"output_frame": ""}  # keep the output frame as the input frame
     nodes.append(
         ComposableNode(
             package="autoware_pointcloud_preprocessor",
             plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent",
             name="ring_outlier_filter",
             remappings=[
                 ("input", "rectified/pointcloud_ex"),
-                ("output", "pointcloud"),
+                ("output", "pointcloud_before_sync"),
             ],
+            parameters=[ring_outlier_filter_node_param],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
     )
@@ -127,47 +167,10 @@ def create_parameter_dict(*args):
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=nodes,
+        output="both",
     )
 
-    distortion_component = ComposableNode(
-        package="autoware_pointcloud_preprocessor",
-        plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent",
-        name="distortion_corrector_node",
-        remappings=[
-            ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
-            ("~/input/imu", "/sensing/imu/imu_data"),
-            ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"),
-            ("~/output/pointcloud", "rectified/pointcloud_ex"),
-        ],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-
-    distortion_relay_component = ComposableNode(
-        package="topic_tools",
-        plugin="topic_tools::RelayNode",
-        name="pointcloud_distortion_relay",
-        namespace="",
-        parameters=[
-            {"input_topic": "mirror_cropped/pointcloud_ex"},
-            {"output_topic": "rectified/pointcloud_ex"}
-        ],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-
-    # one way to add a ComposableNode conditional on a launch argument to a
-    # container. The `ComposableNode` itself doesn't accept a condition
-    distortion_loader = LoadComposableNodes(
-        composable_node_descriptions=[distortion_component],
-        target_container=container,
-        condition=launch.conditions.IfCondition(LaunchConfiguration("use_distortion_corrector")),
-    )
-    distortion_relay_loader = LoadComposableNodes(
-        composable_node_descriptions=[distortion_relay_component],
-        target_container=container,
-        condition=launch.conditions.UnlessCondition(LaunchConfiguration("use_distortion_corrector")),
-    )
-
-    return [container, distortion_loader, distortion_relay_loader]
+    return [container]
 
 
 def generate_launch_description():
@@ -178,17 +181,38 @@ def add_launch_arg(name: str, default_value=None, description=None):
         launch_arguments.append(
             DeclareLaunchArgument(name, default_value=default_value, description=description)
         )
+
+    common_sensor_share_dir = get_package_share_directory("common_sensor_launch")
 
     add_launch_arg("base_frame", "base_link", "base frame id")
     add_launch_arg("container_name", "velodyne_composable_node_container", "container name")
     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("output_as_sensor_frame", "True", "output final pointcloud in sensor frame")
     add_launch_arg(
         "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
     )
+    add_launch_arg("frame_id", "lidar", "frame id")
     add_launch_arg("use_multithread", "False", "use multithread")
     add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")
-
+    add_launch_arg(
+        "distortion_correction_node_param_path",
+        os.path.join(
+            common_sensor_share_dir,
+            "config",
+            "distortion_corrector_node.param.yaml",
+        ),
+        description="path to parameter file of distortion correction node",
+    )
+    add_launch_arg(
+        "ring_outlier_filter_node_param_path",
+        os.path.join(
+            common_sensor_share_dir,
+            "config",
+            "ring_outlier_filter_node.param.yaml",
+        ),
+        description="path to parameter file of ring outlier filter node",
+    )
     set_container_executable = SetLaunchConfiguration(
         "container_executable",
         "component_container",