Add Laser Filter

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>
   <!-- -->
 
   <!--

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>

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}