ROS tool box's cantransform() and gettransform() still trigger on a stale tf.

1 次查看（过去 30 天）
Matthew Coombes
Matthew Coombes 2023-12-13
评论： Josh Chen 2023-12-18
I am using Apriltag_ros, and this publishes a tf transform between the camera and the Apriltag. I have collected a number of datasets to use to do landmark slam, and i wish to use this tf transforms to get the relitive location of the apriltags. To extract this camera -> tag transform i use canTransfom() to see if the transfom is avalible and then gettransform() to extract the relitive location.
This works well until the apriltags are no longer detected and the camera -> tag tf stops being published and goes stale. However cantransform remains true, and getransform() seems to just extract the last published transform.
Is there anyway to prevent this? Thanks a lot for any help you can provide me with.

请先登录，再回答此问题。

回答（1 个）

Josh Chen
Josh Chen 2023-12-13
Hi Mattew,
It sounds like this might be related to the 'BufferTime'. By default, the buffer time for buffering transformations is 10. Can you try specifying the buffer time to a smaller value and see if it helps?
Thanks,
Josh
  4 个评论
显示 2更早的评论
Matthew Coombes
Matthew Coombes 2023-12-13
Are you saying that if i used rosbagreader i might not have the buffer time problem? can i set the buffer time in rosbag reader.
Here is the bag file i am using.
https://lunet-my.sharepoint.com/:u:/g/personal/ttmjc2_lunet_lboro_ac_uk/EfRC9XD84DFKm9qQ_2jfPkwBXUsP7BDlZLQhXrG6E33rpQ?e=xDJwJr
The following is the test code i am using. The plot shows the robot (triangle) driving around (in the odom frame) and the tag locations (text by tag number). As you can see some of them move wildly, this is when an of stagnent transform is being used from gettransfom(). As i am plotting them in the odom frame they should stay mosly still as the apriltags are in fixed locations.
clear
close all
clc
bagName = 'output.bag';
bagSel = rosbag(bagName);
% Extract robot odometry
odomselect = select(bagSel,'Topic','/odometry/filtered');
odomObjs = odomselect.readMessages;
%% tag tf names
TAGS = 0:88;
possibleTags={['tag_',num2str(TAGS(1))]};
for i = TAGS(2:end)
possibleTags{i} = ['tag_',num2str(i)];
end
for i = 1:1:length(odomObjs)
figure(1)
[~, PoseSE2] = translateOdomMsg(odomObjs{i});
plot(PoseSE2(1),PoseSE2(2),'^');hold on
for ii = 1:length(possibleTags)
if bagSel.canTransform('base_link', possibleTags{ii}, odomObjs{i}.Header.Stamp)
TagTstamped = bagSel.getTransform('base_link', possibleTags{ii}, odomObjs{i}.Header.Stamp);
[~, landSE2] = translateTransformStampedMsg(TagTstamped);
% Plot tag location in odom frame
R = [cos(PoseSE2(3)) -sin(PoseSE2(3)); sin(PoseSE2(3)) cos(PoseSE2(3))];
rotP = R*[landSE2(1);landSE2(2)];
priorLand = rotP+[PoseSE2(1);PoseSE2(2)];
% plot(priorLand(1),priorLand(2),'*')
text(priorLand(1),priorLand(2),num2str(ii))
end
end
axis([-20 20 -20 20])
drawnow
hold off
end
%% FUN
function [ToutSE2, poseSE2] = translateTransformStampedMsg(Tin)
%translateTransformMsg Extract the 4x4 homogeneous transformation matrix,
% ToutSE3, from a TransformStamped message object. This function also
% returns a second output, poseSE2, which is an SE(2) pose vector
% computed by projecting ToutSE2 to the XY plane. Note the formulation
% used is approximate and relies on the assumption that the robot mostly
% moves on the flat ground.
% Copyright 2020 The MathWorks, Inc.
%Tin - TransformStamped message object
x = Tin.Transform.Translation.X;
y = Tin.Transform.Translation.Y;
z = Tin.Transform.Translation.Z;
qx = Tin.Transform.Rotation.X;
qy = Tin.Transform.Rotation.Y;
qz = Tin.Transform.Rotation.Z;
qw = Tin.Transform.Rotation.W;
q = [ qw, qx, qy, qz]; % Note the sequence for quaternion in MATLAB
% ToutSE3 = robotics.core.internal.SEHelpers.poseToTformSE3([x,y,z,q]);
YPR = quat2eul(q);
poseSE2 = [x,y,YPR(1)];
ToutSE2 = robotics.core.internal.SEHelpers.poseToTformSE2(poseSE2);
end
function [ToutSE3, poseSE2] = translateOdomMsg(Tin)
%translateTransformMsg Extract the 4x4 homogeneous transformation matrix,
% ToutSE3, from a TransformStamped message object. This function also
% returns a second output, poseSE2, which is an SE(2) pose vector
% computed by projecting ToutSE2 to the XY plane. Note the formulation
% used is approximate and relies on the assumption that the robot mostly
% moves on the flat ground.
% Copyright 2020 The MathWorks, Inc.
%Tin - TransformStamped message object
x = Tin.Pose.Pose.Position.X;
y = Tin.Pose.Pose.Position.Y;
z = Tin.Pose.Pose.Position.Z;
qx = Tin.Pose.Pose.Orientation.X;
qy = Tin.Pose.Pose.Orientation.Y;
qz = Tin.Pose.Pose.Orientation.Z;
qw = Tin.Pose.Pose.Orientation.W;
q = [ qw, qx, qy, qz]; % Note the sequence for quaternion in MATLAB
ToutSE3 = robotics.core.internal.SEHelpers.poseToTformSE3([x,y,z,q]);
YPR = quat2eul(q);
poseSE2 = [x,y,YPR(1)];
end
Josh Chen
Josh Chen 2023-12-18
I just tried this on my side as well, looks like rosbagreader also doesn't give the option to set the buffer range.
Well, I guess one thing you can try to replace 'canTransform' is to write a function to read all '/tf' messages from the bag file, based on difference between odomObjs{i}.Header.Stamp the msg.Transforms.Header.Stamp information, return boolean to indicate whether such transform is valid.

请先登录，再进行评论。

类别

在 Help Center 和 File Exchange 中查找有关 ROS Network Connection and Exploration 的更多信息

标签

产品


版本

R2022b

Community Treasure Hunt

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

Start Hunting!

Translated by