use laser_filter pipeline for scanner

[fmessmer]

according to @ipa-jba, laser_filters is quite powerful

investigate setting up a proper filter_pipeline for e.g. the sick scanners that would replace:

• cob_scan_filter via http://wiki.ros.org/laser_filters#LaserScanAngularBoundsFilter
• max_range_fix via http://wiki.ros.org/laser_filters#LaserScanRangeFilter

@ipa-bnm @ipa-fmw FYI

[fmessmer]

@ipa-srd
can you explain again, why it was necessary to avoid NaN in the laser scan and replace it with max_range?

[stefandoerr]

With https://github.com/ipa320/cob_driver/pull/358 max range measurements are properly set, i.e. their values are >= than the configured max_range of the respective laser scanner. This is needed so that a node can properly identify max range measurements within the ranges array. For example. the localization module normally filters out max range measurements to avoid matching those measurements against the map.
However, in the current setup, this would lead to an undesired behavior in the costmap which will discard those max range measurements for ray tracing. In open areas where you have a lot of max range measurements, this leads to the problem that dynamic objects are not cleared properly. With the feature implemented by @ipa-jba in ros-perception/laser_filters#56, this problem can be resolved. The filter sets all max range measurements to inf. Together with setting the inf_is_valid param in the obstacle_layer to true, this fixes the problem.

[stefandoerr]

Example config:

scan_filter_chain:
 - name: self_meas
   type: LaserScanAngularBoundsFilter
   params:
     lower_angle: 0.78
     upper_angle: 3.91
 - name: max_range
   type: LaserScanRangeFilter
   params:
     use_message_range_limits: true
upper_replacement_value: .inf

Example node

+  <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter">
+    <rosparam command="load" file="$(find striml_hardware_config)/robots/$(arg robot)/config/laser_filter_chain.yaml" />
+    <remap from="scan" to="/base_laser_front/scan" />
+    <remap from="scan_filtered" to="scan" />
+  </node>

[jabbenseth]

in the example config upper_replacement_value should be on the same indentation level as use_message_range

[fmessmer]

once we use this filter_chain, i.e. setting out-of-range values to inf, where (in which nav.yaml) is the inf_is_valid param to be set?

[fmessmer]

the LaserScanAngularBoundsFilter has weird behavior:

• it cuts away scan points in the given interval rather than keeping just those
• also negative values for the angle parameters seem not to cause problems

thus, I'm just adding the LaserScanRangeFilter and keeping the current cob_scan_filter for the angular bounds

[stefandoerr]

http://wiki.ros.org/costmap_2d/hydro/obstacles
and add in pkg_nav_config/robots//nav/move_base_local_costmap_params.yaml in the respective obstacle layer

[fmessmer]

I tested the related PRs today, i.e. "max_range" of sick_s300 + RangeFilter pipeline + inf_is_valid in costmap

However, the behavior is not as expected, i.e. ranges that are set to inf in the laser scan used as a costmap observation source, does not really clear the costmap properly...
@ipa-srd has confirmed that the behavior seen is not correct

@ipa-jba can we have a look at this together sometime next week?

[fmessmer]

to debug raytracing, @ipa-srd suggested to temporarily remove the inflation layer to better see the actually blocked cells

[stefandoerr]

without having looked into code the chain: setting max range to inf -> using inf_is_valid in costmap does not help, i.e. occupied areas are not cleared although the ray tracing of max range measurements should go through the cell. A successfully tested workaround is to have a LaserScanRangeFiltersetting max range to some value higher than costmap bounds but lower than obstacle_range param in costmap params and using this scan msg exclusively for move_base.

Example of the node:

  <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter_planning">
    <rosparam command="load" file="$(find striml_hardware_config)/robots/$(arg robot)/config/laser_filter_chain_planning.yaml" />
    <remap from="scan" to="scan" />
    <remap from="scan_filtered" to="scan_planning" />
  </node>

and the laser filter config:

scan_filter_chain:
 - name: max_range_to_finite
   type: LaserScanRangeFilter
   params:
     use_message_range_limits: false
     upper_threshold: 16.0
     upper_replacement_value: 16.0

move_base_local_costmap_params.yaml:

# The parameters for the rolling_window (must be set if rolling_window = true)
width: 8.0         # default: 10
height: 8.0        # default: 10
resolution: 0.05   # default: 0.05
origin_x: 0.0       # default: 0.0
origin_y: 0.0       # default: 0.0

obstacle_layer:

  # Definition of the sensors/observation sources
  observation_sources: laser_scan_front

  # Parameters of the source: laser_scan_front
  laser_scan_front:
    sensor_frame: base_laser_link # The frame of the origin of the sensor, default: ""
    topic: /scan_planning       # The topic on which sensor data comes in for this source, default: source_name
    data_type: LaserScan                # The data type associated with the topic, default: "PointCloud"
    marking: true                       # Whether or not this observation is used to mark obstacles, default: true
    clearing: true                      # Whether or not this observation is used to clear out freespace, default: false


  # The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters
  obstacle_range: 18.0  # default: 2.5

  # The default range in meters at which to raytrace out obstacles from the map using sensor data
  raytrace_range: 18.0  # default: 3.0

@ipa-fxm fyi

[fmessmer]

thanks, I will try this next week
are you actually using your pipeline/config on a real robot/agv?

[stefandoerr]

yes, currently running on a customer agv in an industrial environment where a lot of max range measurements appear

[fmessmer]

I'll re-open, as this issue documents quite a bit regarding issues not yet solved:

• replace cob_scan_filter via http://wiki.ros.org/laser_filters#LaserScanAngularBoundsFilter
• implement proper inf_is_valid behavior (as it affects RangeFilter parameters

[stefandoerr]

as discussed with @ipa-fxm, the current solution should be reworked. Only the scan that is going into the costmap should be filtered to 10 m. The filter should have no effects on the standard scan and also not on scan_unified.
We suggest to have an additional filter at the end of the current chain which does the 10 m cut and the outcoming msg is named something like scan_costmap or similar to make it clear.

[fmessmer]

I implemented this for the unity_robots...it's a pr still which @HannesBachter wanted to test together with slam...
that pr could be ported to cob_robots sidentically...
@ipa-srd @HannesBachter please see https://github.com/unity-robotics/unity_robots/pull/46

[fmessmer]

as discussed today with @ipa-srd @HannesBachter, the scan toolchain looks like this

front - scan_raw - (cob_scan_filter) - scan (i.e. 30m) -|
left - scan_raw - (cob_scan_filter) - scan (i.e. 30m) -|- (unifier) - scan_unified
right - scan_raw - (cob_scan_filter) - scan (i.e. 30m) -|

... scan (30m)- (range filter) - scan_ranged (10m) -|
... scan (30m)- (range filter) - scan_ranged (10m) -|- (unifier) - scan_unified_ranged
... scan (30m)- (range filter) - scan_ranged (10m) -|

... scan_unified_ranged - (zone filter) - costmap_scan_filtered

like this scan_unified is back to the long-range scan as before, e.g. used for planning, rviz, ...
like this scan_unified_ranged is a new topic only used in the local_costmap layer_0 for planning

https://github.com/unity-robotics/unity_robots/pull/46 and https://github.com/unity-robotics/msh/pull/267 now implement this for unity_robots
after testing by @HannesBachter, I will migrate this to cob_robots, too

[fmessmer]

@ipa-srd @HannesBachter
how about this?

front - scan_raw - (cob_scan_filter) - scan (i.e. 30m) -|
left - scan_raw - (cob_scan_filter) - scan (i.e. 30m) -|- (unifier) - scan_unified
right - scan_raw - (cob_scan_filter) - scan (i.e. 30m) -|

... scan_unified - (range filter) - scan_unified_filtered (10m) - (zone filter) - costmap_scan_filtered

pushing scan_unified through additional filters, i.e. range filter and zone filter seems the easier thing to do (less nodes: 1vs.6 range filter 1vs.2 scan unifier)...in particular because scan_ranged topics are nowhere used anyway...afaik

[stefandoerr]

@ipa-jba @ipa-flg, I discussed this with @ipa-fxm and in our opinion the second chain makes more sense since it's more performant but we would need to change the costmap params in the way that the obstacle layer only subscribes to scan_unified_filtered (instead of currently subscribing to the scan's individually). You see any problem with that?

[fmessmer]

I discussed it with @ipa-flg and I'm going to implement the second version both for unity_robots and cob_robots

then - in order to use the range-filtered scans to improve costmap clearing - @ipa320/navigation-push will need to adjust their respective costmap yaml files to accept the single, range_filtered, unified scan topic /scan_unified_filtered rather than the three individual scans /base_laser_front/scan, /base_laser_left/scan, /base_laser_right/scan!

The navigation configs still work without changing anything.
However, they still use the full-range scans and cannot benefit the improved costmap clearing behavior!
Note that there won't be individual, range_filtered scans with this approach!

[fmessmer]

the laser filter pipeline has been set up for scan_unified in #757
now it could be used in the costmap.yaml - if desired...

thus, I'm closing this issue