Bug report

Required Info:

• Operating System: Ubuntu
• Version or commit hash: master

Inside the test testInflationOrderCorrectness in inflation_tests, validatePointInflation is called to compare the point inflation with the inflated costmap. The cells are processed by increasing distance, but not all cells are being explored.
Below is a mask from the test where 1 represents the processed cells and 254 is the lethal obstacle:

    0  13   1   1  37  33  23  13   0   0

   13   1   1   1   1  87  55  29  13   0

    1   1   1   1   1 219 121  55  29  13

    1   1   1   1   1 253 219 121  55  23

    1   1   1   1   1 253 253 219  87  33

    1   1   1   1   1  254 253 253 102  37

    1   1   1   1   1 253 253 219  87  33

   13   1   1   1   1 253 219 121  55  23

    0  13   1   1  87 102  87  55  29  13

    0   0  13  23  33  37  33  23  13   0

Expected behavior

To process all cells within inflated radius.

Actual behavior

Only a subset of cells are processed.

Additional information

The issue is caused as a CellData reference is used to iterate the vector which becomes invalidated due when another cell is added to the vector.

Possible solutions include instead of using a reference to use a copy of the cell for iteration or to update the reference after every addition to the vector.