Changed publisher to cmd_vel_raw so they are filtered, fixed velocity bugs