minor cleanup, removing unnecessary slots
continous update of center of mass in groups
removing old files not needed anymore
more cleanup and fixes
added publishing og waypoint markers