search

stereolabs / zed-ros2-wrapper

ROS 2 wrapper for the ZED SDK
https://www.stereolabs.com/docs/ros2/
Apache License 2.0
134 stars 139 forks source link

GNSS Fusion is invalid #199

Closed EnchengLiu closed 4 months ago

EnchengLiu commented 5 months ago

Preliminary Checks

Description

Hi, I was trying to use ZED2i to conduct a GNSS fusion with GPS. But after I check the message If found the mGeoPoseStatus is not OK in the component.cpp, so in the terminal It print "waiting for a valid GNSS fused pose..." all the time. I just want to know how can I fix it and make the rest part of processGeoPose run.

here is the terminal log log.txt

[zed_wrapper-2] [DEBUG] [1701738521.061117496] [zed2i.zed_node]: GNSS dT: 9836332 nsec [zed_wrapper-2] [DEBUG] [1701738521.061135450] [zed2i.zed_node]: Stored Ts: 1701738521060715687 [zed_wrapper-2] [DEBUG] [1701738521.070794234] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 70489128 nsec = 1701738521070489128 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.070846813] [zed2i.zed_node]: GNSS dT: 9773441 nsec [zed_wrapper-2] [DEBUG] [1701738521.070866269] [zed2i.zed_node]: Stored Ts: 1701738521070489128 [zed_wrapper-2] [DEBUG] [1701738521.080857551] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 80609050 nsec = 1701738521080609050 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.080901239] [zed2i.zed_node]: GNSS dT: 10119922 nsec [zed_wrapper-2] [DEBUG] [1701738521.080909542] [zed2i.zed_node]: Stored Ts: 1701738521080609050 [zed_wrapper-2] [DEBUG] [1701738521.090540915] [zed2i.zed_node]: Fusion Positional Tracking started [zed_wrapper-2] [DEBUG] [1701738521.091626545] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 91428496 nsec = 1701738521091428496 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.091648430] [zed2i.zed_node]: GNSS dT: 10819446 nsec [zed_wrapper-2] [DEBUG] [1701738521.091656251] [zed2i.zed_node]: Stored Ts: 1701738521091428496 [zed_wrapper-2] [DEBUG] [1701738521.091668703] [zed2i.zed_node]: Datum ingested - [1701738521091428496 nsec] 37.9152°,-122.334° / 0 m [zed_wrapper-2] [2023-12-05 01:08:41 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking] differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 931471875 ms. [zed_wrapper-2] [DEBUG] [1701738521.101633677] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 101424927 nsec = 1701738521101424927 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.101664791] [zed2i.zed_node]: GNSS dT: 9996431 nsec [zed_wrapper-2] [DEBUG] [1701738521.101672547] [zed2i.zed_node]: Stored Ts: 1701738521101424927 [zed_wrapper-2] [DEBUG] [1701738521.101701824] [zed2i.zed_node]: Datum ingested - [1701738521101424927 nsec] 37.9152°,-122.334° / 0 m [zed_wrapper-2] [2023-12-05 01:08:41 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking] differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 931471885 ms. [zed_wrapper-2] [DEBUG] [1701738521.111426817] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 110964223 nsec = 1701738521110964223 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.111488318] [zed2i.zed_node]: GNSS dT: 9539296 nsec [zed_wrapper-2] [DEBUG] [1701738521.111504352] [zed2i.zed_node]: Stored Ts: 1701738521110964223 [zed_wrapper-2] [DEBUG] [1701738521.111527999] [zed2i.zed_node]: Datum ingested - [1701738521110964223 nsec] 37.9152°,-122.334° / 0 m [zed_wrapper-2] [2023-12-05 01:08:41 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking] differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 931471894 ms. [zed_wrapper-2] [DEBUG] [1701738521.121924740] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 121624749 nsec = 1701738521121624749 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.121965312] [zed2i.zed_node]: GNSS dT: 10660526 nsec [zed_wrapper-2] [DEBUG] [1701738521.121982786] [zed2i.zed_node]: Stored Ts: 1701738521121624749 [zed_wrapper-2] [DEBUG] [1701738521.122006079] [zed2i.zed_node]: Datum ingested - [1701738521121624749 nsec] 37.9152°,-122.334° / 0 m [zed_wrapper-2] [2023-12-05 01:08:41 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking] differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 931471905 ms. [zed_wrapper-2] [DEBUG] [1701738521.131039778] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 130767539 nsec = 1701738521130767539 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.131080110] [zed2i.zed_node]: GNSS dT: 9142790 nsec [zed_wrapper-2] [DEBUG] [1701738521.131088037] [zed2i.zed_node]: Stored Ts: 1701738521130767539 [zed_wrapper-2] [DEBUG] [1701738521.131102207] [zed2i.zed_node]: Datum ingested - [1701738521130767539 nsec] 37.9152°,-122.334° / 0 m [zed_wrapper-2] [2023-12-05 01:08:41 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking] differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 931471914 ms. [zed_wrapper-2] [DEBUG] [1701738521.141264293] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 141044082 nsec = 1701738521141044082 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.141302832] [zed2i.zed_node]: GNSS dT: 10276543 nsec [zed_wrapper-2] [DEBUG] [1701738521.141319363] [zed2i.zed_node]: Stored Ts: 1701738521141044082 [zed_wrapper-2] [DEBUG] [1701738521.141340743] [zed2i.zed_node]: Datum ingested - [1701738521141044082 nsec] 37.9152°,-122.334° / 0 m [zed_wrapper-2] [2023-12-05 01:08:41 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking] differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 931471925 ms. [zed_wrapper-2] [DEBUG] [1701738521.150955646] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 150735141 nsec = 1701738521150735141 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.150988173] [zed2i.zed_node]: GNSS dT: 9691059 nsec [zed_wrapper-2] [DEBUG] [1701738521.150996160] [zed2i.zed_node]: Stored Ts: 1701738521150735141 [zed_wrapper-2] [DEBUG] [1701738521.151008779] [zed2i.zed_node]: Datum ingested - [1701738521150735141 nsec] 37.9152°,-122.334° / 0 m [zed_wrapper-2] [2023-12-05 01:08:41 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking] differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 931471934 ms. [zed_wrapper-2] [ERROR] [1701738521.156384185] [zed2i.zed_node]: Fusion error: NO NEW DATA AVAILABLE [zed_wrapper-2] [DEBUG] [1701738521.156427865] [zed2i.zed_node]: GNSS synced frame ts: 1701738521038960776 nsec [zed_wrapper-2] [DEBUG] [1701738521.156450236] [zed2i.zed_node]: DeltaT: 0 sec [1701738473.003679752-1701738473.003679752 [zed_wrapper-2] [INFO] [1701738521.156575536] [zed2i.zed_node]: Odometry aligned to last tracking pose [zed_wrapper-2] [INFO] [1701738521.156591663] [zed2i.zed_node]: * Initial odometry [odom -> base_link] - {0.010,-0.060,-0.015} {0.000,-0.000,0.000} [zed_wrapper-2] [DEBUG] [1701738521.156623422] [zed2i.zed_node]: Getting static TF from 'gnss' to 'base_link' [zed_wrapper-2] [INFO] [1701738521.156658602] [zed2i.zed_node]: Static transform Sensor to Base [gnss -> base_link] [zed_wrapper-2] [INFO] [1701738521.156675833] [zed2i.zed_node]: * Translation: {0.000,0.000,0.000} [zed_wrapper-2] [INFO] [1701738521.156683072] [zed2i.zed_node]: * Rotation: {0.000,-0.000,0.000} [zed_wrapper-2] [DEBUG] [1701738521.156696432] [zed2i.zed_node]: Waiting for a valid GNSS fused pose... [zed_wrapper-2] [DEBUG] [1701738521.161146025] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 160688633 nsec = 1701738521160688633 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.161223620] [zed2i.zed_node]: GNSS dT: 9953492 nsec [zed_wrapper-2] [DEBUG] [1701738521.161241003] [zed2i.zed_node]: Stored Ts: 1701738521160688633 [zed_wrapper-2] [DEBUG] [1701738521.161274312] [zed2i.zed_node]: Datum ingested - [1701738521160688633 nsec] 37.9152°,-122.334° / 0 m [zed_wrapper-2] [2023-12-05 01:08:41 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking] differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 931471944 ms. [zed_wrapper-2] [DEBUG] [1701738521.170918273] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 170636871 nsec = 1701738521170636871 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.170982041] [zed2i.zed_node]: GNSS dT: 9948238 nsec [zed_wrapper-2] [DEBUG] [1701738521.171012947] [zed2i.zed_node]: Stored Ts: 1701738521170636871 [zed_wrapper-2] [DEBUG] [1701738521.171055457] [zed2i.zed_node]: Datum ingested - [1701738521170636871 nsec] 37.9152°,-122.334° / 0 m [zed_wrapper-2] [2023-12-05 01:08:41 UTC][ZED][WARNING] The timestamp of the data you are attempting to ingest for [GeoTracking] differs from the current synchronization timestamp. Please ensure you are using the correct timestamp units. Timestamp difference: 931471954 ms. [zed_wrapper-2] [DEBUG] [1701738521.181497954] [zed2i.zed_node]: GNSS Ts: 1701738521 sec + 181190710 nsec = 1701738521181190710 nsec fused [zed_wrapper-2] [DEBUG] [1701738521.181547187] [zed2i.zed_node]: GNSS dT: 10553839 nsec [zed_wrapper-2] [DEBUG] [1701738521.181557478] [zed2i.zed_node]: Stored Ts: 1701738521181190710 [zed_wrapper-2] [DEBUG] [1701738521.181600451] [zed2i.zed_node]: Datum ingested - [1701738521181190710 nsec] 37.9152°,-122.334° / 0 m

` In the attachment is the common.yaml and navsatfix topic message. Screenshot from 2023-12-04 18-21-34

As mGeoPoseStatus = mFusion.getGeoPose(mLastGeoPose); and mLastGeoPose is a reference, I can't find any initialization of mLastGeoPose in the wrapper, So I just guess maybe there should initialize that first before running? Just small try.

Steps to Reproduce

1.Modify common.yaml files 2.Plug in GPS and ZED2i 3.Run Zed2i.launch file

Expected Result

Data successfully fused

Actual Result

always Waiting for a valid GNSS fused pose... mGeoPoseStatus is not ok

ZED Camera model

ZED2i

Environment

Ubuntu 20.04
ZED SDK Version: 4.0.7

Anything else?

No response

github-actions[bot] commented 4 months 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