ROS创建了移动turtlesim的类

时间:2018-04-11 01:07:11

标签: c++ ros

我想创建一个将turtlesim节点移动到x,y位置的类。但是,当我检查rostopic / turtle1 / cmd_vel时,它没有显示任何Twist,并且turtlesim节点根本不移动。我仍然得到了乌龟的位置,但由于turlte没有移动,所以它只是永远停留在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);
}

1 个答案:

答案 0 :(得分:0)

将节点句柄传递给类时的常见错误(如您的情况)是忘记传递私有节点句柄。所以也许你的程序实际上是在主题上发送命令,而不是你期望的那个。

因此,首先检查是否有任何其他cmd vel与

一起发送

rostopic列表

如果,正如我所料,有一个类似的名称主题,使用不同的前缀,请尝试修改您的主要

ros :: NodeHandle n(&#34;〜&#34;);