Turn the robot using the odometry message

edit

I want to create function turn where I will enter some angle and the robot would turn using the odometry message. I have a problem with waiting for odometry message, because the fuction will be called with the theta_current = 0, which is initial value. I thought that if I use function waitForMessage and afterwards spinOnce, I should get the current value. But I am wrong and I have no idea how to solve that problem. And yes I did a lot of googling but I found only the examples using tf.

//global variables
    double x_current = 0; //used to move
    double y_current = 0; //used to move
    double theta_current = 0; //used to move

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

      velocity_pub = n.advertise<geometry_msgs::Twist>("/robot1/cmd_vel", 1000);



        ros::topic::waitForMessage<nav_msgs::Odometry>("/robot1/odom");
        ros::Subscriber sub = n.subscribe("/robot1/odom",1000,getOdom);

        ros::spinOnce();

        turn(90.0, 0.2, 1);

      ros::Rate r(10);
      while(ros::ok())
      {
          ros::spinOnce();
          r.sleep();
      }

      return 0;
    }


    void set_velocities(float lin_vel, float ang_vel)
    {
        geometry_msgs::Twist msg;
        msg.linear.x = lin_vel;
        msg.angular.z = ang_vel;

        velocity_pub.publish(msg);
    }

    void turn(float angleDeg, double angularSpeed, int direction)
    {
        float currentAngleDeg = theta_current*M_PI/180; //convert current angle in radians to degrees
        float endAngle = currentAngleDeg + currentAngleDeg;
        ros::Rate rate(10);
        while(currentAngleDeg < endAngle)
        {
            set_velocities(0.0, 0.2);
            ros::spinOnce();
            rate.sleep();

        }
        set_velocities(0.0, 0.0);
    }


    void getOdom(const nav_msgs::Odometry::ConstPtr& msg)
    {
        theta_current = msg->twist.twist.angular.z;
        x_current = msg->pose.pose.position.x;
        y_current = msg->pose.pose.position.y;
    }