Default implementation of _retrieve_colored_point_cloud()

[Victorlouisdg]

Describe your changes

• Added default implementation of _retrieve_colored_point_cloud() (untested). This will make the get_colored_point_cloud() method available for Realsense camera's and the MultiprocessRGBDRecevier.
• Added more logging in multiprocess code

Checklist

• Have you modified the changelog?
• If you made changes to the hardware interfaces, have you tested them using the manual tests?

[Victorlouisdg]

Tested with Multiprocessing and with a Realsense camera. Seems to work as expected:

[Victorlouisdg]

Working with this I discovered two issues:

1. My point cloud filtering broke, it seems to filter out incorrect points. I believe this is because create_from_rgbd_image does not always perserve the "image-based" order of points in the point cloud. Applying the flattened confidence_map resulted in good points being filtered and bad points remaining.
2. When reading point clouds from shared memory, every ~5 seconds one of the point clouds is oriented weirdly/partially missing (different issue from Point cloud is sometimes incomplete near camera (ZED2i) #104). The reason seems to be that the shared memory is being read while it is being written with new data. I will add a read/write locking mechanism to see if this fixes it.

[Victorlouisdg]

Problem 2 described in my previous comment seems to be fixed by adding a read/write lock to the shared memory. I still need to investigate if my theory about problem 1 is correct. If it is that case that open3d sometimes reorders points, we should try to disable that.

[Victorlouisdg]

With the locks, I also implemented that all receivers make a their own copy of the data in shared memory. However, this is apparently very costly, and even Gorilla with it's 32 cores can't keep up with the max resolution data stream from a Zed2i at 15 fps.

This is a problem for me as I can't maintain 15 fps for the video recording.

[Victorlouisdg]

Preallocating "buffer" numpy arrays and copying with buffer_array[:] = shm_array[:] solves the issue. Apparently shm_array.copy() is at least 10x as costly. An important caveat of returning a buffer instead of a full copy is that at the next retrieve call, the buffer is overwritten.

[Victorlouisdg]

I'm still struggling to achieve 15 fps at 2K and neural depth mode:

2024-02-16 16:27:04.330 | WARNING  | __main__:<module>:449 - FPS:  12.18 /  15.00 (too slow)
2024-02-16 16:27:04.351 | INFO     | __mp_main__:run:135 - Time to grab images: 0.045 s
2024-02-16 16:27:04.351 | INFO     | __mp_main__:run:141 - Time to wait for lock: 0.000 s
2024-02-16 16:27:04.391 | INFO     | __mp_main__:run:154 - Time to write to shared memory: 0.040 s

With performance depth mode, it's just fast enough to run at ~15 fps:

2024-02-16 16:25:42.484 | DEBUG    | __main__:<module>:451 - FPS:  14.45 /  15.00
2024-02-16 16:25:42.487 | INFO     | __mp_main__:run:135 - Time to grab images: 0.027 s
2024-02-16 16:25:42.487 | INFO     | __mp_main__:run:141 - Time to wait for lock: 0.000 s
2024-02-16 16:25:42.524 | INFO     | __mp_main__:run:154 - Time to write to shared memory: 0.037 s

[Victorlouisdg]

The State of this PR
The initial focus was on implementing a default for the _retrieve_colored_point_cloud() method of StereoRGBDCamera. However, recalculating point clouds from depth maps and RGB images was much slower than anticipated (>200 ms). This led me to implement a MultiprocessStereoRGBDPublisher that passed the point clouds over shared memory. But that could also not handle all the data a ZED2i camera produces at 2K resolution and 15 fps. As a consequence, this PR evolved into a collection of much-needed improvements to the multiprocessing code. However, before I invest any more time into this code, I believe we need to re-evaluate how we use multiprocessing.

Strategic Re-evaluation
While I believe our shared memory data communication is "SOTA" in terms of performance and flexibility (e.g. data transfer speed is close to RAM bandwidth and we can allocate shared memory of whatever shape and size we want at runtime), we need to consider whether it's worth it. Maybe with a little investigation and some custom ROS2 message types, we can get almost the same thing with ROS2, with less error-prone shared memory management code to maintain. I've shared a more detailed analysis in #131