bugfixes for command_vel
removed debug