Fix overflow in grid_map