Buffered packet processing

ros-drivers / ros2_ouster_drivers

ROS2 Drivers for the Ouster OS-0, OS-1, and OS-2 Lidars

https://ouster.com/

Apache License 2.0

134 stars 79 forks source link

Buffered packet processing #97

Closed Qgel closed 2 years ago

Qgel commented 2 years ago

Current blocking processing in OusterDriver::processData() causes packets to be lost if the processing of lidar data takes too much time, see Issue #96.

This PR splits the processing of lidar data into two seperate threads. Previously, the code receiving sensor data from the wire was blocked by the lidar or imu processors, causing dataloss on slower systems.

Modifies sensor interface so receiving functions get passed a pointer to a buffer to write their data to
Implements ringbuffers for lidar and imu data in ouster_driver
Splits processing into two seperate threads:
- receiveData receives from the wire and fills ringbuffers
- processData takes packets from the queue, assembles the lidar scan and runs the processors
Threads are implemented as pure threads instead of ros timers, as timing inconsistencies for the receiver thread were also leading to data loss

SteveMacenski commented 2 years ago

Did you also try compiling with release flags and still see that it couldn't keep up? That seems odd.

Qgel commented 2 years ago

Yes, I was testing in Release or RelWithDebInfo mode the whole time.

It's not that the processing can't keep up, otherwise, this PR wouldn't help. We would just slowly fill up our buffer until we had to drop some frames. Rather, the blocking during processing (for me 30 ms with PCL|IMG|IMU) in processData() caused packets from the wire to be lost.

bertaveira commented 2 years ago

I have tested this with a OS1-64 and it completely solved the flickering. To stress test it I made the entire computer struggle to see if this would solve even in the worst case scenarios by running at the same time stress --cpu 20 --io 4 --vm 4 and there are no section drops at all while the ros2 branch drops like crazy when under stress.

SteveMacenski commented 2 years ago

Thanks for testing, that's good to know!

Qgel commented 2 years ago

I'll be gone for the next few days, so I've just pushed the commit that guards against the race in the head indices. The branch should now be in a good state from my side in case you want to merge while I'm away.

bertaveira commented 2 years ago

Any updates on this merge? Is there any changes still waiting?

bertaveira commented 2 years ago

@Qgel Any updates on the last changes?

SteveMacenski commented 2 years ago

Agreed, it was pretty much small stuff left to be merged!

Qgel commented 2 years ago

Whoops, thought I had already done this. Hope I caught everything.