Time drift between depth frame andHost PC

[gachiemchiep]

Hello guys

Recently, we're developing reconstruction software using D435 and UR5 robot.
The D435 is attached to UR5 and moved around the object to generate a 3d model of it.
As I understand theUR5 driver uses host PC clock for the timestamp.
And D435 should also use the host PC clock for the timestamp
Unfortunately, when we checked it, the D435 is 0.3 seconds behind the host PC clock.
This leads to wrong tf transformation and screw the reconstruction software.

The last frame of UR5 robot (Host PC clock) and D435 look like this.

# rostopic echo /joint_states/header
seq: 2326
stamp: 
  secs: 1599705031
  nsecs: 715743943
frame_id: ''

#  rostopic echo /camera_1/depth/image_rect_raw/header
seq: 587
stamp: 
  secs: 1599705031
  nsecs: 410665076
frame_id: "camera_1_depth_optical_frame"

I'm using v2.32.1 and the settings of depth look like this.

[ INFO] [1599705989.272236011]: Running with LibRealSense v2.32.1


(base) jil@tr_ws:~/workspace/catkin_ws/src/jil_object_tracking_3d/jil_3d_reconstruction/src$ rostopic echo /camera_1/stereo_module/parameter_descriptions
groups: 
  - 
    name: "default"
    type: ''
    parameters: 
      - 
        name: "exposure"
        type: "int"
        level: 0
        description: "Depth Exposure (usec)"
        edit_method: ''
      - 
        name: "gain"
        type: "int"
        level: 0
        description: "UVC image gain"
        edit_method: ''
      - 
        name: "visual_preset"
        type: "int"
        level: 0
        description: "Advanced-Mode Preset"
        edit_method: "{'enum_description': '', 'enum': [{'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 0, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Custom'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 1, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Default'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 2, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Hand'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 3, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'High Accuracy'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 4, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'High Density'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 5, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Medium Density'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 6, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Remove Ir Pattern'}]}"
      - 
        name: "emitter_enabled"
        type: "int"
        level: 0
        description: "Emitter select, 0-disable all emitters, 1-enable laser, 2-enable laser auto (opt),\
  \ 3-enable LED (opt)"
        edit_method: "{'enum_description': '', 'enum': [{'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 1, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Laser'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 2, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Laser Auto'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 0, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Off'}]}"
      - 
        name: "frames_queue_size"
        type: "int"
        level: 0
        description: "Max number of frames you can hold at a given time. Increasing this number will reduce\
  \ frame drops but increase latency, and vice versa"
        edit_method: ''
      - 
        name: "laser_power"
        type: "double"
        level: 0
        description: "Manual laser power in mw. applicable only when laser power mode is set to Manual"
        edit_method: ''
      - 
        name: "inter_cam_sync_mode"
        type: "double"
        level: 0
        description: "Inter-camera synchronization mode: 0:Default, 1:Master, 2:Slave"
        edit_method: ''
      - 
        name: "enable_auto_exposure"
        type: "bool"
        level: 0
        description: "Enable Auto Exposure"
        edit_method: ''
      - 
        name: "error_polling_enabled"
        type: "bool"
        level: 0
        description: "Enable / disable polling of camera internal errors"
        edit_method: ''
      - 
        name: "output_trigger_enabled"
        type: "bool"
        level: 0
        description: "Generate trigger from the camera to external device once per frame"
        edit_method: ''
      - 
        name: "emitter_on_off"
        type: "bool"
        level: 0
        description: "Alternating Emitter Pattern: 0:disabled(default), 1:enabled( emitter is toggled on/off\
  \ on per-frame basis)"
        edit_method: ''
      - 
        name: "global_time_enabled"
        type: "bool"
        level: 0
        description: "Enable/Disable global timestamp."
        edit_method: ''
    parent: 0
    id: 0
max: 
  bools: 
    - 
      name: "enable_auto_exposure"
      value: True
    - 
      name: "error_polling_enabled"
      value: True
    - 
      name: "output_trigger_enabled"
      value: True
    - 
      name: "emitter_on_off"
      value: True
    - 
      name: "global_time_enabled"
      value: True
  ints: 
    - 
      name: "exposure"
      value: 165000
    - 
      name: "gain"
      value: 248
    - 
      name: "visual_preset"
      value: 6
    - 
      name: "emitter_enabled"
      value: 0
    - 
      name: "frames_queue_size"
      value: 32
  strs: []
  doubles: 
    - 
      name: "laser_power"
      value: 360.0
    - 
      name: "inter_cam_sync_mode"
      value: 2.0
  groups: []
min: 
  bools: 
    - 
      name: "enable_auto_exposure"
      value: False
    - 
      name: "error_polling_enabled"
      value: False
    - 
      name: "output_trigger_enabled"
      value: False
    - 
      name: "emitter_on_off"
      value: False
    - 
      name: "global_time_enabled"
      value: False
  ints: 
    - 
      name: "exposure"
      value: 1
    - 
      name: "gain"
      value: 16
    - 
      name: "visual_preset"
      value: 0
    - 
      name: "emitter_enabled"
      value: 0
    - 
      name: "frames_queue_size"
      value: 0
  strs: [](base) jil@tr_ws:~/workspace/catkin_ws/src/jil_object_tracking_3d/jil_3d_reconstruction/src$ rostopic echo /camera_1/stereo_module/parameter_descriptions
groups: 
  - 
    name: "default"
    type: ''
    parameters: 
      - 
        name: "exposure"
        type: "int"
        level: 0
        description: "Depth Exposure (usec)"
        edit_method: ''
      - 
        name: "gain"
        type: "int"
        level: 0
        description: "UVC image gain"
        edit_method: ''
      - 
        name: "visual_preset"
        type: "int"
        level: 0
        description: "Advanced-Mode Preset"
        edit_method: "{'enum_description': '', 'enum': [{'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 0, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Custom'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 1, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Default'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 2, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Hand'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 3, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'High Accuracy'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 4, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'High Density'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 5, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Medium Density'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 6, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Remove Ir Pattern'}]}"
      - 
        name: "emitter_enabled"
        type: "int"
        level: 0
        description: "Emitter select, 0-disable all emitters, 1-enable laser, 2-enable laser auto (opt),\
  \ 3-enable LED (opt)"
        edit_method: "{'enum_description': '', 'enum': [{'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 1, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Laser'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 2, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Laser Auto'}, {'srcline': 0, 'description': '', 'srcfile': '/does/this/really/matter.cfg',\
  \ 'cconsttype': 'const int', 'value': 0, 'ctype': 'int', 'type': 'int', 'name':\
  \ 'Off'}]}"
      - 
        name: "frames_queue_size"
        type: "int"
        level: 0
        description: "Max number of frames you can hold at a given time. Increasing this number will reduce\
  \ frame drops but increase latency, and vice versa"
        edit_method: ''
      - 
        name: "laser_power"
        type: "double"
        level: 0
        description: "Manual laser power in mw. applicable only when laser power mode is set to Manual"
        edit_method: ''
      - 
        name: "inter_cam_sync_mode"
        type: "double"
        level: 0
        description: "Inter-camera synchronization mode: 0:Default, 1:Master, 2:Slave"
        edit_method: ''
      - 
        name: "enable_auto_exposure"
        type: "bool"
        level: 0
        description: "Enable Auto Exposure"
        edit_method: ''
      - 
        name: "error_polling_enabled"
        type: "bool"
        level: 0
        description: "Enable / disable polling of camera internal errors"
        edit_method: ''
      - 
        name: "output_trigger_enabled"
        type: "bool"
        level: 0
        description: "Generate trigger from the camera to external device once per frame"
        edit_method: ''
      - 
        name: "emitter_on_off"
        type: "bool"
        level: 0
        description: "Alternating Emitter Pattern: 0:disabled(default), 1:enabled( emitter is toggled on/off\
  \ on per-frame basis)"
        edit_method: ''
      - 
        name: "global_time_enabled"
        type: "bool"
        level: 0
        description: "Enable/Disable global timestamp."
        edit_method: ''
    parent: 0
    id: 0
max: 
  bools: 
    - 
      name: "enable_auto_exposure"
      value: True
    - 
      name: "error_polling_enabled"
      value: True
    - 
      name: "output_trigger_enabled"
      value: True
    - 
      name: "emitter_on_off"
      value: True
    - 
      name: "global_time_enabled"
      value: True
  ints: 
    - 
      name: "exposure"
      value: 165000
    - 
      name: "gain"
      value: 248
    - 
      name: "visual_preset"
      value: 6
    - 
      name: "emitter_enabled"
      value: 0
    - 
      name: "frames_queue_size"
      value: 32
  strs: []
  doubles: 
    - 
      name: "laser_power"
      value: 360.0
    - 
      name: "inter_cam_sync_mode"
      value: 2.0
  groups: []
min: 
  bools: 
    - 
      name: "enable_auto_exposure"
      value: False
    - 
      name: "error_polling_enabled"
      value: False
    - 
      name: "output_trigger_enabled"
      value: False
    - 
      name: "emitter_on_off"
      value: False
    - 
      name: "global_time_enabled"
      value: False
  ints: 
    - 
      name: "exposure"
      value: 1
    - 
      name: "gain"
      value: 16
    - 
      name: "visual_preset"
      value: 0
    - 
      name: "emitter_enabled"
      value: 0
    - 
      name: "frames_queue_size"
      value: 0
  strs: []
  doubles: 
    - 
      name: "laser_power"
      value: 0.0
    - 
      name: "inter_cam_sync_mode"
      value: 0.0
  groups: []
dflt: 
  bools: 
    - 
      name: "enable_auto_exposure"
      value: True
    - 
      name: "error_polling_enabled"
      value: True
    - 
      name: "output_trigger_enabled"
      value: False
    - 
      name: "emitter_on_off"
      value: False
    - 
      name: "global_time_enabled"
      value: True
  ints: 
    - 
      name: "exposure"
      value: 8500
    - 
      name: "gain"
      value: 16
    - 
      name: "visual_preset"
      value: 0
    - 
      name: "emitter_enabled"
      value: 1
    - 
      name: "frames_queue_size"
      value: 16
  strs: []
  doubles: 
    - 
      name: "laser_power"
      value: 150.0
    - 
      name: "inter_cam_sync_mode"
      value: 0.0
  groups: []

  doubles: 
    - 
      name: "laser_power"
      value: 0.0
    - 
      name: "inter_cam_sync_mode"
      value: 0.0
  groups: []
dflt: 
  bools: 
    - 
      name: "enable_auto_exposure"
      value: True
    - 
      name: "error_polling_enabled"
      value: True
    - 
      name: "output_trigger_enabled"
      value: False
    - 
      name: "emitter_on_off"
      value: False
    - 
      name: "global_time_enabled"
      value: True
  ints: 
    - 
      name: "exposure"
      value: 8500
    - 
      name: "gain"
      value: 16
    - 
      name: "visual_preset"
      value: 0
    - 
      name: "emitter_enabled"
      value: 1
    - 
      name: "frames_queue_size"
      value: 16
  strs: []
  doubles: 
    - 
      name: "laser_power"
      value: 150.0
    - 
      name: "inter_cam_sync_mode"
      value: 0.0
  groups: []

Is there any method to sync the clocks between D435 and the Host PC?

[MartyG-RealSense]

Hi @gachiemchiep The subject of syncing between host computer time and ROS time is discussed in the link below, with advice provided by Doronhi the RealSense ROS wrapper developer.

#796

I note that you seem to be changing the value of the frame queue size in your settings. Is this correct please? If so, doing so is not recommended as you can end up accidentally breaking the streams with your chosen value. Dorodnic the RealSense SDK Manager describes this in the comment linked to below:

IntelRealSense/librealsense#5041 (comment)

I believe the default frame queue size is '1', with '2' being suggested by the librealsense frame management documentation if using both color and depth streams instead of just a depth stream.

https://github.com/IntelRealSense/librealsense/wiki/Frame-Buffering-Management-in-RealSense-SDK-2.0#latency-vs-performance

Also, do you have any Frame metadata isn't available! messages in your ROS log when you perform roslaunch?

[gachiemchiep]

Hello @MartyG-RealSense .
We did change some parameters inside the realsense launch file. We will revert the change in that file and send you the log again.

We didn't change the frame_queue_size value, so I think those values are default values of realsense-ros. Based on a comment here #937 (comment) . I think the value of frame_queue_size is totally fine.

[gachiemchiep]

Hello @MartyG-RealSense
Yes we have that message appear in ros launch console

[ WARN] [1599793582.728492795]: Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)

We used the default realsense launch file and now the latency is as :

---
# rostopic echo /joint_states/header

seq: 1853
stamp: 
  secs: 1599793858
  nsecs: 846618389
frame_id: ''

#  rostopic echo /camera_1/depth/image_rect_raw/header

seq: 498
stamp: 
  secs: 1599793858
  nsecs: 822190046
frame_id: "camera_1_depth_optical_frame"

[MartyG-RealSense]

It is recommended to match a particular ROS wrapper version as closely as possible with a specific librealsense SDK version listed on the ROS wrapper's Releases page.

It looks as though the best fit for SDK 2.32.1 (the one that you listed as being used by you) would be ROS wrapper 2.2.12 if you are not using it already.

https://github.com/IntelRealSense/realsense-ros/releases/tag/2.2.12

In regard to the Frame metadata isn't available warning, Doronhi the RealSense ROS wrapper developer offers advice in the link below:

#932 (comment)

I am not sure how to interpret the latency readings from using the default realsense launch file. Is it better or worse than the 0.3 seconds that you reported originally please?

[MartyG-RealSense]

Hi @gachiemchiep Do you require further assistance with this case, please? Thanks!

[doronhi]

Frame metadata is needed for accurate time stamps. Without it the timestamps are approximately the host time when the frame arrived to the host which could be 0.3 seconds after it was taken. You should try to fix the Frame metadata isn't available warning.

[gachiemchiep]

Hello @dorodnic and @MartyG-RealSense
Thank you for your guys' advice. we're working on this problem.

[MartyG-RealSense]

Thanks very much for the update of your progress @gachiemchiep

[gachiemchiep]

Hello, using librealsense2 2.32.1 and realsense-ros 2.2.12 fix our problem.
The time drift between depth frame and host pc (robot) is less than 20ms now.
Thank you for your support

[MartyG-RealSense]

Great news @gachiemchiep - I'm very pleased that you found a solution. Thanks for letting us know!