Add Robot Hardware Timestamp Synchronization

[srvald]

Summary

This PR introduces a new feature to synchronize ROS 2 message timestamps directly with the Universal Robots controller's internal hardware clock. Additionally, a constant latency compensation parameter has been added.

Changes

• Parameters Added:
  • use_robot_timestamp (bool): Enables anchoring the timeline to the robot's hardware clock.
  • t_delay (double): Constant network latency compensation factor in seconds [s].
• Node Core (RtdePublisherNode):
  • Implemented initial reference clock anchoring (t_0 and T_0) upon receiving the first RTDE package.
  • Reconstructed the subsequent timeline based on monotonic robot time elapsed increments (dt).
• Documentation: Updated README.rst and doc/usage.rst with the new parameters' descriptions.

---

Verification & Testing

The feature was validated using the following launch configuration with a real UR robot:

ros2 launch ur_rtde_publisher rtde_publisher.launch.xml \
  robot_ip:=10.54.5.218 \
  output_recipe:='["safety_status", "robot_mode", "payload_inertia", "actual_TCP_pose", "target_TCP_pose"]' \
  rtde_frequency:=500 \
  tf_prefix:="test" \
  use_robot_timestamp:=true \
  t_delay:=0.004

Log Analysis & Mathematical Proof

Looking at the initialization sequence, we can verify that the timeline synchronization behaves exactly as specified:

1. Reference Anchor Setup (First Packet):

   • Host ROS Time: 1779781131.140454
   • Robot Hardware Timestamp (T_0): 9010.081000
   • Applied Latency Compensation (t_delay:=0.004): The adjusted base time (t_0) calculation matches perfectly:
     1779781131.140454 - 0.004 = 1779781131.136454
   • Resulting Packet Time: 1779781131.136454 (with dt = 0.000000)

2. Timeline Progression (Subsequent Packets):

   • Packet 2: Robot Timestamp increases to 9010.087000 (delta = 0.006). The calculated packet_time increments to 1779781131.142454, tracking the robot clock linearly instead of the host processor time.

[INFO] [launch]: All log files can be found below /home/mirserv/.ros/log/2026-05-26-09-38-50-850791-ia-li-9yp6qn3.unirobotts.local-89160
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ur_rtde_publisher_node-1]: process started with pid [89175]
[ur_rtde_publisher_node-1] [INFO] [1779781131.101168948] [ur_rtde_publisher_node]: Value use_robot_timestamp: true
[ur_rtde_publisher_node-1] [INFO] [1779781131.101232119] [ur_rtde_publisher_node]: Value t_delay_:  0
[ur_rtde_publisher_node-1] [INFO] [1779781131.102926203] [ur_rtde_publisher_node]: YAML file loaded successfully.
[ur_rtde_publisher_node-1] [INFO] [1779781131.103047220] [ur_rtde_publisher_node]: Creating publishers for recipe with 5 variables
[ur_rtde_publisher_node-1] [INFO] [1779781131.106152112] [ur_rtde_publisher_node]: Created SafetyStatus publisher for safety_status on topic rtde/safety_status
[ur_rtde_publisher_node-1] [INFO] [1779781131.106577672] [ur_rtde_publisher_node]: Created RobotMode publisher for robot_mode on topic rtde/robot_mode
[ur_rtde_publisher_node-1] [INFO] [1779781131.109139294] [ur_rtde_publisher_node]: Created InertiaStamped publisher for payload_inertia on topic rtde/payload_inertia
[ur_rtde_publisher_node-1] [INFO] [1779781131.109583048] [ur_rtde_publisher_node]: Created PoseStamped publisher for actual_TCP_pose on topic rtde/actual_TCP_pose
[ur_rtde_publisher_node-1] [INFO] [1779781131.109823781] [ur_rtde_publisher_node]: Created PoseStamped publisher for target_TCP_pose on topic rtde/target_TCP_pose
[ur_rtde_publisher_node-1] [INFO] [1779781131.109873953] [ur_rtde_publisher_node]: Created 5 publishers from recipe
[ur_rtde_publisher_node-1] [INFO] [1779781131.139103809] [ur_rtde_publisher_node]: RTDE client started (ip=10.54.5.218) with 5 keys.
[ur_rtde_publisher_node-1] [INFO] [1779781131.139302828] [ur_rtde_publisher_node]: RTDE publisher node initialized successfully
[ur_rtde_publisher_node-1] [INFO] [1779781131.139334772] [ur_rtde_publisher_node]: RTDE read thread started
[ur_rtde_publisher_node-1] [INFO] [1779781131.140454147] [ur_rtde_publisher_node]: #########################################
[ur_rtde_publisher_node-1] [INFO] [1779781131.140500948] [ur_rtde_publisher_node]: Value ROS time: 1779781131.140454
[ur_rtde_publisher_node-1] [INFO] [1779781131.140543414] [ur_rtde_publisher_node]: using timestamp
[ur_rtde_publisher_node-1] [INFO] [1779781131.140567481] [ur_rtde_publisher_node]: Value timestamp: 9010.081000
[ur_rtde_publisher_node-1] [INFO] [1779781131.140599812] [ur_rtde_publisher_node]: Value t_0 (adjusted): 1779781131.136454
[ur_rtde_publisher_node-1] [INFO] [1779781131.140617320] [ur_rtde_publisher_node]: Value T_0: 9010.081000
[ur_rtde_publisher_node-1] [INFO] [1779781131.140652734] [ur_rtde_publisher_node]: Value dt: 0.000000
[ur_rtde_publisher_node-1] [INFO] [1779781131.140682401] [ur_rtde_publisher_node]: Value packet time: 1779781131.136454
[ur_rtde_publisher_node-1] [INFO] [1779781131.163475996] [ur_rtde_publisher_node]: #########################################
[ur_rtde_publisher_node-1] [INFO] [1779781131.163562976] [ur_rtde_publisher_node]: Value ROS time: 1779781131.163470
[ur_rtde_publisher_node-1] [INFO] [1779781131.163584980] [ur_rtde_publisher_node]: Value timestamp: 9010.087000
[ur_rtde_publisher_node-1] [INFO] [1779781131.163592858] [ur_rtde_publisher_node]: Value dt: 0.006000
[ur_rtde_publisher_node-1] [INFO] [1779781131.163607137] [ur_rtde_publisher_node]: Value packet time: 1779781131.142454
[ur_rtde_publisher_node-1] [INFO] [1779781131.163854249] [ur_rtde_publisher_node]: #########################################