getTransform for first/last TF from ROS bag

18 views (last 30 days)
Hi,
I wanted to access first/last TF from a ROS1 bag with getTransform method. However, finding the correct sourceTime to prevent the following errors can be tricky :
>> getTransform(tf2_bag,'F_RP','F_FDB',tf2_bag.EndTime)
Lookup would require extrapolation -0.058425515s into the future. Requested time 1656341857.275348902 but the latest data is at time
1656341857.216923475, when looking up transform from frame [F_FDB] to frame [F_RP]
>> getTransform(tf2_bag,'F_RP','F_FDB',tf2_bag.StartTime)
Lookup would require extrapolation 0.026089186s into the past. Requested time 1656341833.890916109 but the earliest data is at time
1656341833.917005301, when looking up transform from frame [F_FDB] to frame [F_RP]
From what i understood, getTransform method when not given any sourceTime will search for the last available TF, which is useful. But is there a way to use a similar approach for the first available TF ?
In the meantime, I used a custom function to solve this issue but it could be useful to have a more efficient solution built-in.
function [tf, time] = getClosestTransform(obj, targetFrame, sourceFrame, sourceTime)
%getClosestTransform Return transformation between two coordinate frames.
% If no transform is found at exact sourceTime. Search nearest available
% transform in time.
timestep=0.1; %s
maxDeltaTime=1; %s
deltaTime=0;
while deltaTime<maxDeltaTime
if canTransform(obj, targetFrame, sourceFrame, sourceTime+deltaTime)
time=sourceTime+deltaTime;
tf = getTransform(obj, targetFrame, sourceFrame, time);
break;
elseif canTransform(obj, targetFrame, sourceFrame, sourceTime-deltaTime)
time=sourceTime-deltaTime;
tf = getTransform(obj, targetFrame, sourceFrame, time);
break;
else
deltaTime=deltaTime+timestep;
end
end
if deltaTime>=maxDeltaTime
error('Failed to find a transform from frame [%s] to frame [%s] at time %f +/- %fs.',targetFrame,sourceFrame,sourceTime,deltaTime);
end
end
Cheers,
Antoine H.

Answers (1)

Cam Salzberger
Cam Salzberger on 4 Aug 2022
Hello Antoine,
If the bag contains the transformation messages in standard message recorded format, could you simply use this?
bagTfSel = select(bag, "Topic", "/tf");
firstLastMsg = readMessages(bagTfSel, [1 size(bagTfSel.AvailableMessages, 1)]);
firstMsg = firstLastMsg{1};
firstStamp = firstMsg.Header.Stamp;
lastMsg = firstLastMsg{end};
lastStamp = lastMsg.Header.Stamp;
-Cam

Products


Release

R2022a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by