Occupancy Grid optimization node implemented and working. Forgot to add the file to commit last time