Turning 90degrees using the odometry orientation message

edit

I am trying to get the orientation of the robot and turn it for example 90degrees. I am getting the the robots angle if I keep the program running, but I am not able to ensure that I will get the orientation information before calling turn() function. When the turn() function is called I thought that this problem should solve functions waitForMessage and spinOnce. So my question is how to get the orientation value and than call the function turn()? When the turn fuction is called the yaw_current contains still the initial value 0. What am I missing?

double yaw_current=0;


    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);

      /// odom subscriber
        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);

      /// end


      ros::Rate r(10);
      while(ros::ok())
      {


         // set_velocities(0.0, 0.0);

          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);
    }


//turn to the right (negative angle change)
void turn(float angleDeg, double angularSpeed, int direction)
{
    float angleRad = angleDeg*M_PI/180;
    float endAngleRad = yaw_current - angleRad;
    if (endAngleRad < -M_PI)
    {
       endAngleRad = M_PI + (endAngleRad + M_PI);
    }
    ros::Rate rate(10);
    while(yaw_current > endAngleRad)
    {
        set_velocities(0.0, 0.2);
        ros::spinOnce();
        rate.sleep();

    }
    set_velocities(0.0, 0.0);
}


    void getOdom(const nav_msgs::Odometry::ConstPtr& msg)
    {

        tf::Quaternion quat;
        tf::quaternionMsgToTF(msg->pose.pose.orientation, quat);

        tf::Matrix3x3(quat).getRPY(roll_current,pitch_current,yaw_current);
        ROS_INFO("roll=%f, pitch=%f, yaw=%f ",roll_current,pitch_current,yaw_current);
    }