Question on turtlesim tutorial: Go to Goal

edit

Hello,

I am new to ROS and I am trying to implement the Go to Goal tutorial in C++ (the tutorial itself is in python but wanted to implemented it in C++). The part I am having trouble understanding is, in the function move2goal() there is a while loop who's condition depends on the magnitude of the error between the current pose and goal pose and it is in this loop that the commanded velocity is calculated. I ran the script and it works fine.

When I try to implement the same code in C++ I too have a function, velocity_calc() which has a while loop based on the magnitude error. It is in this loop that I calculate the commanded velocity (same as tutorial). The problem I am facing is, when I call this function in my main() (like they did in the tutorial) the compiler gets stuck in the while loop and is not able to update the current pose ie go to the call back function. I did over come this by replacing the while loop with an if condition in my callback and got it to work. But I am still curious as to why the while loop does not work for me. How is the python script able to update the current pose in the while loop?

turtle_cl.h

#ifndef TURTLECL_H
#define TURTLECL_H
#include <ros ros.h="">
#include <turtlesim pose.h="">
#include <geometry_msgs twist.h="">
#include <math.h>

namespace turtle_cl {   class turtleCL {

  public:
    turtleCL();
    void velocity_calc();



  private:
    ros::NodeHandle nh_;
    ros::NodeHandle nh_private_;

    ros::Subscriber pose_subscriber_;
    ros::Publisher vel_publisher_;

    void poseCallback(const turtlesim::PoseConstPtr &msg);


    turtlesim::Pose current_pose;
    geometry_msgs::Twist cmd_vel;

    float k_p_lin, k_p_ang, tol, goal_x, goal_y, mag;   };

}
#endif

#include "turtle_cl/turtlecl.h"

namespace turtle_cl
{
  turtleCL::turtleCL() :
    nh_(ros::NodeHandle()),
    nh_private_(ros::NodeHandle("~"))
  {

    pose_subscriber_ = nh_.subscribe<turtlesim::pose>
                        ("/turtle1/pose",10, &turtleCL::poseCallback, this);

    vel_publisher_ = nh_.advertise<geometry_msgs::twist>
                        ("/turtle1/cmd_vel",10,this);

    nh_private_.param<float>("p_gain_linear",k_p_lin,0.1);
    nh_private_.param<float>("p_gain_ang",k_p_ang,0.1);
    nh_private_.param<float>("tolerance",tol,0.1);
    nh_private_.param<float>("xGoal",goal_x,1);
    nh_private_.param<float>("yGoal",goal_y,1);

  }

  void turtleCL::poseCallback(const turtlesim::PoseConstPtr &msg)
  {
    current_pose = *msg;
    float mag1, mag2;
    mag1 = pow((goal_x - msg->x),2);
    mag2 = pow((goal_y - msg->y),2);
    mag = sqrt(mag1 + mag2);
    // ROS_INFO("in callback");
    // if(mag >= tol)
    //   velocity_calc();

  }

  void turtleCL::velocity_calc()
  {

    //ROS_INFO("in velocity_cal %f",mag);
    while(mag >= tol)
    {
      float velx,angx,angy;
      angx = goal_x - current_pose.x;
      angy = goal_y - current_pose.y;
      cmd_vel.linear.x = k_p_lin * mag;
      cmd_vel.angular.z = k_p_ang * atan2(angy,angx) - current_pose.theta;
      vel_publisher_.publish(cmd_vel);
    }
    //ROS_INFO("in while %f",current_pose.x);

  }

}

#include <ros ros.h="">
#include <turtle_cl turtlecl.h="">


int main(int argc, char** argv)
{
  ros::init(argc,argv,"turtle_cl_node");
  ros::NodeHandle nh;

  turtle_cl::turtleCL hello;
  hello.velocity_calc();

  ros::spin();
}