Revision history [back]

What you need is a fixed reference for both goal and robot pose. Here's an example with /map frame:

while(ros::ok())
{
    //compute robot angle in /map frame
    tf::StampedTransform poseRobot;
    geometry_msgs::PoseStamped robot_pose;
    double yaw_r(50);
    try
    {
        ROS_DEBUG("Pose du robot ...");
        listener.lookupTransform("/map","/base_link",ros::Time(0), poseRobot);

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

        yaw_r = tf::getYaw(robot_pose.pose.orientation);  
    }
    catch(tf::TransformException &ex)
    {
        // continue
        ROS_INFO("error. no robot pose!");
    }

            //angleDes is the goal angle in /map frame you get it by subscribing to goal 

            //delta_yaw is what you want to get
            double delta_yaw = angleDes - yaw_r;
    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.

What you need is a fixed reference for both goal and robot pose. Here's an example with /map frame:frame (sorry it is a C++ cause I don't master Python code):

while(ros::ok())
{
    //compute robot angle in /map frame
    tf::StampedTransform poseRobot;
    geometry_msgs::PoseStamped robot_pose;
    double yaw_r(50);
    try
    {
        ROS_DEBUG("Pose du robot ...");
        listener.lookupTransform("/map","/base_link",ros::Time(0), poseRobot);

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

        yaw_r = tf::getYaw(robot_pose.pose.orientation);  
    }
    catch(tf::TransformException &ex)
    {
        // continue
        ROS_INFO("error. no robot pose!");
    }

            //angleDes is the goal angle in /map frame you get it by subscribing to goal 

            //delta_yaw is what you want to get
            double delta_yaw = angleDes - yaw_r;
    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.

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())
{
    //compute robot angle in /map frame
    tf::StampedTransform poseRobot;
    geometry_msgs::PoseStamped robot_pose;
    double yaw_r(50);
    try
    {
        ROS_DEBUG("Pose du robot ...");
        listener.lookupTransform("/map","/base_link",ros::Time(0), poseRobot);

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

        yaw_r = tf::getYaw(robot_pose.pose.orientation);  
    }
    catch(tf::TransformException &ex)
    {
        // continue
        ROS_INFO("error. no robot pose!");
    }

            //angleDes is the goal angle in /map frame you get it by subscribing to goal 

            //delta_yaw is what you want to get
            double delta_yaw = angleDes - yaw_r;
    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.

success =false;
try
{
    ROS_DEBUG("transform from goalFrame to mapFrame ...");

    ros::Time current_transform = ros::Time::now(); 
    listener2.getLatestCommonTime(currentGoal.header.frame_id, "map", current_transform, NULL);
    currentGoal.header.stamp = current_transform;

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

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())
{
    //compute robot angle in /map frame
//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)
&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!");
}