Occupancy Grid optimization node implemented and working