ROS: velocity command almost working