Revision history [back]

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. 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;
    tf::StampedTransform transformFP;
    current_time = ros::Time::now();


    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.    
    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. */



yaw2 =  tf::getYaw(transformFP.getRotation()); 
            v.setValue(transformFP.getOrigin().x() , transformFP.getOrigin().y() , transformFP.getOrigin().z());

    leader_footprint.setOrigin(v);
    q.setRPY(0.0, 0.0, yaw2);
    leader_footprint.setRotation(q );
    br.sendTransform(tf::StampedTransform(leader_footprint,     ros::Time::now(), "odom", "leader_base_footprint"));



    double th2 = yaw2;

    geometry_msgs::Quaternion odom2_quat = tf::createQuaternionMsgFromYaw(th2);
    transformConverterFP.header.stamp = current_time;   
    transformConverterFP.header.frame_id = "odom";
        transformConverterFP.child_frame_id = "base_footprint";
    transformConverterFP.transform.translation.x = transformFP.getOrigin().x();
    transformConverterFP.transform.translation.y = transformFP.getOrigin().y();
    transformConverterFP.transform.translation.z = transformFP.getOrigin().z();
    transformConverterFP.transform.rotation = odom2_quat;
    ROS_INFO_STREAM("X pose " <<  transformConverterFP.transform.translation.x );
    ROS_INFO_STREAM("Y pose " << transformConverterFP.transform.translation.y);
    ROS_INFO_STREAM("Z pose " << transformConverterFP.transform.translation.z );
    ROS_INFO_STREAM("Yaw Interposed :  "<< yaw2 );
    ROS_INFO_STREAM("Yaw Z "<< transformFP.getRotation().w() );
    ROS_INFO_STREAM("Yaw Z "<< transformFP.getRotation().z() );

    pub.publish(transformConverter);
    pubFoot.publish(transformConverterFP);
    loopRate.sleep();


}

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;
transform;  //for the base_link
    tf::StampedTransform transformFP;
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 



 /*This is where the base_footprint is being created.    
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.setRPY(0.0, 0.0, yaw2);
    leader_footprint.setRotation(q );
//this is where the leader_footprint gets added to the transform tree
    br.sendTransform(tf::StampedTransform(leader_footprint,     ros::Time::now(), "odom", "leader_base_footprint"));


 //The rest of the code just used to be able to send the StampedTransform over a topic Don't worry too much about this part
    double th2 = yaw2;

    geometry_msgs::Quaternion odom2_quat = tf::createQuaternionMsgFromYaw(th2);
    transformConverterFP.header.stamp = current_time;   
    transformConverterFP.header.frame_id = "odom";
        transformConverterFP.child_frame_id = "base_footprint";
    transformConverterFP.transform.translation.x = transformFP.getOrigin().x();
    transformConverterFP.transform.translation.y = transformFP.getOrigin().y();
    transformConverterFP.transform.translation.z = transformFP.getOrigin().z();
    transformConverterFP.transform.rotation = odom2_quat;
    ROS_INFO_STREAM("X pose " <<  transformConverterFP.transform.translation.x );
    ROS_INFO_STREAM("Y pose " << transformConverterFP.transform.translation.y);
    ROS_INFO_STREAM("Z pose " << transformConverterFP.transform.translation.z );
    ROS_INFO_STREAM("Yaw Interposed :  "<< yaw2 );
    ROS_INFO_STREAM("Yaw Z "<< transformFP.getRotation().w() );
    ROS_INFO_STREAM("Yaw Z "<< transformFP.getRotation().z() );

    pub.publish(transformConverter);
    pubFoot.publish(transformConverterFP);
    loopRate.sleep();


}