Movement logic works (tuned by GB)
Merge branch 'navigation'
Merge branch 'master' into rgbdproc