Currently I'm using this method:
double pos_x, pos_y, quat_z, quat_w, orientation_angle_x;
void poseCallback(const nav_msgs::Odometry& msgIn)
{
pos_x = msgIn.pose.pose.position.x;
pos_y = msgIn.pose.pose.position.y;
quat_z = msgIn.pose.pose.orientation.z;
quat_w = msgIn.pose.pose.orientation.w;
orientation_angle_x = 2*atan2(quat_z,quat_w);
// cout<<"Angle ====>>"<<(float)(orientation_angle_x*(180.0/3.1415))<<endl;
}
void updatePosition(ros::NodeHandle nh, string pose_str, double& temp_pos_x, double& temp_pos_y, double& temp_orientation_angle_x)
{
ros::Rate rate(5);
ros::Subscriber sub2 = nh.subscribe(pose_str, 1, &poseCallback);
pos_x = 0;
while(ros::ok()) {
ros::spinOnce();
if(pos_x!=0)
{
temp_pos_x = pos_x; temp_pos_y = pos_y;
temp_orientation_angle_x = orientation_angle_x;
cout<<"\t"<<pose_str<<"\t POS_X = "<<pos_x<<"\t POS_Y = "<<pos_y<<"\t ANGLE = "<<orientation_angle_x<<endl;
break;
}
else
cout<<"\t Updating Position: Waiting for information..."<<endl;
// Wait until it's time for another iteration.
rate.sleep();
}
}
But I think there should another easier method.