Need to debug global/local frame of cmd_vector and obstacle avoid