First attempt at adding synchronization
Real-time edge coloring now working. Synchronization errors appear to be solved.
fixed angle bug in pose estimate tool and 2d nav goal tool