How to process geometry messages of type PoseWithCovarianceStamped and PoseStampe

edit

Hi, I am using ROS fuerte version on Ubuntu 10.04.

I have a screen shot below (WHOLE TOPIC.jpg) which tells about what type of messages are published on topics amcl_pose and move_base_simple/goal :

WHOLE TOPIC.jpg

Messages of amcl_pose are passed to a callback function,called currentPosCallback

void CmdVelMux::currentPosCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{      
 double xPos;
 double yPos;
 double zPos;
 xPos = msg->pose.pose.position.x;
 ROS_INFO("the position value is %lf \n ",xPos);
}

Messages of move_base_simple/goal are passed to a callback function,called goalPosCallback

void CmdVelMux::goalPosCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{      
    double x;
    double y;
    double z;   
    x = msg->pose.position.x;
    y = msg->pose.position.y;
   z = msg->pose.position.z;
   ROS_INFO("the position value is %lf \n ",x);
}

Question 1:I want to calculate angle between current goal(from move_base_simple/goal topic) and robot position (amcl_pose).

In trigonometry, we can calculate angle between two points P1(x1,y1) and P2(x2,y2). Is there any way to do this type of thing here?

I want to store this angle in class member data, what is the guarantee that it is the most updated one?

How to do that?

Thanks for any kind of help................