From 9c390380088722d57ace28114adb6e1f927bffd1 Mon Sep 17 00:00:00 2001
From: "David V. Lu" <davidvlu@gmail.com>
Date: Mon, 11 Jan 2021 10:11:31 -0500
Subject: [PATCH] Add Laser Filter

---
 stretch_core/launch/rplidar.launch                  | 13 +++++++++++++
 stretch_core/package.xml                            |  1 +
 .../config/common_costmap_params.yaml               |  2 +-
 3 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/stretch_core/launch/rplidar.launch b/stretch_core/launch/rplidar.launch
index c772d02..e678c9e 100644
--- a/stretch_core/launch/rplidar.launch
+++ b/stretch_core/launch/rplidar.launch
@@ -9,6 +9,19 @@
     <param name="angle_compensate"    type="bool"   value="true"/>
     <param name="scan_mode"           type="string" value="Boost"/>
   </node>
+
+  <node pkg="laser_filters" type="scan_to_scan_filter_chain"
+      name="laser_filter">
+    <rosparam ns="scan_filter_chain">
+        - name: shadows
+          type: laser_filters/ScanShadowsFilter
+          params:
+            min_angle: 10
+            max_angle: 170
+            neighbors: 20
+            window: 1
+    </rosparam>
+  </node>
   <!-- -->
 
   <!--
diff --git a/stretch_core/package.xml b/stretch_core/package.xml
index 9d1e5bb..f152327 100644
--- a/stretch_core/package.xml
+++ b/stretch_core/package.xml
@@ -74,6 +74,7 @@
   <exec_depend>actionlib</exec_depend>
   <exec_depend>actionlib_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>laser_filters</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>control_msgs</exec_depend>
   <exec_depend>trajectory_msgs</exec_depend>
diff --git a/stretch_navigation/config/common_costmap_params.yaml b/stretch_navigation/config/common_costmap_params.yaml
index dc090cd..9bc0825 100644
--- a/stretch_navigation/config/common_costmap_params.yaml
+++ b/stretch_navigation/config/common_costmap_params.yaml
@@ -5,4 +5,4 @@ inflation_radius: 0.55
 
 observation_sources: laser_scan_sensor
 
-laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}
+laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan_filtered, marking: true, clearing: true}