lookupTransform at tf2::TimePointZero returns TransformStamped with wrong stamp

[doisyg]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• Installation type:
  • bin
• Version or commit hash:
  • last available Humble binaries
• DDS implementation:
  • Cyclone
• Client library (if applicable):
  • rclcpp

Steps to reproduce issue

Use

lookupTransform(
    const std::string & target_frame, const std::string & source_frame,
    const TimePoint & time)

with time set to tf2::TimePointZero

Check the returned geometry_msgs::msg::TransformStamped header.stamp

Expected behavior

It should be set to the actual time of the last available transform between target_frame and source_frame.

Actual behavior

The returned geometry_msgs::msg::TransformStamped header.stamp is 0

Additional information

Maybe it is the actual intended behavior, but then how to know the time of the returned transform ?

[doisyg]

Update: I have this behavior only for static transforms

[clalancette]

So I don't actually think that the problem is static transforms, or, at least, not only static transforms. On Rolling, if I do:

$ ros2 run tf2_ros static_transform_publisher --x 1 --frame-id foo --child-frame-id bar
[INFO] [1672761369.565637888] [static_transform_publisher_qyIfnlMM3fyfkffp]: Spinning until stopped - publishing transform
translation: ('1.000000', '0.000000', '0.000000')
rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
from 'foo' to 'bar'

Then I get:

$ ros2 topic echo /tf_static
transforms:
- header:
    stamp:
      sec: 1672761369
      nanosec: 563991215
    frame_id: foo
  child_frame_id: bar
  transform:
    translation:
      x: 1.0
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
---

That said, there does look to be a corner case in tf2 where it can return a zero timestamp: https://github.com/ros2/geometry2/blob/9139e0c266cfd579d55e2ef36b786fb334b15665/tf2/src/buffer_core.cpp#L636-L651 . That is, if the target frame is the same as the source frame, and the time point is TimePointZero, and the frame number is not in the cache, then it can return an identity transform with a zero time. Could that possibly be what you are seeing?

[doisyg]

I have the same result as you if doing $ ros2 topic echo /tf_static, but this is not using lookupTransform. It is easy to test with tf2_echo which is using lookupTransform with tf2::TimePointZero/ tf2::TimePoint(): https://github.com/ros2/geometry2/blob/9139e0c266cfd579d55e2ef36b786fb334b15665/tf2_ros/src/tf2_echo.cpp#L140-L143

$ ros2 run tf2_ros static_transform_publisher --x 1 --frame-id foo --child-frame-id bar

$ ros2 run tf2_ros tf2_echo foo bar
[INFO] [1672791952.303256053] [tf2_echo]: Waiting for transform foo ->  bar: Invalid frame ID "foo" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [1.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.000]
- Rotation: in RPY (degree) [0.000, -0.000, 0.000]
- Matrix:
  1.000  0.000  0.000  1.000
  0.000  1.000  0.000  0.000
  0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000

[doisyg]

Fixed tf: timestamp of tf is 0

With tf_test_fixed.urdf

<?xml version="1.0" ?>
<robot name="tf_test">
  <link name="foo">
  </link>
  <link name="bar"/>
  <joint name="foo_to_bar" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 1.0"/>
    <parent link="foo"/>
    <child link="bar"/>
  </joint>
</robot>

$ ros2 run robot_state_publisher robot_state_publisher tf_test_fixed.urdf

$ ros2 run tf2_ros tf2_echo foo bar
[INFO] [1672792343.113754836] [tf2_echo]: Waiting for transform foo ->  bar: Invalid frame ID "foo" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [1.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.000]
- Rotation: in RPY (degree) [0.000, -0.000, 0.000]
- Matrix:
  1.000  0.000  0.000  1.000
  0.000  1.000  0.000  0.000
  0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000

Non fixed tf: timestamp is from last available

With tf_test_prismatic.urdf

<?xml version="1.0" ?>

$ ros2 run robot_state_publisher robot_state_publisher tf_test_prismatic.urdf

$ ros2 run joint_state_publisher joint_state_publisher

$ ros2 run tf2_ros tf2_echo foo bar
[INFO] [1672792523.782666204] [tf2_echo]: Waiting for transform foo ->  bar: Invalid frame ID "foo" passed to canTransform argument target_frame - frame does not exist
At time 1672792524.679667805
- Translation: [0.000, 0.000, 1.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.000]
- Rotation: in RPY (degree) [0.000, -0.000, 0.000]
- Matrix:
  1.000  0.000  0.000  0.000
  0.000  1.000  0.000  0.000
  0.000  0.000  1.000  1.000
  0.000  0.000  0.000  1.000