Zed2i sensor data (imu, mag, etc) latency?

thandal commented 2 months ago

Preliminary Checks

[X] This issue is not a duplicate. Before opening a new issue, please search existing issues.
[X] This issue is not a question, feature request, or anything other than a bug report directly related to this project.

Description

In messages published by the zed-ros2-wrapper, zed imu message header timestamps appear to have almost 0.8 s latency. From my observations, it seems like they are being incorrectly "backdated" somehow.

Steps to Reproduce

Run the ros2 zed wrapper
Run something like ros2 topic echo -i --field header --once /zed/zed_node/imu/data
Compare the received/logged timestamp with the header timestamp (Or log messages in a bag file and do the same comparison later)

Expected Result

Shouldn't be more than ~1ms latency for zed imu (or pose, odom, etc).

Actual Result

Using the ros2 zed wrapper, I've been logging zed data in ros bag files. There are two obvious timestamps to look at:

The message header timestamp (this is usually set by the application)
The time-of-logging timestamp

When I look at the difference (2) - (1) for most messages, I see what you might expect: <1 ms for messages that are a simple pass-through, ~100 ms for a 10 Hz laser scan that is timestamped at the start of the scan.

But I see some very strange numbers for the zed messages: For the zed odom and pose messages, there is 300 ms latency. And for the zed imu message, there is 790 ms latency! This means that when a zed imu message is logged, it claims that its data comes from ~800 ms in the past!

I looked at the wrapper code, and while the handling of timestamps is a little complex, they seem to be properly plumbed through from the proprietary function mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT);

NOTE: I originally found this because when I plotted signals like the yaw rate, it seemed like the zed was somehow anticipating the other sensors by almost 1 s! So I'm pretty sure that the zed_imu timestamps are incorrectly "backdated", rather than my other sensors all being incorrectly delayed.

The figure shows some interesting zig-zags... but they're small compared to the overall effect:

ZED Camera model

ZED2i

Environment

Ubuntu 22.04
Ros2 Iron
ZED_SDK_Ubuntu22_cuda12.1_v4.0.8
zed-ros2-wrapper from github
Zed2i

Anything else?

No response

thandal commented 2 months ago

I did some more testing:

double-checked that I had a vanilla checkout of the code
launched using the default ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i to make sure it wasn't something about my config files

Now I'm seeing evidence that the timestamp is drifting! The latency actually started out negative (log timestamp was before the header timestamp), but appears to be slowly drifting up:

thandal commented 2 months ago

Ok, I might have figured something out. I've been going a little crazy because sometimes the latency seems good, sometimes its bad (like 1.1s), and I couldn't figure out why.

Then while I was looking through some recent logs, I saw the timestamps go from good (~0.01s) to bad (~1.1s)! I looked around for other signals, and it seems like it might be the first time that GNSS data is integrated, the timestamps suddenly jump. This would explain why some of my log files are good and some are bad: I've been turning gnss_fusion on and off while trying to debug the heading issue.

I'm going do some more experiments with gnss_fusion turned off, in the hopes that I can get consistently reasonable timestamps.

And of course, if it is a problem with gnss_fusion, it would be great if this issue were fixed in 4.1!

thandal commented 1 month ago

... and the experiments were a failure. After ~20 seconds, the timestamp often but not always goes bad. I wrote a simple rospy node to display the timestamp delta, but wasn't able to figure out any set of parameters that prevented it. So I've given up and just overridden the bad sensor data timestamps in the wrapper.

Here are some logs that show the transition from "good" to "bad" timestamp deltas (key moment is bolded):

from zed_timestamp_node.py: ... timestamp delta @ 1710791198.655 0.003 timestamp delta @ 1710791198.665 0.002 timestamp delta @ 1710791198.675 0.000 timestamp delta @ 1710791198.685 -1.432 timestamp delta @ 1710791198.695 -1.432 timestamp delta @ 1710791198.705 -1.430 ...

from zed_wrapper: ... [zed_wrapper-6] [DEBUG] [1710791198.654219260] [zed.zed_node]: IMU MEAN freq: 99.3342 [zed_wrapper-6] [DEBUG] [1710791198.654223679] [zed.zed_node]: Barometer freq: 24.7142 [zed_wrapper-6] [DEBUG] [1710791198.654262961] [zed.zed_node]: Publishing IMU message [zed_wrapper-6] [DEBUG] [1710791198.664214820] [zed.zed_node]: Sensors callback: counting subscribers [zed_wrapper-6] [DEBUG] [1710791198.664284504] [zed.zed_node]: SENSOR LAST PERIOD: 0.0110271 sec @90.6857 Hz [zed_wrapper-6] [DEBUG] [1710791198.664290943] [zed.zed_node]: IMU MEAN freq: 99.3299 [zed_wrapper-6] [DEBUG] [1710791198.664295286] [zed.zed_node]: Magnetometer freq: 49.4333 [zed_wrapper-6] [DEBUG] [1710791198.664335418] [zed.zed_node]: Publishing IMU message [zed_wrapper-6] [DEBUG] [1710791198.664368770] [zed.zed_node]: Publishing MAG RAW message [zed_wrapper-6] [DEBUG] [1710791198.674386256] [zed.zed_node]: Sensors callback: counting subscribers [zed_wrapper-6] [DEBUG] [1710791198.674470931] [zed.zed_node]: SENSOR LAST PERIOD: 0.0120158 sec @83.2236 Hz [zed_wrapper-6] [DEBUG] [1710791198.674478528] [zed.zed_node]: IMU MEAN freq: 99.3249 [zed_wrapper-6] [DEBUG] [1710791198.674518878] [zed.zed_node]: Publishing IMU message [zed_wrapper-6] [DEBUG] [1710791198.684453665] [zed.zed_node]: Sensors callback: counting subscribers [zed_wrapper-6] [DEBUG] [1710791198.684566427] [zed.zed_node]: SENSOR LAST PERIOD: 1.44241 sec @0.693282 Hz [zed_wrapper-6] [DEBUG] [1710791198.684574092] [zed.zed_node]: IMU MEAN freq: 99.3253 [zed_wrapper-6] [DEBUG] [1710791198.684579118] [zed.zed_node]: Magnetometer freq: 49.4289 [zed_wrapper-6] [DEBUG] [1710791198.684643404] [zed.zed_node]: Publishing IMU message [zed_wrapper-6] [DEBUG] [1710791198.684673777] [zed.zed_node]: Publishing MAG RAW message [zed_wrapper-6] [DEBUG] [1710791198.694510002] [zed.zed_node]: Sensors callback: counting subscribers [zed_wrapper-6] [DEBUG] [1710791198.694639548] [zed.zed_node]: SENSOR LAST PERIOD: 0.0101099 sec @98.9129 Hz [zed_wrapper-6] [DEBUG] [1710791198.694647115] [zed.zed_node]: IMU MEAN freq: 99.3226 [zed_wrapper-6] [DEBUG] [1710791198.694651999] [zed.zed_node]: Barometer freq: 24.7139 [zed_wrapper-6] [DEBUG] [1710791198.694709790] [zed.zed_node]: Publishing IMU message [zed_wrapper-6] [DEBUG] [1710791198.695702924] [zed.zed_node]: Depth extraction enabled [zed_wrapper-6] [DEBUG] [1710791198.704563113] [zed.zed_node]: Sensors callback: counting subscribers

Attached is the code for zed_timestamp_node.py zed_timestamp_node.py.zip

Myzhar commented 1 month ago

Hi @thandal thank you for the exhaustive feedback. I also saw your message on the forum. Have you tried to do a native C++ or Python application outside ROS where you retrieve the sensor data and check the timestamp to confirm that this is a ZED SDK issue and not something caused by the ROS2 Wrapper or by ROS 2?

thandal commented 1 month ago

Good question.

I tried adding imu_timestamp deltas to a couple of different zed sdk sample python programs (export_sensors.py, positional_tracking.py, and depth_sensing.py). In all cases, the timestamp deltas started out in the range of 0.002 seconds. I saw timestamp deltas sometimes go slightly negative (meaning the imu data is from the future), but only by small amounts, up to 1/100th of a second. Not great, but not really a big problem.

So I ran some more tests with the zed_ros2_wrapper, and I found something interesting: the bad timestamp delta shift (when it jumps to something in the range of +-1.4 seconds) seems to consistently happen almost exactly 30 seconds after I launch the zed_ros2_wrapper binary. After a quick look through the wrapper code, I can't find anything with that sort of interval, but maybe it is another clue.

JiaqiangZhang commented 1 month ago

Hi, @thandal , Thanks for finding this issue. Have you managed to find a solution for the latency issue with ros2? Or you decided to not use ROS2 wrapper to get sensor data?

thandal commented 1 month ago

I haven't found a solution. I was going to test the 4.1 release (as soon as the wrapper is upgraded, maybe this week) to see if the issue might have somehow been fixed.

For now, I just override the sensor timestamps to the current time, which is not great but close enough to let me make progress.

On Mon, Apr 8, 2024 at 10:25 AM jqzhang @.***> wrote:

Hi, @thandal https://github.com/thandal , Thanks for finding this issue. Have you managed to find a solution for the latency issue with ros2? Or you decided to not use ROS2 wrapper to get sensor data?

— Reply to this email directly, view it on GitHub https://github.com/stereolabs/zed-ros2-wrapper/issues/206#issuecomment-2042896710, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAT6ESHTCTBY6MJHARHBWPLY4KSG3AVCNFSM6AAAAABDXD4732VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDANBSHA4TMNZRGA . You are receiving this because you were mentioned.Message ID: @.***>

JiaqiangZhang commented 1 month ago

Hi, @Myzhar May I ask if this issue will be fixed in the 4.1 release? Thanks

Myzhar commented 1 month ago

The SDK team performed tests to try to replicate this problem, but they could not. The Wrapper will be updated to be compatible with the ZED SDK v4.1 in the next few days, I recommend testing it with the new SDK as soon as it is ready.

JiaqiangZhang commented 1 month ago

Hi, @thandal and @Myzhar I run the wrapper and the codezed_timestamp_node.py.zip. I kept them running for more than 1min, following are the last several outputs. The results seems fine on my computer:

#zed wrapper log
[zed_wrapper-2] [DEBUG] [1712654501.921741145] [zed.zed_node]: Sensors callback: counting subscribers
[zed_wrapper-2] [DEBUG] [1712654501.921836478] [zed.zed_node]: SENSOR LAST PERIOD: 0.00494981 sec @202.028 Hz
[zed_wrapper-2] [DEBUG] [1712654501.921880060] [zed.zed_node]: IMU MEAN freq: 196.909
[zed_wrapper-2] [DEBUG] [1712654501.921943266] [zed.zed_node]: Publishing IMU message
[zed_wrapper-2] [DEBUG] [1712654501.926822475] [zed.zed_node]: Sensors callback: counting subscribers
[zed_wrapper-2] [DEBUG] [1712654501.926959155] [zed.zed_node]: SENSOR LAST PERIOD: 0.00495005 sec @202.018 Hz
[zed_wrapper-2] [DEBUG] [1712654501.926998755] [zed.zed_node]: IMU MEAN freq: 196.906
[zed_wrapper-2] [DEBUG] [1712654501.927060774] [zed.zed_node]: Publishing IMU message
[zed_wrapper-2] [DEBUG] [1712654501.928981769] [zed.zed_node]: Depth extraction enabled
[zed_wrapper-2] [DEBUG] [1712654501.931912046] [zed.zed_node]: Sensors callback: counting subscribers
[zed_wrapper-2] [DEBUG] [1712654501.932033850] [zed.zed_node]: SENSOR LAST PERIOD: 0.00495005 sec @202.018 Hz
[zed_wrapper-2] [DEBUG] [1712654501.932076663] [zed.zed_node]: IMU MEAN freq: 196.902
[zed_wrapper-2] [DEBUG] [1712654501.932143012] [zed.zed_node]: Publishing IMU message
[zed_wrapper-2] [DEBUG] [1712654501.936936874] [zed.zed_node]: Sensors callback: counting subscribers
[zed_wrapper-2] [DEBUG] [1712654501.936977243] [zed.zed_node]: SENSOR LAST PERIOD: 0.00495005 sec @202.018 Hz
[zed_wrapper-2] [DEBUG] [1712654501.936993446] [zed.zed_node]: IMU MEAN freq: 196.936
[zed_wrapper-2] [DEBUG] [1712654501.937008043] [zed.zed_node]: Magnetometer freq: 50.5342
[zed_wrapper-2] [DEBUG] [1712654501.937036608] [zed.zed_node]: Publishing IMU message
[zed_wrapper-2] [DEBUG] [1712654501.942009264] [zed.zed_node]: Sensors callback: counting subscribers
[zed_wrapper-2] [DEBUG] [1712654501.942074427] [zed.zed_node]: SENSOR LAST PERIOD: 0.00495005 sec @202.018 Hz
[zed_wrapper-2] [DEBUG] [1712654501.942100059] [zed.zed_node]: IMU MEAN freq: 196.946
[zed_wrapper-2] [DEBUG] [1712654501.942121989] [zed.zed_node]: Barometer freq: 24.9699
[zed_wrapper-2] [DEBUG] [1712654501.942165989] [zed.zed_node]: Publishing IMU message
[zed_wrapper-2] [DEBUG] [1712654501.947097788] [zed.zed_node]: Sensors callback: counting subscribers
[zed_wrapper-2] [DEBUG] [1712654501.947223713] [zed.zed_node]: SENSOR LAST PERIOD: 0.00495005 sec @202.018 Hz
[zed_wrapper-2] [DEBUG] [1712654501.947266945] [zed.zed_node]: IMU MEAN freq: 196.945
[zed_wrapper-2] [DEBUG] [1712654501.947335739] [zed.zed_node]: Publishing IMU message
[zed_wrapper-2] [DEBUG] [1712654501.952182890] [zed.zed_node]: Sensors callback: counting subscribers
[zed_wrapper-2] [DEBUG] [1712654501.952301411] [zed.zed_node]: SENSOR LAST PERIOD: 0.00494981 sec @202.028 Hz
[zed_wrapper-2] [DEBUG] [1712654501.952338078] [zed.zed_node]: IMU MEAN freq: 196.915
[zed_wrapper-2] [DEBUG] [1712654501.952408827] [zed.zed_node]: Publishing IMU message
[zed_wrapper-2] [DEBUG] [1712654501.957260518] [zed.zed_node]: Sensors callback: counting subscribers
[zed_wrapper-2] [DEBUG] [1712654501.957372823] [zed.zed_node]: SENSOR LAST PERIOD: 0.00742483 sec @134.683 Hz
[zed_wrapper-2] [DEBUG] [1712654501.957411795] [zed.zed_node]: IMU MEAN freq: 196.918
[zed_wrapper-2] [DEBUG] [1712654501.957443294] [zed.zed_node]: Magnetometer freq: 50.5318
[zed_wrapper-2] [DEBUG] [1712654501.957509015] [zed.zed_node]: Publishing IMU message

# timestamp code
timestamp delta @ 1712654501.922 0.005
timestamp delta @ 1712654501.928 0.005
timestamp delta @ 1712654501.933 0.005
timestamp delta @ 1712654501.937 0.005
timestamp delta @ 1712654501.942 0.005
timestamp delta @ 1712654501.948 0.006
timestamp delta @ 1712654501.953 0.006
timestamp delta @ 1712654501.958 0.003

My envi is zed2i, ubuntu 20.04 with foxy; the sdk and wrapper is 4.0.8.

github-actions[bot] commented 1 week ago

This issue is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days

stereolabs / zed-ros2-wrapper