From f1a426ed414feb5e2582be4466d3bcef4f3c8e5c Mon Sep 17 00:00:00 2001
From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Date: Mon, 2 Aug 2021 00:54:06 +0900
Subject: [PATCH] Enable time series outlier filter (#314)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>
---
 .../aip_xx1/pointcloud_preprocessor.launch.py | 39 ++++++++++++++++++-
 1 file changed, 37 insertions(+), 2 deletions(-)

diff --git a/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py b/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py
index 351e6b53d76d8..138082ff9846e 100644
--- a/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py
+++ b/sensing_launch/launch/aip_xx1/pointcloud_preprocessor.launch.py
@@ -16,14 +16,17 @@
 
 import launch
 from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
 from launch.actions import OpaqueFunction
 from launch.actions import SetLaunchConfiguration
 from launch.conditions import IfCondition
 from launch.conditions import UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
 import yaml
 
 
@@ -121,7 +124,7 @@ def launch_setup(context, *args, **kwargs):
         name='scan_ground_filter',
         remappings=[
             ('input', 'measurement_range_cropped/pointcloud'),
-            ('output', 'no_ground/pointcloud')
+            ('output', 'no_ground/oneshot/pointcloud')
         ],
         parameters=[{
             'global_slope_max': 10.0,
@@ -134,6 +137,37 @@ def launch_setup(context, *args, **kwargs):
         }],
     )
 
+    load_laserscan_to_occupancy_grid_map = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                FindPackageShare('laserscan_to_occupancy_grid_map'),
+                '/launch/laserscan_to_occupancy_grid_map.launch.py'
+            ]
+        ),
+        launch_arguments={
+            'container':
+            '/sensing/lidar/pointcloud_preprocessor/pointcloud_preprocessor_container',
+            'input/obstacle_pointcloud': 'no_ground/oneshot/pointcloud',
+            'input/raw_pointcloud': 'concatenated/pointcloud',
+            'output': 'occupancy_grid',
+            'use_intra_process': LaunchConfiguration('use_intra_process'),
+        }.items()
+    )
+
+    occupancy_grid_map_outlier_component = ComposableNode(
+        package=pkg,
+        plugin='pointcloud_preprocessor::OccupancyGridMapOutlierFilterComponent',
+        name='occupancy_grid_map_outlier_filter',
+        remappings=[
+            ('~/input/occupancy_grid_map', 'occupancy_grid'),
+            ('~/input/pointcloud', 'no_ground/oneshot/pointcloud'),
+            ('~/output/pointcloud', 'no_ground/pointcloud'),
+        ],
+        extra_arguments=[{
+            'use_intra_process_comms': LaunchConfiguration('use_intra_process')
+        }],
+    )
+
     # set container to run all required components in the same process
     container = ComposableNodeContainer(
         name='pointcloud_preprocessor_container',
@@ -143,6 +177,7 @@ def launch_setup(context, *args, **kwargs):
         composable_node_descriptions=[
             cropbox_component,
             ground_component,
+            occupancy_grid_map_outlier_component,
         ],
         output='screen',
     )
@@ -160,7 +195,7 @@ def launch_setup(context, *args, **kwargs):
         condition=UnlessCondition(LaunchConfiguration('use_concat_filter')),
     )
 
-    return [container, concat_loader, passthrough_loader]
+    return [container, concat_loader, passthrough_loader, load_laserscan_to_occupancy_grid_map]
 
 
 def generate_launch_description():