필터 지우기
필터 지우기

getTransform for first/last TF from ROS bag

조회 수: 3 (최근 30일)
Antoine Harlé
Antoine Harlé 2022년 6월 28일
답변: Cam Salzberger 2022년 8월 4일
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에서 Specialized Messages에 대해 자세히 알아보기

제품


릴리스

R2022a

Community Treasure Hunt

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

Start Hunting!

Translated by