What you need is a fixed reference for both goal and robot pose. Here's an example with /map frame (sorry it is a C++ cause I don't master Python code):
while(ros::ok())
{
//declare some variables
tf::StampedTransform poseRobot;
geometry_msgs::PoseStamped robot_pose;
//initialize robot angle
double yaw_r(50);
//try what's between {}
try
{
ROS_DEBUG("Pose du robot ...");
// have the transform from /map to /base_link (robot frame) and put it in "poseRobot": this is the robot pose in /map!
listener.lookupTransform("/map","/base_link",ros::Time(0), poseRobot);
//get the orientation: it is a quaternion so don't worry about the z. It doesn't mean there is a 3D thing here!
robot_pose.pose.orientation.x = poseRobot.getRotation().getX();
robot_pose.pose.orientation.y = poseRobot.getRotation().getY();
robot_pose.pose.orientation.z = poseRobot.getRotation().getZ();
robot_pose.pose.orientation.w = poseRobot.getRotation().getW();
// convert the quaternion to an angle (the yaw of the robot in /map frame!)
yaw_r = tf::getYaw(robot_pose.pose.orientation);
}
catch(tf::TransformException &ex) //if the try doesn't succeed, you fall here!
{
// continue
ROS_INFO("error. no robot pose!");
}
//angleDes is the goal angle in /map frame you get it by subscribing to goal
//The angleDes you should get it in a call back normally by converting the goals orientation to a yaw. Exactly as we have done to get robot's yaw
//delta_yaw is what you want to get
double delta_yaw = angleDes - yaw_r;
//and here I just make sure my angle is between minus pi and pi!
if (delta_yaw > M_PI)
delta_yaw -= 2*M_PI;
if (delta_yaw <= -M_PI)
delta_yaw += 2*M_PI;
You can replace /map frame with /odom frame.
Suppose now that your goal is not in /map frame.
This how you convert it with tf:
//some bool to check if conversion is alright
success =false;
try
{
ROS_DEBUG("transform from goalFrame to mapFrame ...");
//prepare conversion by sync'ing times
ros::Time current_transform = ros::Time::now();
listener2.getLatestCommonTime(currentGoal.header.frame_id, "map", current_transform, NULL);
currentGoal.header.stamp = current_transform;
//conversion
geometry_msgs::PoseStamped goalInMap;
listener2.transformPose("/map", currentGoal, goalInMap);
angleDes = tf::getYaw(goalInMap.pose.orientation);
success =true;
}
catch(tf::TransformException &ex)
{
// continue
ROS_DEBUG("error. no transform from goal frame to map frame!");
}