From b58f81845b461f886e1f4d864cf30394558b1259 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= <bjoern.ellensohn@gmail.com>
Date: Fri, 13 Oct 2023 09:56:09 +0200
Subject: [PATCH] update: implement laser filter

---
 config/box_filter.yaml             | 15 +++++++++++++++
 launch/robot_lidar.launch.py       |  6 ++++++
 launch/robot_scan_filter.launch.py | 18 ++++++++++++++++++
 3 files changed, 39 insertions(+)
 create mode 100644 config/box_filter.yaml
 create mode 100644 launch/robot_scan_filter.launch.py

diff --git a/config/box_filter.yaml b/config/box_filter.yaml
new file mode 100644
index 0000000..763c1e7
--- /dev/null
+++ b/config/box_filter.yaml
@@ -0,0 +1,15 @@
+scan_to_scan_filter_chain:
+  ros__parameters:
+    filter1:
+      name: box_filter
+      type: laser_filters/LaserScanBoxFilter
+      params:
+        box_frame: laser_frame
+        max_x: 0.16
+        max_y: 0.16
+        max_z: 0.1
+        min_x: -0.41
+        min_y: -0.16
+        min_z: -0.2
+        
+        invert: false # activate to remove all points outside of the box
\ No newline at end of file
diff --git a/launch/robot_lidar.launch.py b/launch/robot_lidar.launch.py
index e981c97..869b62b 100644
--- a/launch/robot_lidar.launch.py
+++ b/launch/robot_lidar.launch.py
@@ -8,6 +8,7 @@ from launch.actions import DeclareLaunchArgument
 from launch.actions import LogInfo
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from launch.substitutions import ThisLaunchFileDir
 
 def generate_launch_description():
     channel_type =  LaunchConfiguration('channel_type', default='serial')
@@ -68,4 +69,9 @@ def generate_launch_description():
             }],
             #namespace = "/rmp"
         )
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([ThisLaunchFileDir(), 'robot_scan_filter.launch.py']),
+            #launch_arguments={'my_arg': 'new_value'}.items()  # You can pass arguments here
+        ),
     ])
diff --git a/launch/robot_scan_filter.launch.py b/launch/robot_scan_filter.launch.py
new file mode 100644
index 0000000..6a1e16c
--- /dev/null
+++ b/launch/robot_scan_filter.launch.py
@@ -0,0 +1,18 @@
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package="laser_filters",
+            executable="scan_to_scan_filter_chain",
+            parameters=[
+                PathJoinSubstitution([
+                    get_package_share_directory("cps_rmp220_support"),
+                    "config", "box_filter.yaml",
+                ])],
+        )
+    ])
\ No newline at end of file