0
$\begingroup$

Rosanswers logo

Hello

I tried to make a node that gives me the distance between the fixed point and the baselink over the time. But I get wrong distances. hould be in the range of 0 to some like 10 meters. But I got totaly uncenece datas.. like start from 5 and than go till 15 . So no cence results

This is my node

    #include <ros/ros.h>
#include <tf/transform_listener.h>


int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");

  ros::NodeHandle node;
  tf::TransformListener listener;

  ros::Rate rate(10.0);

  while (node.ok())

    {
    tf::StampedTransform transform;
    try{
      listener.lookupTransform("/base_link", "/door1",  
                               ros::Time(0), transform);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
    }


    double x = transform.getOrigin().x();
        double y = transform.getOrigin().y();
    double dist1 = sqrt(x*x + y*y);
    ROS_INFO("%f", dist1);

    rate.sleep();


  }
  return 0;
};

And this the label in the launch file

Any help???

This my frames.pdfimage description

Yes Sure

The output of

rosrun tf tf_echo /base_link /door1 is

At time 1301628270.947
- Translation: [4.651, 10.067, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.532, 0.847]
            in RPY [0.000, 0.000, -1.122]
At time 1301628270.947
- Translation: [4.651, 10.067, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.532, 0.847]
            in RPY [0.000, 0.000, -1.122]
At time 1301628270.947
- Translation: [4.651, 10.067, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.532, 0.847]
            in RPY [0.000, 0.000, -1.122]
^CAt time 1301628270.947
- Translation: [4.651, 10.067, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.532, 0.847]
            in RPY [0.000, 0.000, -1.122]

and the distance is 

[ INFO] [1345720655.721469719, 1301628270.653578061]:  Dist to Door1 =11.086891
[ INFO] [1345720655.868379126, 1301628270.793675137]:  Dist to Door1 =11.087869
[ INFO] [1345720656.020382546, 1301628270.803828608]:  Dist to Door1 =11.090092
[ INFO] [1345720656.039301088, 1301628270.950554490]:  Dist to Door1 =11.090092

And the output of rosrun tf tf_echo /world /door1

At time 1301628270.978
- Translation: [4.500, 4.500, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY [0.000, -0.000, 0.000]
At time 1301628270.978
- Translation: [4.500, 4.500, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY [0.000, -0.000, 0.000]
At time 1301628270.978
- Translation: [4.500, 4.500, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY [0.000, -0.000, 0.000]
^CAt time 1301628270.978
- Translation: [4.500, 4.500, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY [0.000, -0.000, 0.000]

[ INFO] [1345721049.231849091, 1301628270.478065900]:  Dist to Door1 =6.513392
[ INFO] [1345721049.371391812, 1301628270.634387495]:  Dist to Door1 =6.513717
[ INFO] [1345721049.535096985, 1301628270.778485784]:  Dist to Door1 =6.513787
[ INFO] [1345721049.536815193, 1301628270.778485784]:  Dist to Door1 =6.513787
[ INFO] [1345721049.673510162, 1301628270.947420969]:  Dist to Door1 =6.513787

And from the rosrun tf tf_echo /world /base_link

At time 1301628270.947
- Translation: [4.850, 2.665, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.811, 0.586]
            in RPY [0.000, -0.000, 1.890]
At time 1301628270.947
- Translation: [4.850, 2.665, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.811, 0.586]
            in RPY [0.000, -0.000, 1.890]
At time 1301628270.947
- Translation: [4.850, 2.665, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.811, 0.586]
            in RPY [0.000, -0.000, 1.890]
^CAt time 1301628270.947
- Translation: [4.850, 2.665, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.811, 0.586]
            in RPY [0.000, -0.000, 1.890]

[ INFO] [1345721361.623291271, 1301628270.605040589]:  Dist to Door1 =1.868834
[ INFO] [1345721361.767260212, 1301628270.773273617]:  Dist to Door1 =1.869268
[ INFO] [1345721361.918387565, 1301628270.910657656]:  Dist to Door1 =1.868726
[ INFO] [1345721361.918494447, 1301628270.910657656]:  Dist to Door1 =1.868726

Originally posted by Astronaut on ROS Answers with karma: 330 on 2012-08-22

Post score: 0


Original comments

Comment by Astronaut on 2012-08-22:
What is the comment or answer of the question? I can not see it

Comment by Martin Günther on 2012-08-22:
Can you post the output of these commands?

rosrun tf tf_echo /base_link /door1
rosrun tf tf_echo /world /door1
rosrun tf tf_echo /world /base_link

... along with the distances you get (i.e., the output of your program)?

Comment by Astronaut on 2012-08-23:
i just post it

Comment by Lorenz on 2012-08-23:
Please always set the tags correctly to maximize the chance that the right people who can answer your question get notified.

Comment by Astronaut on 2012-08-23:
Ok, How to set this tag???

Comment by Martin Günther on 2012-08-23:
You can retag your question by clicking the "retag" button.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I tried to make a node that gives me the distance between the fixed point and the baselink over the time. [...]

  listener.lookupTransform("/base_link", "/door1",  
                          ros::Time(0), transform);

What are you trying to do? Find the distance between /world and /base_link or between base_link and door1? Your explanation and your code don't match.

Also, I am a little confused what your "Dist to Door1" outputs mean. In the first two cases, it seems as if they give the distance between the frames you mention in the rosrun tf tf_echo output, but the third one seems to be wrong.

From the tf_echo outputs it seems that everything is correct: /door1 is about 6.5 meters away from /world, /base_link is about 5.5 meters away from /world, and because they are rotated by about 180° and /world is almost in the middle between the two frames, the total distance from /base_link to /door1 is about 11 m.

You have to look at the RPY angles (in radian). Simply put, the rotation is applied before the translation.


Originally posted by Martin Günther with karma: 11816 on 2012-08-23

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.