Fixed a bug so that cmd vectors are correctly published
Strengthened obstacle avoid