getTransform for first/last TF from ROS bag

7 次查看(过去 30 天)
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.

回答(1 个)

Cam Salzberger
Cam Salzberger 2022-8-4
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

类别

Help CenterFile Exchange 中查找有关 Network Connection and Exploration 的更多信息

产品


版本

R2022a

Community Treasure Hunt

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

Start Hunting!

Translated by