getTransform for first/last TF from ROS bag

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.

Answer 1

[Cam Salzberger]

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