getTransform for first/last TF from ROS bag
조회 수: 3 (최근 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.
댓글 수: 0
답변 (1개)
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
댓글 수: 0
참고 항목
카테고리
Help Center 및 File Exchange에서 Specialized Messages에 대해 자세히 알아보기
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!