SpiralIterator crashes when used on moving grid_map

[skohlbr]

I'm on 16.04/Kinetic/GridMap 1.6.0 from debs and attempting to use a SpiralIterator on a GridMap that is moved along with the robot pose. Below is the code I use, minimally edited for brevity (e.g. omitting the computation of start_pos etc.).

void processMap()
{
  double radius = 0.2;

  const grid_map::Matrix& grid_data = grid_map_["occupancy_log_odds"];

  grid_map::Position start_pos(some_x_coord, some_y_coord);
  grid_map::Index found_start_index(-1, -1);

  for (grid_map::SpiralIterator iterator(grid_map_, start_pos, radius);
      !iterator.isPastEnd(); ++iterator) {

    // Crashes in below line
    grid_map::Index index(*iterator);
    
    if ((grid_data (index.x(), index.y())) > 0.0f){
      found_start_index = index;
      break;
    }
  }
}

This reliably segfaults when dereferencing the iterator , backtrace below. Given the iterator use is pretty basic, I'm not sure how I screwed up. I'm under the assumption that the iterator can be used like this on a "moving" GridMap (e.g. one on which move() is called). Any hints are appreciated.

1   Eigen::DenseStorage<int, 2, 2, 1, 0>::DenseStorage                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   DenseStorage.h        192 0x7fabbc8fa31c 
2   Eigen::PlainObjectBase<Eigen::Array<int, 2, 1, 0, 2, 1>>::PlainObjectBase                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            PlainObjectBase.h     493 0x7fabbc8f74bf 
3   Eigen::Array<int, 2, 1, 0, 2, 1>::Array                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              Array.h               231 0x7fabbc8f5673 
4   GridMapProcessor::processMap                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     grid_map_processor.cpp      293 0x7fabbc8f2199 

[skohlbr]

Note the segfault on dereferencing sounds a lot like the problem described and fixed in issue #114. I'm wondering if perhaps the fact that I use a moving map causes a similar fault.

[maximilianwulf]

Hey @skohlbr,

AFAIK, the spiral iterator does not check if the current index is a valid index in the grid map.

I think if you add a check before you access the data:

if(!grid_map_.at(grid_map::Index(*iterator).isValid()) {
  continue;
}

you should be fine. I did not try it on my machine though.

[lucbettaieb]

@maximilianwulf I've run into a similar problem recently. It turns out that if you operate a SpiralIterator on a map that has moved, it will be unable to proceed after some point because of an indexing error. I was trying to debug this an observed an index jump from normal looking (125, 126) -> (126, 127) ... etc to something like (42342, 0)

Not sure if that's helpful. I can post more information when I see it happen again. I've since switched to the PolygonIterator for my purposes.

[maximilianwulf]

@lucbettaieb thanks for reporting. Did it lead to a segmentation fault in your case?

[lucbettaieb]

@maximilianwulf It did not lead to a segfault, as de-referencing the iterator was successful, it just yielded a garbage value (something like (42342, 0)) for an index. This got caught by a call to getPosition(index, position) which returned false as the index was > the size of the map. As a result, my program was unable to progress, though it did not crash.

[maximilianwulf]

Hm, so there is no unit test covering this use case, see here.

I will note it and add some if at some point. Feel free to submit one by yourself :)

[AndrewZheng-1011]

Hey,

I just want to add my case of a similar issue with the spiral iterator with simple test . Given a unit test code where the objective is when the center is outside the map itself, sometimes the spiral iterator is able to "figure out" that this is not within the map, and therefore, the for loop is not executed (Test 1). Other times, it does not do this and iterates a few times before giving undecipherable indices, which when accessing the map, gives segmentation fault (Test 2).

  grid_map::Position center{10, 10};  // Test 1: No error, but this center results in no loop execution
// grid_map::Position center{2.18117, -0.1483}; // Test 2: This creates errors

  grid_map::SpiralIterator it(map, center, radius);
  // std::cout << std::boolalpha;
  // std::cout << "Is it past end?: " << it.isPastEnd() << std::endl;

  for (grid_map::SpiralIterator it(map, center, radius); !it.isPastEnd();
       ++it) {
    // std::cout << "Index: " << *it << std::endl;
    grid_map::Position position;
    map.getPosition(*it, position);

    map.at("z", *it) = 0.0;
  }

For now, I will be adding a check condition for my application, but hopefully this example showcased another simple example where spiral iterator crashes.