1
$\begingroup$

I want a callback to respond to an update between any of the tf2 transforms between frame A and B on the /tf topic. It should use the most recent transform available between every frame. Some frames update on a low frequency but remail valid until a newer one appears.

My first attempt at realizing this was using regular tf2_buffer_->lookupTransform functions in this fashion:

transform = tf_buffer_->lookupTransform(vehicle_frame, reference_frame, tf2::TimePointZero);

This yielded the following behavior:

  • This seemed to search backwards to find the newest time that it has all required information to do a transformation.
  • At that time, it looks up the transformation

My problem I observed with this approach is that if one of my frames is old (although still valid) the messagefilter returns the transformation at that time, instead of using newer transforms of other frames through the tree.

I do not care about historical data of my transforms, and want to have convenient access to the most recent buffered data and transformation functionality regardless of its time since its last published.

An approach that worked was looking up the latest transforms and assign transformations manually:

// Lookup the two latest transforms to get from reference_frame to measurement_frame
transform_ref_to_world = tf_buffer_->lookupTransform(fixed_frame, reference_frame, rclcpp::Time(0));
transform_target_to_world = tf_buffer_->lookupTransform(fixed_frame, measurement_frame, rclcpp::Time(0));

// convert from geometry_msgs::msg::TransformStamped to tf2::Transform
transform_ref_to_world_tf = geometry_msgs_transform_to_tf2_transform(transform_ref_to_world.transform);
transform_target_to_world_tf = geometry_msgs_transform_to_tf2_transform(transform_target_to_world.transform);

// Combine the latest values
result_transform_tf = transform_target_to_world_tf.inverse().operator*(transform_ref_to_world_tf);

This does what I want, but relies on a hardcoded transform tree structure.

Does anyone know a more charming way to achieve this?

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.