turtlebot simulator gives wrong angle

edit

Hi all,

I writing code to rotate the turtlebot to a given angle and after that print out the angle by subscring the "/odom" topic when I enter the angle it rotate to the angle and print out the angle but the printed angle is wrong ....

can anyone help me .....

thanks

this's my code ::

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>

using namespace std;

ros::Subscriber pose_subscriber;
ros::Publisher velocity_publisher;
nav_msgs::Odometry robot_pose;  

const double PI = 3.14159265;

void rotate(double angular_speed,double angle,bool clockwise);
double degrees2radians(double angle_in_degrees);
void poseCallback(const nav_msgs::Odometry::ConstPtr& pose_message);

int main(int argc,char** argv){
    double angle;   
    bool clockwise;
    ros::init(argc,argv,"GotoGoal_node");
    ros::NodeHandle nh;
    velocity_publisher = nh.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/navi",1000);
    pose_subscriber = nh.subscribe("odom",1000,poseCallback);
    cout << "enter angle : ";
    cin >> angle;
    cout << "clockwise : ";
    cin >> clockwise;
    rotate(degrees2radians(angle),degrees2radians(angle),clockwise);
    ros::spin();
    return 0;
}


void rotate(double angular_speed,double relative_angle,bool clockwise){
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = 0;
    vel_msg.linear.y = 0;
    vel_msg.linear.z = 0;
    vel_msg.angular.x = 0;
    vel_msg.angular.y = 0;
    if(clockwise)
        vel_msg.angular.z = -abs(angular_speed);
    else
        vel_msg.angular.z = abs(angular_speed);
    double t0 = ros::Time::now().toSec();
    double current_angle = 0.0;
    ros::Rate loop_rate(100);
    do{
        velocity_publisher.publish(vel_msg);
        double t1 = ros::Time::now().toSec();
        current_angle = angular_speed*(t1-t0);
        ros::spinOnce();
        loop_rate.sleep();
    }while(current_angle < relative_angle);
    vel_msg.angular.z = 0;
    velocity_publisher.publish(vel_msg);
}
double degrees2radians(double angle_in_degrees){
    return angle_in_degrees*(PI/180.0);
}


void poseCallback(const nav_msgs::Odometry::ConstPtr& pose_message){
    double roll,pitch,yaw;
    tf::Quaternion q;
    tf::quaternionMsgToTF(pose_message->pose.pose.orientation,q);
    tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
    cout << " yaw : " << yaw*(180.0/PI) << endl ; 
}

this photo before entering the angle ::

link text

and this after entering 90 degree angle and clockwise ::

link text