Check for "nan" in Laser-Scans

[skasperski]

MultiMapper.cpp(300): if(_it == 0 || isnan(_it))

[quattrinili]

I think also the -inf/+inf values should be checked by isinf(*it) (here the related REP http://www.ros.org/reps/rep-0117.html about distance measurements). With the current version in the Indigo repository, mapper seems to get stuck with one core of the CPU over 100% usage and so unable to properly update the map in real time.

[skasperski]

Can you describe in what situation the mapper gets stuck and why you believe that it is related to this issue? Are there any Errors or Warnings?

[quattrinili]

There aren't any errors or warnings from the mapper. Below a brief description and observations of the trials I performed (basically using the default parameters from the tutorial 3 of nav2d, apart from changing the relevant ones, like the frames or the sensor range):

1. in Stage, using the nodes for the turtlebot to control three simulated turtlebots: the mapper works fine. I observed that, as expected, the sensor readings are always a double value as, at least using the wrapper of ROS for Stage, the laser sensor is error-free.
2. with a real Turtlebot2 on which a Hokuyo URG-04LX (the 4m laser sensor) is mounted: the mapper takes a while (the order of some minutes) to process the first scan to be integrated into the map (the next ones are not processed because I let the robot stays still). I observed that there are some inf values in the readings. (and not directly the one from Stage).
3. similar to 2., with a real Turtlebot2 and the Kinect, using depthimage_to_laserscan: also in this case the mapper takes a while (a little bit more than 2.) to process the scan. The sensor readings contained some nan values. Both in 2. and 3., the mapper process uses 100% of the CPU during the processing of the scan.

I also debugged very briefly the mapper with gdb, and I noticed that in 2. and 3. it seems that the most of the time is spent on TraceLine and related functions, so the problem may lie in OpenKarto not able to process the nan and the inf values.

Given these observations, I changed in multimapper.cpp the if condition (at line 295 of the current version on git) also considering the nan and the inf values and the mapper seems to work fine also in 2. and 3. Nevertheless, a thought should be spent on setting the correct values according to the PEP I mentioned in the previous post.

If you need to investigate more on the problem, I have a couple of rosbag files that I can share with you to test the mapper on the same scenarios I tested.

Although not related to the above specific issue, I tell you what I'm doing (maybe you encountered this other issue): I'm trying to use the mapper with two real turtlebots (then I will extend it to more than 2), using multimaster_fkie to share the relevant topics between them as each of them is running a rosmaster and they are connected with an ad hoc network through WiFi. For now the sharing of the map seems to work from the robot with id 1 to the robot with id 2, but it seems that for some reasons the robot with id 2 is not able to start the mapping, although I manually provide the initialpose (in Stage it worked without any problem).

On Wed, Aug 19, 2015 at 3:58 AM, Sebastian Kasperski < notifications@github.com> wrote:

Can you describe in what situation the mapper gets stuck and why you believe that it is related to this issue? Are there any Errors or Warnings?

— Reply to this email directly or view it on GitHub https://github.com/skasperski/navigation_2d/issues/10#issuecomment-132482457 .

[skasperski]

Yes, bag-file and launch-file that reproduce the problem would be very useful. Trace-Line indeed sounds like a part of OpenKarto's grid map rendering algorithm.

[quattrinili]

Actually, also the other problem that I mentioned (the fact that the mapping doesn't start when the initial pose is provided on robots with id greater than or equal to 2) seems to be related to the same issue mentioned in the previous posts: at line 538 of MultiMapper.cpp there should be a check for nan and inf values.

I'm a little bit busy in this period, but I will try to share with you the bag-file and launch-file (which I have to write) in these days.

On Wed, Aug 19, 2015 at 9:01 AM, Sebastian Kasperski < notifications@github.com> wrote:

Yes, bag-file and launch-file that reproduce the problem would be very useful. Trace-Line indeed sounds like a part of OpenKarto's grid map rendering algorithm.

— Reply to this email directly or view it on GitHub https://github.com/skasperski/navigation_2d/issues/10#issuecomment-132583218 .

[skasperski]

I pushed an update to branch indigo-dev, to check for infinite and nan values: https://github.com/skasperski/navigation_2d/commit/458fb9eea217413d5502a7f9915d60636b2c3a1e

So far I only tested it with Stage, which I think doesn't produce these kind of values. It looks like Karto doesn't know any sort of invalid reading, so for now I use scan.range_max in all these cases. Could you please test this with your setup whether it solves any of the issues, especially if any of the warnings I added are thrown during runtime?

[quattrinili]

Thanks for the update. Sorry for not sharing the bag files with you yet, but I will be very busy until the middle of September.

By the way, I tested with the real robots (just with the Kinect) and, if the robot_id is set to 1, it doesn't get stuck anymore taking a lot of time to process the first scan. In the case of the Kinect, as I said, there are some nan values and so the warning about them appear during runtime. I am still not sure how to treat those values though (range_max or 0 values?). If another robot is added and so the scans are shared on the karto topic, the mapper on the robot with id > 1 crashes (SEGFAULT).

Debugging with gdb, I found that the problem is at line 256 of MultiMapper.cpp ( https://github.com/skasperski/navigation_2d/blob/indigo-dev/nav2d_karto/src/MultiMapper.cpp#L256), because the identifier from the data structure mLaser is passed as an argument to the constructor, but in the case of the robots with id > 1, mLaser is not initialized yet, but the function createFromRosMessage can be called when scans from robot with id 1 are received ( https://github.com/skasperski/navigation_2d/blob/indigo-dev/nav2d_karto/src/MultiMapper.cpp#L557 ). Thus I think that the function createFromRosMessage should include a parameter for the identifier, e.g. in the MultiMapper.cpp (and the related header file): l223: karto::LocalizedLaserScanPtr MultiMapper::createFromRosMessage(const karto::Identifier& rSensorIdentifier, const sensor_msgs::LaserScan& scan) l257: return new karto::LocalizedRangeScan(rSensorIdentifier, readings); l327: karto::LocalizedLaserScanPtr laserScan = createFromRosMessage(mLaser->GetIdentifier(), *scan); l558: karto::LocalizedLaserScanPtr localizedScan = createFromRosMessage(robot, scan->scan);

On Sun, Aug 23, 2015 at 11:14 AM, Sebastian Kasperski < notifications@github.com> wrote:

I pushed an update to branch indigo-dev, to check for infinite and nan values: 458fb9e https://github.com/skasperski/navigation_2d/commit/458fb9eea217413d5502a7f9915d60636b2c3a1e

So far I only tested it with Stage, which I think doesn't produce these kind of values. It looks like Karto doesn't know any sort of invalid reading, so for now I use scan.range_max in all these cases. Could you please test this with your setup whether it solves any of the issues, especially if any of the warnings I added are thrown during runtime?

— Reply to this email directly or view it on GitHub https://github.com/skasperski/navigation_2d/issues/10#issuecomment-133858875 .

[skasperski]

Adding 0-values (or even negative ones) is a bad idea, because then Karto will start adding obstacles at the sensor's position (or even behind it). So I guess adding max_range is the currently best solution.

About the crash: This was definitely a bug in my new method, that is fixed now. (hopefully).

[quattrinili]

Actually, from the map generated in my settings, it seems that setting them to 0 does not add any obstacle. So, right now in my specific scenario I am setting some of them to 0, so that it mitigate the problem of having freespace for example after a wall, which might become problematic especially for exploration purpose, as the number of frontiers to evaluate increases.

I will test your code in the next days and I'll let you know whether any other bug pops up in that part.

Thanks.

On Mon, Aug 31, 2015 at 10:29 AM, Sebastian Kasperski < notifications@github.com> wrote:

Adding 0-values (or even negative ones) is a bad idea, because then Karto will start adding obstacles at the sensor's position (or even behind it). So I guess adding max_range is the currently best solution.

About the crash: This was definitely a bug in my new method, that is fixed now. (hopefully).

— Reply to this email directly or view it on GitHub https://github.com/skasperski/navigation_2d/issues/10#issuecomment-136388243 .