Occupancy Grid setOccupied now working