[ros-users] costmap_2d questions

Eitan Marder-Eppstein eitan at willowgarage.com
Thu Sep 23 16:46:10 UTC 2010


Jack,

The filtering of max_range points out of the laser scan occurs when the
LaserScan is projected into a PointCloud. Once the costmap gets the cloud,
there's no information about what max_range would be... so there's not
really an easy patch for that. You'd have to hack the laser_projector (
http://www.ros.org/wiki/laser_geometry) to get the behavior that you want.
There used to be an option to the projector to keep max_range values, but I
think it went away a long time ago. The overhead of the filter wouldn't be
so awful, as it just requires looping through the scan once, but I guess
every little bit counts... especially when you have as many points as the
velodyne produces.

I think I'd suggest trying the filter, I actually have an example of it
here:
navigation_experimental/assisted_teleop/include/assisted_teleop/max_range_filter.h,
navigation_experimental/assisted_teleop/filter_plugins.xml,
navigation_experimental/assisted_teleop/manifest.xml

If it proves to be too slow, then you can think about hacking the projector,
but another advantage of using the filter is it allows you to use the raw
scan for marking, and the filtered one for clearing which avoids putting
unnecessary obstacles into the map.

Hope this helps,

Eitan


On Thu, Sep 23, 2010 at 7:12 AM, Jack O'Quin <jack.oquin at gmail.com> wrote:

> On Tue, Sep 21, 2010 at 1:33 PM, Eitan Marder-Eppstein
> <eitan at willowgarage.com> wrote:
> > On Mon, Sep 20, 2010 at 6:36 PM, Jack O'Quin <jack.oquin at gmail.com>
> wrote:
> >> On Mon, Sep 20, 2010 at 1:13 PM, Eitan Marder-Eppstein
> >> <eitan at willowgarage.com> wrote:
>
> >> > You're right in your recollection about max_range readings being
> treated
> >> > as
> >> > unknown. If you want the costmap to use this information differently,
> >> > you'll
> >> > have to write a laser filter that takes max_range values in your laser
> >> > scan
> >> > and assigns them slightly less than max_range values. When we've done
> >> > this,
> >> > we typically set up one observation_source that only puts marking
> >> > information into the costmap that uses the unfiltered scan and one
> >> > observation_source that clears obstacle information in the costmap and
> >> > uses
> >> > the filtered scan topic. In this way, we don't put max range hits in
> as
> >> > obstacles, but we do use them to clear obstacles out.
> >>
> >> That helps. I don't think I could have figured that out from the docs.
> >> It would help if there were an explicit statement that cells traversed
> >> by max range readings are unknown.
> >>
> >> What is the rationale for not treating max range reading as clearing
> >> intervening cells? Is it mostly a performance issue due to the ray
> >> tracing overhead?
> >
> > The rationale is that you can't actually tell the difference between an
> > error and max_range with a laser range finder. Points that come back as
> > max_range may, for example, have actually hit a black surface. So,
> treating
> > them as valid hits may actually be unsafe and cause you to clear
> obstacles
> > out that shouldn't.
>
> While that can happen, we have not found it to be a big problem with
> our devices (Velodyne HDL-64E point cloud for long ranges, Sick LMS200
> for short ranges). When phantom scans occur, the occupancy map usually
> does a good job of filtering them out before they become a problem.
>
> We need to track relatively fast-moving moving vehicles. If we do not
> clear cells traversed by max range headings, cars will leave a long
> trail of false-positive "occupied" cells behind them. There often will
> not be another obstacle in range behind the moving car to clear out
> those cells.
>
> So, maybe we do need to filter the input as you suggest. That sounds
> like it might be a lot of extra overhead.
>
> Is there any possibility to add clearing of max_range readings as a
> Costmap2D option? I have not studied the implementation, but would be
> willing to attempt a patch if there is any interest in adding that
> feature.
> --
>  joq
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20100923/c010c93b/attachment-0003.html>


More information about the ros-users mailing list