search

PRBonn / kinematic-icp

A LiDAR odometry pipeline for wheeled mobile robots
https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/kissteam2025icra.pdf
MIT License
197 stars 22 forks source link

Fix for at least two types of timestamps in PointCloud2 messages #13

Closed tizianoGuadagnino closed 3 weeks ago

tizianoGuadagnino commented 1 month ago

This is mainly to tackle the issue reported in #12 . In essence, some drivers (in this case the Livox driver) express the timestamps in nanoseconds, while other vendors (e.g. Ouster if my mind doesn't fail me) report the stamp directly in seconds. To handle the scenario I decided to count the digits of the timestamps, as we know that ROS stores the seconds and nanoseconds in int32. As this type cannot handle more than 10 digits, any stamp with more than 10 digits in the integer part of the floating point is assumed to be expressed in nanoseconds and converted.

It will need some testing to be sure but on the preliminary analysis, I don't see any difference in results for our data. @KenN7 can you test it on your stuff? or if you can send me the data ;)

KenN7 commented 3 weeks ago

Hi there, I cannot send you my data unfortunately, but i can tell you, that from my initial tests, it seems to work fine :)

KenN7 commented 3 weeks ago

I was only wondering whether it is normal that the method outputs less points on rviz when the computation is faster ? (in debug vs release)

tizianoGuadagnino commented 3 weeks ago

I was only wondering whether it is normal that the method outputs less points on rviz when the computation is faster ? (in debug vs release)

I would guess that is a rendering issue from RVIZ, if you check the size of the published PointCloud2 msg it doesn't seems to change

tizianoGuadagnino commented 3 weeks ago

@benemer from my side we can merge this, I will wait for your trigger

benemer commented 3 weeks ago

What do you think about this solution? It is a bit lighter and still uses the fact that ROS represents seconds in int32. This means that if your timestamp is larger than this, it must be measured in nanoseconds (assuming it's either seconds or nanoseconds).

In both solutions, this conversion fails if the message contains a timestamp in nanoseconds smaller than the max value of an int32, which is 2.147483647 seconds. This should not be very important since it only occurs if you use nanoseconds and relative timestamps starting from 0. But in that case, the way we infer the timestamps of the beginning and end of the scan will also fail because this assumes absolute timestamps for the points.

This conversion is not easy because the message timestamp could also contain milliseconds (for whatever reason). However, with this, we at least support the most common scenarios.

tizianoGuadagnino commented 3 weeks ago

Any solution we bring will be ugly and not elegant in some way. I will stick to what was here to close this after 2 weeks because this was already tested in both scenarios. Any breaking point in this part is going to be my fault. Hopefully, someone else come up with more issues like this so that we can further robustify this part. Thanks @KenN7 for now ;)