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.
Merge pull request #1 from vergenzt/dstargrid-dimensions

Fix a bug with GridMap dimensions in GridMapUtils
Merge pull request #1 from psigen/master

Merge stuff back