ROS创建移动的类

我想创建一个将 turtlesim 节点移动到 x，y 位置的类。但是，当我检查 rostopic/turtle1/cmd_vel 时，它没有显示任何扭曲，并且 turtlesim 节点根本没有移动。我仍然得到的位置，但由于没有移动，所以它只是永远卡在while循环中。任何帮助将不胜感激。

class PathFinding{
Private:
   turtlesim::Pose initLocation;
   turtlesim::Pose finalLocation;
   double distance_tolerance;
   ros::Publisher cmd_vel_pub;
   ros::Subscriber pose_sub;
   ros::NodeHandle nh;
public:
   PathFinding(turtlesim::Pose goal, double d, ros::NodeHandle &n){
          finalLocation = goal;
          nh = n;
          distance_tolerance = d;
          cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
          pose_sub = nh.subscribe("/turtle1/pose", 1000, &PathFinding::poseCallBack,this);
   }
   double getDistance(double x1, double y1, double x2,double y2){
         return sqrt(pow((x1-x2),2) + pow((y1-y2),2));
   }
   void moveGoal(){
          geometry_msgs::Twist vel_msg;
          ros::Rate loop_rate(10);
          do{
                 vel_msg.linear.x = 1.5*getDistance(initLocation.x,initLocation.y,finalLocation.x,finalLocation.y);
                 vel_msg.linear.y = 0;
                 vel_msg.linear.z = 0;
                 vel_msg.angular.x = 0;
                 vel_msg.angular.y = 0;
                 vel_msg.angular.z = 4*(atan2(finalLocation.y-initLocation.y,finalLocation.x-initLocation.x) - initLocation.theta);
                 cmd_vel_pub.publish(vel_msg);
                 ros::spinOnce();
                 loop_rate.sleep();
          }

          while(getDistance(initLocation.x,initLocation.y,finalLocation.x,finalLocation.y) > distance_tolerance);
          vel_msg.linear.x = 0;
          vel_msg.angular.z = 0;
          cmd_vel_pub.publish(vel_msg);
}

    void poseCallBack(const turtlesim::Pose &p_msg){
        initLocation.x = p_msg.x;
        initLocation.y = p_msg.y;
        initLocation.theta = p_msg.theta;
    }
};
}

这就是我使用它的方式：

int main(int argc, char **argv) {
    ros::init(argc, argv, "turtleClassMoving");
    ros::NodeHandle n;
    turtlesim::Pose goal;
    goal.x = 1;
    goal.y = 1;
    goal.theta = 0;
    PathFinding t(goal, 0.5, n);
    t.moveGoal();
    ros::spin();
    return(0);
}