ROS tool box's cantransform() and gettransform() still trigger on a stale tf.

I am using Apriltag_ros, and this publishes a tf transform between the camera and the Apriltag. I have collected a number of datasets to use to do landmark slam, and i wish to use this tf transforms to get the relitive location of the apriltags. To extract this camera -> tag transform i use canTransfom() to see if the transfom is avalible and then gettransform() to extract the relitive location.

This works well until the apriltags are no longer detected and the camera -> tag tf stops being published and goes stale. However cantransform remains true, and getransform() seems to just extract the last published transform.

Is there anyway to prevent this? Thanks a lot for any help you can provide me with.

Answer 1

[Josh Chen]

Hi Mattew,

It sounds like this might be related to the 'BufferTime'. By default, the buffer time for buffering transformations is 10. Can you try specifying the buffer time to a smaller value and see if it helps?

Thanks,

Josh

[Matthew Coombes]

Thanks a lot for your answer. I am doing this from a bag file. so for example

bag = rosbag('x.bag')

tfTransform = bag.gettransform('camera','tag_0',spesifedTime).

Can i spesify a buffer time from a bag file. As bag.BufferTime does not exist.

[Josh Chen]

Oh sorry the for confusion, when looking at your original post, I thought you are referring to rostf.

If you are extracting from a bag file, there is actually a new API - rosbagreader. Can you try and see if this gives you the expected behavior?

Or if it's okay to share a dummy bag file to reproduce this issue, I can help take a closer look on my end.

[Matthew Coombes]

Are you saying that if i used rosbagreader i might not have the buffer time problem? can i set the buffer time in rosbag reader.

Here is the bag file i am using.

https://lunet-my.sharepoint.com/:u:/g/personal/ttmjc2_lunet_lboro_ac_uk/EfRC9XD84DFKm9qQ_2jfPkwBXUsP7BDlZLQhXrG6E33rpQ?e=xDJwJr

The following is the test code i am using. The plot shows the robot (triangle) driving around (in the odom frame) and the tag locations (text by tag number). As you can see some of them move wildly, this is when an of stagnent transform is being used from gettransfom(). As i am plotting them in the odom frame they should stay mosly still as the apriltags are in fixed locations.

clear

close all

clc

bagName = 'output.bag';

bagSel = rosbag(bagName);

% Extract robot odometry

odomselect = select(bagSel,'Topic','/odometry/filtered');

odomObjs = odomselect.readMessages;

%% tag tf names

TAGS = 0:88;

possibleTags={['tag_',num2str(TAGS(1))]};

for i = TAGS(2:end)

possibleTags{i} = ['tag_',num2str(i)];

end

for i = 1:1:length(odomObjs)

figure(1)

[~, PoseSE2] = translateOdomMsg(odomObjs{i});

plot(PoseSE2(1),PoseSE2(2),'^');hold on

for ii = 1:length(possibleTags)

if bagSel.canTransform('base_link', possibleTags{ii}, odomObjs{i}.Header.Stamp)

TagTstamped = bagSel.getTransform('base_link', possibleTags{ii}, odomObjs{i}.Header.Stamp);

[~, landSE2] = translateTransformStampedMsg(TagTstamped);

% Plot tag location in odom frame

R = [cos(PoseSE2(3)) -sin(PoseSE2(3)); sin(PoseSE2(3)) cos(PoseSE2(3))];

rotP = R*[landSE2(1);landSE2(2)];

priorLand = rotP+[PoseSE2(1);PoseSE2(2)];

% plot(priorLand(1),priorLand(2),'*')

text(priorLand(1),priorLand(2),num2str(ii))

end

end

axis([-20 20 -20 20])

drawnow

hold off

end

%% FUN

function [ToutSE2, poseSE2] = translateTransformStampedMsg(Tin)

%translateTransformMsg Extract the 4x4 homogeneous transformation matrix,

% ToutSE3, from a TransformStamped message object. This function also

% returns a second output, poseSE2, which is an SE(2) pose vector

% computed by projecting ToutSE2 to the XY plane. Note the formulation

% used is approximate and relies on the assumption that the robot mostly

% moves on the flat ground.

% Copyright 2020 The MathWorks, Inc.

%Tin - TransformStamped message object

x = Tin.Transform.Translation.X;

y = Tin.Transform.Translation.Y;

z = Tin.Transform.Translation.Z;

qx = Tin.Transform.Rotation.X;

qy = Tin.Transform.Rotation.Y;

qz = Tin.Transform.Rotation.Z;

qw = Tin.Transform.Rotation.W;

q = [ qw, qx, qy, qz]; % Note the sequence for quaternion in MATLAB

% ToutSE3 = robotics.core.internal.SEHelpers.poseToTformSE3([x,y,z,q]);

YPR = quat2eul(q);

poseSE2 = [x,y,YPR(1)];

ToutSE2 = robotics.core.internal.SEHelpers.poseToTformSE2(poseSE2);

end

function [ToutSE3, poseSE2] = translateOdomMsg(Tin)

%translateTransformMsg Extract the 4x4 homogeneous transformation matrix,

% ToutSE3, from a TransformStamped message object. This function also

% returns a second output, poseSE2, which is an SE(2) pose vector

% computed by projecting ToutSE2 to the XY plane. Note the formulation

% used is approximate and relies on the assumption that the robot mostly

% moves on the flat ground.

% Copyright 2020 The MathWorks, Inc.

%Tin - TransformStamped message object

x = Tin.Pose.Pose.Position.X;

y = Tin.Pose.Pose.Position.Y;

z = Tin.Pose.Pose.Position.Z;

qx = Tin.Pose.Pose.Orientation.X;

qy = Tin.Pose.Pose.Orientation.Y;

qz = Tin.Pose.Pose.Orientation.Z;

qw = Tin.Pose.Pose.Orientation.W;

q = [ qw, qx, qy, qz]; % Note the sequence for quaternion in MATLAB

ToutSE3 = robotics.core.internal.SEHelpers.poseToTformSE3([x,y,z,q]);

YPR = quat2eul(q);

poseSE2 = [x,y,YPR(1)];

end

[Josh Chen]

I just tried this on my side as well, looks like rosbagreader also doesn't give the option to set the buffer range.

Well, I guess one thing you can try to replace 'canTransform' is to write a function to read all '/tf' messages from the bag file, based on difference between odomObjs{i}.Header.Stamp the msg.Transforms.Header.Stamp information, return boolean to indicate whether such transform is valid.