Ask Your Question
0

transform broadcaster orientation problem

asked 2014-10-01 13:05:47 -0600

choog gravatar image

updated 2014-10-01 14:19:55 -0600

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. Straight line When robot starts turning

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Please descrive what "go bad" means. What you expect to see and what you are actually seeing. Also documentation in your code as to what you want to happen would be helpful.

tfoote gravatar imagetfoote ( 2014-10-01 13:44:50 -0600 )edit

I have edited my post hopefully it's a little more clear. Thanks Mr. tfoote creator of the tf universe.

choog gravatar imagechoog ( 2014-10-01 14:20:38 -0600 )edit

Also I put a description on the pictures but the are not coming up. The top picture is showing the turtlebot and the broadcasted base_footprint. The bottom picture is showing what happens when the turtlebot is going in positive direction on the y axis the distance of the broadcasted frame is kept

choog gravatar imagechoog ( 2014-10-01 14:23:11 -0600 )edit

and so is the orientation. The top picture is what goes wrong when the robot turns.

choog gravatar imagechoog ( 2014-10-01 14:23:32 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2014-10-01 15:25:56 -0600

tfoote gravatar image

Your problem is that you're inverting the order of the publishing without inverting the matrix. You lookup the transform from base_footprint to odom, but you publish odom to leader_base_footprint. Inverting the direction requires inverting the transform.

Note you have the same problem with your base_link to base_footprint, but it's likely the identity or very small with out rotations, so you don't notice.

edit flag offensive delete link more

Comments

Thank you that actually fixed the problem. Now time for the Odom frame.

choog gravatar imagechoog ( 2014-10-02 13:39:47 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-10-01 13:05:47 -0600

Seen: 608 times

Last updated: Oct 01 '14