Skip to content
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.

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

Unusable timestamps in D435i/D455 on Tinkerboard #1906

Closed
ramonvp opened this issue May 31, 2021 · 35 comments
Closed

Unusable timestamps in D435i/D455 on Tinkerboard #1906

ramonvp opened this issue May 31, 2021 · 35 comments
Labels

Comments

@ramonvp
Copy link

ramonvp commented May 31, 2021

Hi everyone, I am not sure if this issue should be posted here or in the librealsense repo, since I don't really know the origin of the issue. My platform:

Required Info  
Camera Model D435i, D455
Firmware Version 5.12.13.50
Operating System & Version Armbian 20.05.7 Focal
Kernel Version (Linux Only) 5.4.123-rockchip
Platform ARM
SDK Version 2.45
Language C++, Python
Segment others

Issue Description

I am trying to make a D435i work on the Tinkerboard S with Focal, which only offers USB2. I compiled both SDK v2.45 and RealSense ROS v2.3.0 from source. I also patched the kernel as indicated by the linux instructions. When launching rs_camera.launch, I can get the depth image as well as the RGB image. However, the timestamps look to me very odd and with a huge offset.
I have already read almost every issue posted in this forum and other ones as well, including:

To better observe the issue, consider the following Python script:

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image

def depth_cb(msg):
    diff = (rospy.Time.now() - msg.header.stamp).to_sec()
    print("Time diff is : " + str(diff))

rospy.init_node("header_publisher")

depth_sub = rospy.Subscriber("/camera/depth/image_rect_raw", Image, depth_cb)

rospy.spin()

I first tried this script in my Intel workstation. I actually have tried with 2 different computers, both with USB3.2:

  • Ubuntu 18 + Kernel 5.4.0-56-generic + ROS Melodic (kernel unpatched)
  • Ubuntu 20 + Kernel 5.8.0-44-generic + ROS Noetic (kernel unpatched)
    In both cases, I get all streams (depth, rgb, infra1, infra2, etc...) with the expected 30Hz, and the scripts outputs:
Time diff is : 0.004414319
Time diff is : -0.0028548250000000053
Time diff is : -0.0036184799999999795
Time diff is : -0.004709960000000013
Time diff is : -0.0039980419999999794
Time diff is : -0.0026743420000000517
Time diff is : -0.0009248259999999897
Time diff is : 0.002310276
Time diff is : 0.003344535
Time diff is : 0.005857705
Time diff is : 0.007889747
Time diff is : 0.01012206
.......

And then quickly stabilizes to a difference of around 10ms, which is a very reasonable value.

However, when I run this same script on the Tinkerboard, I get this very different output:

Time diff is : 13161.960861921
Time diff is : 13161.956075191
Time diff is : 13161.952967882
Time diff is : 13161.950196981
Time diff is : 13161.946646213
Time diff is : 13161.943586111
Time diff is : 13161.94018054
Time diff is : 13161.937459945
....
Time diff is : 13197.146675586
Time diff is : 13203.94861412

And after 15-20seconds, it stabilizes to a value of 13200secs = 3.7hours! It doesn't really stabilize, since the value keeps fluctuating. I am launching the ROS node with the following command, as suggested in other posts:

roslaunch realsense2_camera rs_camera.launch initial_reset:=True enable_sync:=True

This is the full output when launched above commands:

SUMMARY
========

PARAMETERS
 * /camera/realsense2_camera/accel_fps: -1
 * /camera/realsense2_camera/accel_frame_id: camera_accel_frame
 * /camera/realsense2_camera/accel_optical_frame_id: camera_accel_opti...
 * /camera/realsense2_camera/align_depth: False
 * /camera/realsense2_camera/aligned_depth_to_color_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/allow_no_texture_points: False
 * /camera/realsense2_camera/base_frame_id: camera_link
 * /camera/realsense2_camera/calib_odom_file: 
 * /camera/realsense2_camera/clip_distance: -2.0
 * /camera/realsense2_camera/color_fps: -1
 * /camera/realsense2_camera/color_frame_id: camera_color_frame
 * /camera/realsense2_camera/color_height: -1
 * /camera/realsense2_camera/color_optical_frame_id: camera_color_opti...
 * /camera/realsense2_camera/color_width: -1
 * /camera/realsense2_camera/confidence_fps: -1
 * /camera/realsense2_camera/confidence_height: -1
 * /camera/realsense2_camera/confidence_width: -1
 * /camera/realsense2_camera/depth_fps: -1
 * /camera/realsense2_camera/depth_frame_id: camera_depth_frame
 * /camera/realsense2_camera/depth_height: -1
 * /camera/realsense2_camera/depth_optical_frame_id: camera_depth_opti...
 * /camera/realsense2_camera/depth_width: -1
 * /camera/realsense2_camera/device_type: 
 * /camera/realsense2_camera/enable_accel: False
 * /camera/realsense2_camera/enable_color: True
 * /camera/realsense2_camera/enable_confidence: True
 * /camera/realsense2_camera/enable_depth: True
 * /camera/realsense2_camera/enable_fisheye1: False
 * /camera/realsense2_camera/enable_fisheye2: False
 * /camera/realsense2_camera/enable_fisheye: False
 * /camera/realsense2_camera/enable_gyro: False
 * /camera/realsense2_camera/enable_infra1: False
 * /camera/realsense2_camera/enable_infra2: False
 * /camera/realsense2_camera/enable_infra: False
 * /camera/realsense2_camera/enable_pointcloud: False
 * /camera/realsense2_camera/enable_pose: False
 * /camera/realsense2_camera/enable_sync: True
 * /camera/realsense2_camera/filters: 
 * /camera/realsense2_camera/fisheye1_frame_id: camera_fisheye1_f...
 * /camera/realsense2_camera/fisheye1_optical_frame_id: camera_fisheye1_o...
 * /camera/realsense2_camera/fisheye2_frame_id: camera_fisheye2_f...
 * /camera/realsense2_camera/fisheye2_optical_frame_id: camera_fisheye2_o...
 * /camera/realsense2_camera/fisheye_fps: -1
 * /camera/realsense2_camera/fisheye_frame_id: camera_fisheye_frame
 * /camera/realsense2_camera/fisheye_height: -1
 * /camera/realsense2_camera/fisheye_optical_frame_id: camera_fisheye_op...
 * /camera/realsense2_camera/fisheye_width: -1
 * /camera/realsense2_camera/gyro_fps: -1
 * /camera/realsense2_camera/gyro_frame_id: camera_gyro_frame
 * /camera/realsense2_camera/gyro_optical_frame_id: camera_gyro_optic...
 * /camera/realsense2_camera/imu_optical_frame_id: camera_imu_optica...
 * /camera/realsense2_camera/infra1_frame_id: camera_infra1_frame
 * /camera/realsense2_camera/infra1_optical_frame_id: camera_infra1_opt...
 * /camera/realsense2_camera/infra2_frame_id: camera_infra2_frame
 * /camera/realsense2_camera/infra2_optical_frame_id: camera_infra2_opt...
 * /camera/realsense2_camera/infra_fps: 30
 * /camera/realsense2_camera/infra_height: 480
 * /camera/realsense2_camera/infra_rgb: False
 * /camera/realsense2_camera/infra_width: 848
 * /camera/realsense2_camera/initial_reset: True
 * /camera/realsense2_camera/json_file_path: 
 * /camera/realsense2_camera/linear_accel_cov: 0.01
 * /camera/realsense2_camera/odom_frame_id: camera_odom_frame
 * /camera/realsense2_camera/ordered_pc: False
 * /camera/realsense2_camera/pointcloud_texture_index: 0
 * /camera/realsense2_camera/pointcloud_texture_stream: RS2_STREAM_COLOR
 * /camera/realsense2_camera/pose_frame_id: camera_pose_frame
 * /camera/realsense2_camera/pose_optical_frame_id: camera_pose_optic...
 * /camera/realsense2_camera/publish_odom_tf: True
 * /camera/realsense2_camera/publish_tf: True
 * /camera/realsense2_camera/rosbag_filename: 
 * /camera/realsense2_camera/serial_no: 
 * /camera/realsense2_camera/stereo_module/exposure/1: 7500
 * /camera/realsense2_camera/stereo_module/exposure/2: 1
 * /camera/realsense2_camera/stereo_module/gain/1: 16
 * /camera/realsense2_camera/stereo_module/gain/2: 16
 * /camera/realsense2_camera/tf_publish_rate: 0.0
 * /camera/realsense2_camera/topic_odom_in: odom_in
 * /camera/realsense2_camera/unite_imu_method: 
 * /camera/realsense2_camera/usb_port_id: 
 * /rosdistro: noetic
 * /rosversion: 1.15.9

NODES
  /camera/
    realsense2_camera (nodelet/nodelet)
    realsense2_camera_manager (nodelet/nodelet)

ROS_MASTER_URI=http://localhost:11311

process[camera/realsense2_camera_manager-1]: started with pid [7210]
process[camera/realsense2_camera-2]: started with pid [7211]
[ INFO] [1622463313.044422583]: Initializing nodelet with 4 worker threads.
[ INFO] [1622463313.403502744]: RealSense ROS v2.3.0
[ INFO] [1622463313.403814245]: Built with LibRealSense v2.45.0
[ INFO] [1622463313.404075580]: Running with LibRealSense v2.45.0
[ INFO] [1622463313.464884925]:  
 31/05 14:15:13,556 WARNING [2756690960] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: No data available, number: 3d
 31/05 14:15:13,607 WARNING [2756690960] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: No data available, number: 3d
[ INFO] [1622463313.804460933]: Device with serial number 049XXXXXXXXX was found.

[ INFO] [1622463313.804861685]: Device with physical ID 1-1.2-20 was found.
[ INFO] [1622463313.805124187]: Device with name Intel RealSense D455 was found.
[ INFO] [1622463313.806747029]: Device with port number 1-1.2 was found.
[ INFO] [1622463313.807013031]: Device USB type: 2.1
[ WARN] [1622463313.807231782]: Device 049XXXXXXXXX is connected using a 2.1 port. Reduced performance is expected.
[ INFO] [1622463313.807458992]: Resetting device...
[ INFO] [1622463319.919641342]:  
[ INFO] [1622463320.203137991]: Device with serial number 049XXXXXXXXX was found.

[ INFO] [1622463320.203331950]: Device with physical ID 1-1.2-21 was found.
[ INFO] [1622463320.203455909]: Device with name Intel RealSense D455 was found.
[ INFO] [1622463320.204693749]: Device with port number 1-1.2 was found.
[ INFO] [1622463320.204826167]: Device USB type: 2.1
[ WARN] [1622463320.204926792]: Device 049XXXXXXXXX is connected using a 2.1 port. Reduced performance is expected.
[ INFO] [1622463320.209113691]: getParameters...
[ INFO] [1622463320.313030155]: setupDevice...
[ INFO] [1622463320.313147989]: JSON file is not provided
[ INFO] [1622463320.313771576]: ROS Node Namespace: camera
[ INFO] [1622463320.314055661]: Device Name: Intel RealSense D455
[ INFO] [1622463320.314316121]: Device Serial No: 049XXXXXXXXX
[ INFO] [1622463320.314566372]: Device physical port: 1-1.2-21
[ INFO] [1622463320.314813999]: Device FW version: 05.12.13.50
[ INFO] [1622463320.315057250]: Device Product ID: 0x0B5C
[ INFO] [1622463320.315312460]: Enable PointCloud: Off
[ INFO] [1622463320.315580795]: Align Depth: Off
[ INFO] [1622463320.315759004]: Sync Mode: On
[ INFO] [1622463320.316274090]: Device Sensors: 
[ INFO] [1622463320.344475208]: Stereo Module was found.
[ INFO] [1622463320.365591995]: RGB Camera was found.
[ INFO] [1622463320.366235707]: Motion Module was found.
[ INFO] [1622463320.366376874]: (Confidence, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1622463320.366482166]: num_filters: 0
[ INFO] [1622463320.366550125]: Setting Dynamic reconfig parameters.
[ WARN] [1622463320.770441914]: Param '/camera/stereo_module/auto_exposure_limit' has value 0 that is not in range [1, 165000]. Removing this parameter from dynamic reconfigure options.
[ WARN] [1622463320.782879235]: Param '/camera/stereo_module/auto_gain_limit' has value 0 that is not in range [16, 248]. Removing this parameter from dynamic reconfigure options.
[ INFO] [1622463320.865453744]: Done Setting Dynamic reconfig parameters.
[ INFO] [1622463320.866722793]: depth stream is enabled - width: 640, height: 480, fps: 15, Format: Z16
[ INFO] [1622463320.868016051]: color stream is enabled - width: 640, height: 480, fps: 15, Format: RGB8
[ INFO] [1622463320.868177343]: setupPublishers...
[ INFO] [1622463320.874226836]: Expected frequency for depth = 15.00000
[ INFO] [1622463320.882238965]: Expected frequency for color = 15.00000
[ INFO] [1622463320.887432994]: setupStreams...
[ INFO] [1622463320.909705662]: insert Depth to Stereo Module
[ INFO] [1622463320.910048955]: insert Color to RGB Camera
 31/05 14:15:21,012 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
 31/05 14:15:21,062 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
 31/05 14:15:21,113 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
[ INFO] [1622463321.164020020]: SELECTED BASE:Depth, 0
[ INFO] [1622463321.183567047]: RealSense Node Is Up!
[ WARN] [1622463321.347595477]: 
[ERROR] [1622463321.347994479]: An error has occurred during frame callback: Time is out of dual 32-bit range
[ERROR] [1622463321.413401100]: An error has occurred during frame callback: Time is out of dual 32-bit range
 31/05 14:15:21,478 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
 31/05 14:15:21,529 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
 31/05 14:15:21,580 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
 31/05 14:15:21,630 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
 31/05 14:15:21,681 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
 31/05 14:15:21,731 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
 31/05 14:15:21,782 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
 31/05 14:15:21,832 WARNING [2681156624] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61

The issue happens with both D435i and D455 cameras. I am hoping there is some way to fix the issue, considering that other users have reported to being able to make it work with Raspberry Pi 3 and Odroid platforms.

I have also tried with kernel 5.10.xx, with identical results. Anybody has any suggestion on how should I proceed? Suggestions are very welcome, thanks!

@MartyG-RealSense
Copy link
Collaborator

Hi @ramonvp I note from your log that you have already tried the most common approach for dealing with the Time is out of dual 32-bit range - initial_reset and enable_sync.

If changing the reset or sync does not work, there are some less commonly used solutions that can be explored.

A solution for some RealSense ROS users has been to clean their catkin workspace.

#1201 (comment)

A user of Raspberry Pi (an Arm device like your board) switched their Pi from Raspbian to the Pi version of Ubuntu and also used a more stable power connection.

#1178 (comment)

Whilst someone on the same discussion suggested checking that the time on the computer is up to date.

#1178 (comment)

@ramonvp
Copy link
Author

ramonvp commented May 31, 2021

Hi @MartyG-RealSense, thanks for your quick reply.

Regarding the power supply, I already experienced that problem. At first, I was using an adapter that provided 5V-3A to the board, but I was getting these messages:

31/05 18:47:08,525 ERROR [2757739536] (handle-libusb.h:51) failed to open usb interface: 0, error: RS2_USB_STATUS_IO
[ WARN] [1622486828.527893281]: Device 1/1 failed with exception: **failed to set power state**
[ERROR] [1622486828.528583951]: The requested device with  is NOT found. Will Try again.
 31/05 18:47:08,526 ERROR [2782917648] (sensor.cpp:526) acquire_power failed: failed to set power state
 31/05 18:47:08,527 WARNING [2782917648] (rs.cpp:306) null pointer passed for argument "device"

I then switched to a power supply that was capable of providing 5V-4A and the message disappeared.

Changing the OS is not an option and besides, Armbian Focal is the most similar thing to Ubuntu for the Tinkerboard.

Cleaning the workspace: I did try that also, and even I created a separate workspace with only the necessary packages: realsense2_camera and realsense2_description. I cleaned and compiled from scratch several times.

Finally, setting correctly the system date was also one of the first things I checked. I would however expect the published timestamps to be according to the system date, no matter how off it is from the real one. But again, just in case, I double-checked.

The thing that I would like to try, but don't know how, is to disable the GLOBAL TIME feature and use instead the HARDWARE_CLOCK. Any recommendation on how to test this? I can't seem to find an option to pass to the roslaunch file.

Thanks in advance.

@MartyG-RealSense
Copy link
Collaborator

You should be able to disable global time in RealSense ROS by adding the command below to your roslaunch instruction.

global_time_enabled:=false

For example:

roslaunch realsense2_camera rs_camera.launch initial_reset:=True enable_sync:=True global_time_enabled:=false

@ramonvp
Copy link
Author

ramonvp commented May 31, 2021

I can't seem to find the argument 'global_time_enabled'. Even looking into the source code, there is no private parameter retrieved from the parameter server with a similar name or function. Is it possible that this argument/option was removed in a previous version?

@MartyG-RealSense
Copy link
Collaborator

MartyG-RealSense commented May 31, 2021

You should not need to look for this setting in the parameter server. It should just work if you include enable_global_time:=false in the roslaunch instruction.

To be clear, I am not referring to adding a rosparam to the inside of the launch file but instead adding it to the launch command.

If you would prefer to control the setting in ROS1 during runtime, the link below demonstrates its use in a dynamic reconfigure command.

#871

@ramonvp
Copy link
Author

ramonvp commented May 31, 2021

I tried passing the argument in the launch command as you suggested and it doesn't make any difference regarding the timestamp issue.
However, just to make myself clear, if you do:

git clone https://github.com/IntelRealSense/realsense-ros.git
cd realsense-ros
grep -r "enable_global_time"

The search returns empty. This argument is simply not used in current version, if I'm not mistaken.

@MartyG-RealSense
Copy link
Collaborator

MartyG-RealSense commented May 31, 2021

I am not aware of the status of enable_global_time having changed. My understanding from commentary in the past by Doronhi the RealSense ROS wrapper developer though is that disabling global time in ROS does not have as significant an effect on time drift as disabling it in librealsense does.

#796 (comment)

@doronhi
Copy link
Contributor

doronhi commented Jun 1, 2021

Most of the options are derived directly from librealsense2 so they have no specific reference in the realsense2_camera code.
An example for setting global timestamp on can be found here.
Notice that since you use a D435 and not L515 as in the example, you should replace "l500_depth_sensor" with "stereo_module" in the parameter name. You can see the available parameters using rqt_reconfigure. Also, notice that in the specified example the parameters are turned On so you should change that as well.

Disabling the global time in ROS disables it in the librealsense library. What is left in ROS is just adding a constant to every frame to match the gap between the system time and the hardware time, The constant is set once, according to the time difference of the first frame arriving. Consequently, any time difference that will develop is caused by a different rate, if there is one, between the system clock and the device's inner clock.

Global time tries to compensate for the difference between the system's progression rate and the device's. Although I can't see how it may cause the issue above, if disabling it makes a difference it could, perhaps, point us to a bug or to the origin of the problem.

@ramonvp
Copy link
Author

ramonvp commented Jun 2, 2021

Hi @doronhi, thanks for your comments. I tried your suggestion but apparently it did not disable the global_time feature. However, after digging a little bit in the source code, I applied this simple change:

diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
index b654fc3..33b1480 100644
--- a/realsense2_camera/src/base_realsense_node.cpp
+++ b/realsense2_camera/src/base_realsense_node.cpp
@@ -1845,6 +1845,8 @@ void BaseRealSenseNode::setupStreams()
             rs2::sensor sensor = active_sensors[module_name];
             sensor.open(sensor_profile.second);
             sensor.start(_sensors_callback[module_name]);
+            sensor.set_option(RS2_OPTION_GLOBAL_TIME_ENABLED, 0);
+            std::cout << "*** DISABLING GLOBAL_TIME FOR SENSOR " << module_name << std::endl;
             if (sensor.is<rs2::depth_sensor>())
             {
                 _depth_scale_meters = sensor.as<rs2::depth_sensor>().get_depth_scale();

And now I can see the confirming output trace:

*** DISABLING GLOBAL_TIME FOR SENSOR RGB Camera
*** DISABLING GLOBA_TIME FOR SENSOR Stereo Module
[ INFO] [1622666558.159676355]: SELECTED BASE:Depth, 0
[ INFO] [1622666558.179594225]: RealSense Node Is Up!
[ WARN] [1622666558.348953144]: 
[ WARN] [1622666558.349092563]: frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically.

After this change, if I run my test node again, now I get:

Time diff is : 0.012283563
Time diff is : 0.012139558
Time diff is : 0.012227773
Time diff is : 0.012068748
Time diff is : 0.011971235
Time diff is : 0.012006043
Time diff is : 0.012057543
Time diff is : 0.012129068
Time diff is : 0.012917041
Time diff is : 0.011156797
Time diff is : 0.011120319
Time diff is : 0.010409832
Time diff is : 0.011039018

So yeah!! Now this looks better! However, I left the node running for a while and after 1 hour only, it had already accumulated a drift of aprox 300ms, which I guess that was the reason for implementing the "GLOBAL_TIME" feature in the first place! 🤣

So, is this behaviour hiding a bug? My guess is that there must be something during the initialization that fails in my platform when global_time is enabled, but works fine in x86. How can I help to discover this? I really need to make this work.

Thanks again!

@doronhi
Copy link
Contributor

doronhi commented Jun 3, 2021

I could not reproduce using USB2.1 connection but then again, I don't have a Tinkerboard S with me.
Trying to see if it is indeed a librealsense2 issue, could you also see this using realsense-viewer: while displaying "Frame metadata" do you see the "Frame Timestamp" is significantly different than "Time of Arrival"?
image

@ramonvp
Copy link
Author

ramonvp commented Jun 3, 2021

Yes, you are right, frame timestamp and time of arrival are significantly off: 1622729172393.4 vs 1622756096241. What does it mean?
IMG_8357

@doronhi
Copy link
Contributor

doronhi commented Jun 7, 2021

We can determine that the origin of the issue is in the librealsense2 library but I don't know about the cause yet.
You can try to test your idea about the issue in the initialization by turning the "Controls"->"Global Time Enabled" flag on and off and see if it makes a difference. If it does we can mimic that on the ROS as a workaround.

@MartyG-RealSense
Copy link
Collaborator

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

@ramonvp
Copy link
Author

ramonvp commented Jun 14, 2021

Hi @MartyG-RealSense , sorry for the delay. I was last week out of the office and could not perform the test @doronhi suggested. Will report back probably tomorrow with the results.

1 similar comment
@ramonvp
Copy link
Author

ramonvp commented Jun 14, 2021

Hi @MartyG-RealSense , sorry for the delay. I was last week out of the office and could not perform the test @doronhi suggested. Will report back probably tomorrow with the results.

@MartyG-RealSense
Copy link
Collaborator

No problem @ramonvp - thanks very much for the update. I look forward to your test results. Good luck!

@ramonvp
Copy link
Author

ramonvp commented Jun 17, 2021

Hi again @doronhi , I tried toggling the "Global Time Enabled" and when I enabled it back, the huge difference in timestamps still persists. I repeated several times just to make sure the effect is repeatable.
There is however a strange visible behaviour: when I enable the Global Time option back, the first frame timestamp that appear is jumping (like adapting) to the new configuration. Afterwards, you can see that, although in general the time value is increasing, sometimes it jumps back and forth. Might this be a clock sync problem at a hardware level in the USB driver?

@doronhi
Copy link
Contributor

doronhi commented Jun 21, 2021

I don't have an idea right now where the issue might stem from.
This is the code piece that handles the global timestamp.
It seems that something with your environment inserts some irregular readings into the algorithm but I can't think right now of a way to reproduce it so I could investigate some more.

@RealSenseSupport
Copy link
Collaborator

@ramonvp
you can find the definition of different time stamps here
https://github.com/IntelRealSense/librealsense/blob/d5e775b75f7adbf75c911b7c85a4afd2f16defcb/include/librealsense2/hpp/rs_frame.hpp#:~:text=SENSOR_TIMESTAMP%20%20-%20Device%20clock,and%20host%20capabilities%3A

    *SENSOR_TIMESTAMP  - Device clock. For video sensors designates the middle of exposure. Requires metadata support.
    * FRAME_TIMESTAMP   - Device clock. Stamped at the beginning of frame readout and transfer. Requires metadata support.
    * BACKEND_TIMESTAMP - Host (EPOCH) clock in Kernel space. Frame transfer from USB Controller to the USB Driver.
    * TIME_OF_ARRIVAL   - Host (EPOCH) clock in User space. Frame transfer from the USB Driver to Librealsense.
    *Global Timestamp`  Host-corrected derivative of `HW Timestamp` required for multi-sensor/device synchronization

When you see big off between frame time stamp and arrival time stamp
it means the system take time to pass the frame data from usb host to the SDK
you could lower the fps or just streaming depth to see if this is computing power related
as Doronhi mentioned that it seems that something different on your platform when handle the global timestamp calculation
which use system EPOCH timer

and enable global time stamp is mainly for multicam usage that use time stamp to sync between different camera
but for multicamera sync, you can use hardware sync as well

https://dev.intelrealsense.com/docs/external-synchronization-of-intel-realsense-depth-cameras

@MartyG-RealSense
Copy link
Collaborator

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

@MartyG-RealSense
Copy link
Collaborator

Case closed due to no further comments received.

@doronhi
Copy link
Contributor

doronhi commented Jul 20, 2021

Hi @ramonvp ,
Sorry, it's taken so long to address. I now try to reproduce the issue on a NanoPi NEO4, hopefully it shares the same property as the Tinker Board S that creates that issue.
You mentioned that you built librealsense2 from code. Do you recall the cmake command you used? What flags exactly?
Thanks.

@ramonvp
Copy link
Author

ramonvp commented Jul 20, 2021

Hi @doronhi, thanks for taking a deeper look into it.
I can't remember from the top of my head the exact commands I used (I can find that information for you later). However, I also tried with the package available for ROS Noetic and it exhibited the same behaviour, if that helps.

sudo apt install ros-noetic-librealsense2-camera ros-noetic-librealsense2-description

@ramonvp
Copy link
Author

ramonvp commented Jul 20, 2021

Ok, I found my notes, here's a quick guide:

  1. Prepare dependencies

sudo apt install git libxrandr-dev libxinerama-dev libxcursor-dev libusb-1.0-0-dev libglfw3-dev ros-noetic-depth-image-proc

  1. Clone repository
cd
git clone https://github.com/IntelRealSense/librealsense.git
  1. Set default OpenGL version

We need to tell CMake to use the latest OpenGL library. By default, it will detect 2 versions, legacy and new.

cd librealsense
nano CMake/opengl_config.cmake

Add the following lines to the beginning of the file:

if(POLICY CMP0072)
  cmake_policy(SET CMP0072 NEW)
endif()
  1. Prepare environment and build SDK
mkdir build
cd build
cmake ../ -DBUILD_EXAMPLES=true -DBUILD_GRAPHICAL_EXAMPLES=false
make
sudo make install

@MartyG-RealSense
Copy link
Collaborator

Hi @doronhi Can you check out the information provided by @ramonvp in response to your question about the CMake command and flags that they used, please? Thanks!

#1906 (comment)

@doronhi
Copy link
Contributor

doronhi commented Jul 29, 2021

Yes, thank you very much for the detailed description. I also try to reproduce the issue using rs-data-collect.
Unfortunately, I will only be available for this next week.

@doronhi
Copy link
Contributor

doronhi commented Aug 2, 2021

I think I managed to reproduce on a NEO4 NanoPi machine. I need to be sure as this is indeed very weird and I'll keep you posted.

@ramonvp
Copy link
Author

ramonvp commented Aug 3, 2021

That's great to hear @doronhi! Just let me know if there's anything I can do to help.

@doronhi
Copy link
Contributor

doronhi commented Aug 5, 2021

There is a temporary "fix" that prevents raspberry PI from using the global timestamp. See here
The remark says "global timestamp seems to compromise RPi stability" but I have not seen anything like that so far and in any case, this fix just causes global timestamp to work wrong instead of totally disabling it, if that was the intention. @dorodnic

I still need time to investigate but I would like you to replace that line "#ifdef RASPBERRY_PI" with "#ifdef false", build and test again with global time stamp on. It seems like the correct solution to your issue but I don't know what to make of that remark so I'll appreciate your input.

Other than that, it seems that the wrapper should have the option to use librealsense's "system_time domain" directly if one can't use "global time domain". This is not a 1-line change so I'll need some time to test it. I'll try to prioritize it as well.

@ramonvp
Copy link
Author

ramonvp commented Aug 8, 2021

Hi again @doronhi, I have good news for you: your suggestion works! Running now the realsense-viewer with global time stamp enabled shows a match between the time of arrival and the frame timestamp. I am wondering why the code is being compiled with the -DRASPBERRY_PI in my platform, somehow it must have picked it up from somewhere. So, for future builds, should I remember to comment out these lines or do you think there will be a patch/commit in the near future?

Running the script presented in my original post, I am getting now an time diff of around 100ms, which I still think is too much, but probably usable. This can now be really due to a hardware performance issue. The nodelet manager launched in package realsense2_camera is taking, according to top, a huge 56% of my CPU.

This week I will try again to integrate with move_base (Navigation Stack in ROS1) and see how the real robot performs. If you are interested, I can report back with the results.

Thanks for your time, I really appreciate your effort!

@doronhi
Copy link
Contributor

doronhi commented Aug 9, 2021

Hi @ramonvp ,
I am very glad to hear it works.
The -D flag was most likely picked because of the definition in unix_config.cmake
The instability reported when the patch was inserted was most likely related to an old kernel 4 of the raspberry PI while using RSUSB backend.
I'll try to reproduce but if I'll fail I'll issue a PR to remove that patch and report back here.

As for the 100ms difference, I didn't try it on the rockchip. If you believe the source of the issue is realsense (either device, librealsense of the realsense2_camera wrapper) please open a new issue for it.
Regarding the CPU usage, bear in mind that both pointcloud and align_image filters use a lot of CPU. Again, if you can't find a satisfactory explanation in previous issues I do urge you to open a new issue for it.

@doronhi
Copy link
Contributor

doronhi commented Aug 10, 2021

The fix missed the upcoming librealsense2 version 2.49.0 and will be available in the next version - 2.50.0

@MartyG-RealSense
Copy link
Collaborator

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

@ramonvp
Copy link
Author

ramonvp commented Aug 17, 2021

Hello @MartyG-RealSense , the issue has been identified and resolved in the librealsense repo. Please see @doronhi 's PR here: IntelRealSense/librealsense#9595.
You may close this thread. Thanks.

@MartyG-RealSense
Copy link
Collaborator

Thank you very much @ramonvp

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

No branches or pull requests

4 participants