SubmapIterator produces incorrect indices and segfaults? (ROS humble)

[flabrosse]

Please consider the bit of code below (coming from a ROS humble plugin I am working on):

      int neighbourhoodSize = 3;

      grid_map::Position startSubmap(0,0);
      grid_map::Index startIdx;
      gridMap.getIndex(startSubmap, startIdx);
      grid_map::Size submapSize = gridMap.getSize();

      std::cout << "(" << startSubmap(0) << "," << startSubmap(1) << ")"
                << std::endl;
      std::cout << "\t(" << submapSize(0) << "," << submapSize(1) << ")"
                << std::endl;

      // for (grid_map::SubmapIterator mapIter(gridMap, startIdx, submapSize);
      for (grid_map::GridMapIterator mapIter(gridMap);
           !mapIter.isPastEnd();
           ++mapIter)
      {
        std::cout << "Idx " << *mapIter << std::endl;
        float centreElev = gridMap.at("elevation", *mapIter);
        std::cout << "Elev " << centreElev << std::endl;
      }

This code works correctly, displaying

[planner_server-3] (0,0)
[planner_server-3]      (100,113)
[planner_server-3] Idx 0
[planner_server-3] 0
[planner_server-3] Elev nan

for the first element of the map, as expected.

If instead of Using the GridMapIterator like in the code above I use a SubmapIterator (with the line commented out above), I get a segfault and:

[controller_server-1] (0,0)
[controller_server-1]   (100,113)
[controller_server-1] Idx 1105
[controller_server-1]  458

(no elev and segfault).

This is a submap that should be the same as the full map. The same happens (with different values printed out) is I start the submap in a different location and with a different size, all fitting inside the full map.

At some point I thought I could not use at() with an iterator from a submap on the full map, but this is exactly what the iterator demo does.

Did I find a bug in the humble version? Did I miss something?

[Ryanf55]

Hi, it's very possible you found a bug. Thank you for reporting it.

If you can, do you mind forking grid_map, creating a branch, and showing the segfault in a unit test? With that information, I believe it will be quick to resolve.