Closed fmessmer closed 6 years ago
@ipa-srd
can you explain again, why it was necessary to avoid NaN
in the laser scan and replace it with max_range
?
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 https://github.com/ros-perception/laser_filters/pull/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.
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>
in the example config upper_replacement_value
should be on the same indentation level as use_message_range
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?
the LaserScanAngularBoundsFilter
has weird behavior:
thus, I'm just adding the LaserScanRangeFilter
and keeping the current cob_scan_filter
for the angular bounds
http://wiki.ros.org/costmap_2d/hydro/obstacles
and add in pkg_nav_config/robots/
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?
to debug raytracing, @ipa-srd suggested to temporarily remove the inflation layer to better see the actually blocked cells
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 LaserScanRangeFilter
setting 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
thanks, I will try this next week are you actually using your pipeline/config on a real robot/agv?
yes, currently running on a customer agv in an industrial environment where a lot of max range measurements appear
I'll re-open, as this issue documents quite a bit regarding issues not yet solved:
inf_is_valid
behavior (as it affects RangeFilter parametersas 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.
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
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
@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
@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?
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!
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
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:
@ipa-bnm @ipa-fmw FYI