transform broadcaster orientation problem

edit

Hello,

I am using a turtlebot 2 with ros-groovy. I have successfully wrote a publisher that takes the base_link and broadcasts that info. I am working on writing a broadcaster for the base_footprint. Also my base_link broadcaster works perfect. That is why I just followed the same format for that. But I don't understand why this logic is happening.

The base_foot print broadcaster seems to be messing up though. The broadcaster is able to keep the correct distance but when I start turning the robot things go bad. What goes wrong is that as the robot starts turning the base_footprint frame starts revolving around the fixed map frame (0,0,0). What I would expect is that whenever the turtlebot turns in the z direction that the base_footprint stays with the robot model on rviz like it is suppose to. If anyone could help it would be appreciated.

I have 2 examples there of what Is going on so you can get an idea.

while(ros::ok()){
    tf::StampedTransform transform;  //for the base_link
    tf::StampedTransform transformFP; //for the base_footprint
    current_time = ros::Time::now();

//This is for the base_link frame which works great this block of code just gets the transform and that transform is 
//broadcasted as leader_base_link to the tf tree.

    try {
                        listener.waitForTransform("base_link", "base_footprint", ros::Time(0), ros::Duration(10.0));
            listener.lookupTransform("base_link", "base_footprint", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
        }
    yaw =   tf::getYaw(transform.getRotation()); 
        v.setValue(transform.getOrigin().x() , transform.getOrigin().y() , transform.getOrigin().z());

    leader_base_link.setOrigin(v);
    q.setRPY(0.0, 0.0, yaw);
    leader_base_link.setRotation(q );
    br.sendTransform(tf::StampedTransform(leader_base_link,     ros::Time::now(), "base_footprint", "leader_base_link"));

    double th = yaw;
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
    transformConverter.header.stamp = current_time; 
    transformConverter.header.frame_id = "base_footprint";
        transformConverter.child_frame_id = "base_link";
    transformConverter.transform.translation.x = transform.getOrigin().x();
    transformConverter.transform.translation.y = transform.getOrigin().y();
    transformConverter.transform.translation.z = transform.getOrigin().z();
    transformConverter.transform.rotation = odom_quat;





 /*This is where the base_footprint is being created I also believe this is where the code is messing up.
    As the robot turns in the yaw angle the position of the base_footprint starts translating into a + or - direction but the  distance is kept . As you can see in the picture that is what happens when the robot turns in a 180 degrees. The base_footprint gets shifted in 180 degrees  This is following the same exact code as the base_link that works fine as well*/    
            try {
                        listener.waitForTransform("base_footprint", "odom", ros::Time(0), ros::Duration(10.0));
                listener.lookupTransform("base_footprint", "odom", ros::Time(0), transformFP);
            }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
        }

*/  double tryYaw = atan2( (2*transformFP.getRotation().w()*transformFP.getRotation().z() ),                           (transformFP.getRotation().w()*transformFP.getRotation().w() - transformFP.getRotation().z()*transformFP.getRotation().z()));       This is another equation I tried but it basically does the same exact stuff. */



//this is where the yaw angle gets calculated.
yaw2 =  tf::getYaw(transformFP.getRotation()); 
            v.setValue(transformFP.getOrigin().x() , transformFP.getOrigin().y() , transformFP.getOrigin().z());

    leader_footprint.setOrigin(v);
    q ...