Need to debug global/local frame of cmd_vector and obstacle avoid
Fixed a bug so that cmd vectors are correctly published
Strengthened obstacle avoid
Tested waypoints
Merge branch 'master' into feature/state_machine