Fix a bug with GridMap dimensions in GridMapUtils

When the example app in GridDStarPlanning is changed to use a non-square
GridMap, a bug appeared that caused part of the map to be cut off. This fixes
that.