Fixed cmd_vel publishing