Skip to content

Fix discrepancy between tf and odometry/filtered when world_frame is set to map_frame #939

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: jazzy-devel
Choose a base branch
from
Open

Fix discrepancy between tf and odometry/filtered when world_frame is set to map_frame #939

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 18 additions & 19 deletions src/ros_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2178,30 +2178,29 @@ void RosFilter<T>::periodicUpdate()
if (filtered_position->header.frame_id == odom_frame_id_) {
world_transform_broadcaster_->sendTransform(world_base_link_trans_msg_);
} else if (filtered_position->header.frame_id == map_frame_id_) {
try {
tf2::Transform world_base_link_trans;
tf2::fromMsg(
world_base_link_trans_msg_.transform,
world_base_link_trans);

tf2::Transform base_link_odom_trans;
tf2::fromMsg(
tf_buffer_
->lookupTransform(
base_link_frame_id_,
odom_frame_id_,
tf2::TimePointZero)
.transform,
base_link_odom_trans);

tf2::Transform world_base_link_trans;
tf2::fromMsg(
world_base_link_trans_msg_.transform,
world_base_link_trans);

tf2::Transform base_link_odom_trans;
bool can_transform_odom = ros_filter_utilities::lookupTransformSafe(
tf_buffer_.get(),
base_link_frame_id_,
odom_frame_id_,
filtered_position->header.stamp,
tf_timeout_,
Copy link
Collaborator

@ayrton04 ayrton04 Aug 15, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

While I welcome the change, I think this behaviour is going to have to be a parameter, and it should default to false so we don't break anyone else's application. If we have to wait for the odom->base_link transform to be available, it could affect the latency and publication rate of the output (which is why we're using the latest available transform to begin with).

Copy link
Author

@sybrenkappert sybrenkappert Aug 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have had some time to think. In the current form, the latency could indeed be affected in applications that have overridden the transform_timeout parameter. Rather than adding a parameter to enable/disable this behavior, I suggest we add a dedicated timeout parameter for the odom->base_link transform lookup. Reason being:

The transform_timeout parameter is specifically used during the conversion of measurements to the target frame. This purpose differs significantly from retrieving the odometry transform. I can imagine developers would want to set timeouts of these two cases to different values.

By adding a new parameter (e.g. transform_timeout_odom) and setting the default value to zero, the latency in existing applications should not be affected. The accuracy could improve even with the default value as it allows computation of a better transform if there is sufficient data in the transform buffer. Developers can override this new timeout parameter with a value that suits their application to benefit from improved broadcasted transform accuracy. Would this be a good approach?

Copy link
Collaborator

@ayrton04 ayrton04 Aug 28, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, that seems valid!

Copy link
Collaborator

@ayrton04 ayrton04 Aug 28, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe transform_timeout_odom_bl?

Copy link
Collaborator

@ayrton04 ayrton04 Oct 27, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@sybrenkappert any chance you would want to update the PR with my feedback? If you've moved on and dropped this, it's totally fine, but in that case, would you mind closing the PR? Thanks.

Copy link
Author

@sybrenkappert sybrenkappert Oct 27, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ayrton04 Thanks for the feedback! I'm planning on updating the PR, I wasn't able to find time to work on this until now. Hoping to publish an update soon.

base_link_odom_trans);

if(can_transform_odom) {
/*
* First, see these two references:
* http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms#lookupTransform
* http://wiki.ros.org/geometry/CoordinateFrameConventions#Transform_Direction
* We have a transform from map_frame_id_->base_link_frame_id_, but
* it would actually transform a given pose from
* base_link_frame_id_->map_frame_id_. We then used lookupTransform,
* whose first two arguments are target frame and source frame, to
* base_link_frame_id_->map_frame_id_. We then used lookupTransformSafe,
* whose second and third arguments are target frame and source frame, to
* get a transform from base_link_frame_id_->odom_frame_id_.
* However, this transform would actually transform data from
* odom_frame_id_->base_link_frame_id_. Now imagine that we have a
Expand All @@ -2227,7 +2226,7 @@ void RosFilter<T>::periodicUpdate()
map_odom_trans_msg.child_frame_id = odom_frame_id_;

world_transform_broadcaster_->sendTransform(map_odom_trans_msg);
} catch (...) {
} else {
RCLCPP_ERROR_STREAM_SKIPFIRST_THROTTLE(
get_logger(),
*get_clock(),
Expand Down