Ask Your Question
0

get rotation state of a joint with tf::listener.lookupTransform

asked 2020-01-23 09:33:03 -0600

lotfishtaine gravatar image

Hello everyone,

I have a velodyne with a rotating joint. Now, for each velodyne scan, I want to use tf to obtain the instantaneous pose of the lidar in order to be able to transform the 3D point clouds. For that, I use tf::TransformListener listener.lookupTransform :

cloudCallback(inputPoints);
ros::Time scanTime = inputPoints->header.stamp;

listener.waitForTransform("world","velodyne", scanTime, ros::Duration(1.0));
listener.lookupTransform("world","velodyne", scanTime, transform);

however, I get always :

Extrapolation Error looking up joint pose: Lookup would require extrapolation at time 1882.101000000, but only time 1882.137000000 is in the buffer, when looking up transform from frame [velodyne] to frame [world]

I also have tried to use tf2_ros::MessageFilter like here but I wasn't sure that's needed, since I'm getting the transformed data directly, so no need to transform it again.

or trying to define the transform listener as class member variable and initialize it in the constructor as suggested by @tfoote here or here

tf2_ros::Buffer tf_;
tf2_ros::TransformListener tfListener;

Ctr{tfListener(TF_),
...}

transformStamped = tf_.lookupTransform("world",ros::Time::now(),"md4_200/velodyne", scanTime,"world", ros::Duration(1.0));

with that, I get :

Extrapolation Error looking up joint pose: Lookup would require extrapolation at time 6505.806000000, but only time 6505.779000000 is in the buffer, when looking up transform from frame [velodyne] to frame [world]

I have checked using : rosrun tf tf_echo world velodyne

At time 6686.625
- Translation: [0.000, 0.000, 0.060]
- Rotation: in Quaternion [-0.671, 0.223, 0.671, 0.223]
            in RPY (radian) [1.083, 1.571, -2.696]
            in RPY (degree) [62.037, 90.000, -154.477]
At time 6686.625
- Translation: [0.000, 0.000, 0.060]
- Rotation: in Quaternion [-0.671, 0.223, 0.671, 0.223]
            in RPY (radian) [1.083, 1.571, -2.696]
            in RPY (degree) [62.037, 90.000, -154.477]

Can anyone help advise on to solve this issue please?

edit retag flag offensive close merge delete

Comments

I tried with ros::Time()

tf::TransformListener listener;
tf::StampedTransform transform;
try{
    listener.waitForTransform("world","velodyne", ros::Time(), ros::Duration(1.0));
    listener.lookupTransform("world","velodyne", ros::Time(), transform);
}
catch (tf2::LookupException& ex) {
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up joint pose: %s\n", ex.what());
}
catch (tf2::ConnectivityException& ex){
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up joint pose: %s\n", ex.what());
}
catch (tf2::ExtrapolationException& ex){
 ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up joint pose: %s\n", ex.what());
}

it works, but the scans are all offset, because the joint timestamps are different from the scan timestamps. what I want is to get the joint pose precisely in the scan timestamp.

ros::Time scanTime = inputPoints->header.stamp;
lotfishtaine gravatar imagelotfishtaine ( 2020-01-24 04:03:43 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-01-24 13:57:13 -0600

tfoote gravatar image

Your basic problem is that you're asking to lookup data from the transform tree before it's available.

You need to wait for the transform to become available: http://wiki.ros.org/tf/Tutorials/tf%2...

The message filters are likely still the correct way to achieve what you want. http://wiki.ros.org/tf/Tutorials/Usin...

They will hold the data until the transform is available. To use it to query for arbitrary data. You can manually call add to add queries to the system that will then get the callback invoked when they become available. And you can just ignore anything but the timestamp and frame ids.

Reading your original description again though I think that the MessageFilter as implemented is exactly designed for what you want though. It will hold the message until the transform is available. And then you can apply the transform in the callback. Which appears to be exactly what you're talking about doing.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-01-23 09:33:03 -0600

Seen: 15 times

Last updated: Jan 24