[WIP] Depth and IMU data processing

[nadimasmar]

This PR restructures the sensors package and adds processing for incoming IMU and depth data.

1. Added ImuRepublisher and DepthRepublisher processors and integrated them into a single sensor_node executable for easier state sharing (e.g., using the latest IMU orientation when processing depth).

2. Updated CMakeLists.txt to build/install sensor_node and removed obsolete executables/sources from the package.

3. Updated launch to start the Xsens driver only when sim:=false, sensor_node runs regardless of sim.

4. Updated package.xml to include Eigen dependencies and refreshed README.md with detailed IMU/depth processing documentation.

TODO:

• Add a DVL data processor.
• decide on a sensor fusion procedure or a simple state aggregator
• Test and iterate

[JPGC04]

Very nice progress in this pr. All this needs to be tested in real life to make sure data is actually being published in real life.

[JPGC04]

While this probably works and is supported, it is not the proper way to handle library files. Please use #ifndef for these instead for all you .hpp file.

[WilliamZhang2205]

I’d recommend to add a deconstructor (Albert drilled that into me), essentially it’s a clean up of memory usage that isn’t automatically cleaned when ctrl-c. As far as this node is concerned, it should be a one liner since I don’t think there’s memory usage not handled by the OS already. However, as good practice plus being consistent with the future in case one day it is useful I’d say to add it (ironic all the yap for one line command).

[WilliamZhang2205]

Also, I know it probably won’t affect things much but has possible performance impacts been considered for putting all the sensors handled by one node instead of split up ones? Again probably not much impact ahead but a thing to think about in case timing of subscribers and others stuff is off

[nadimasmar]

To answer the first comment, a default deconstructor is added in Sensor_Node.hpp on line 23. Not sure if I am properly adressing your comment. Let me know if I got something wrong.

For the second comment, the fact they are all member variable of one parent object makes information sharing between the processors easier. The depth processor can access the vehcile's orientation directly from the IMU processor (see line 60 in Sensor_Node.cpp). Otherwise, information would have to be sent over a topic. Making that parent object a Node instead of both processors being Nodes seemed cleaner to me. Like this, the processors are ROS-independant.

Let me know if there is anything else.

[WilliamZhang2205]

My bad I did not see that. I think it should be good.

While I get that idea of sharing information and convenience, I still worry about potential issues that can be brought up. Firstly, unless coded for, each ROS2 node seem to operate as a single thread and use only 1 core of the CPU. This can lead to having one sensor waiting on another sensor messing with the accuracy of the state when you end up doing ekf or sensor aggregation. More specifically, while the slower sensor processes its callback, the faster one's data cannot be read and starts building up in a queue. If the queue has no limit, this can lead to running out of memory causing a crash. If the queue has a limit, at one point, sensor data gets erased. Communicating over a topic, from what I can find online, takes about 1-10ms, which is like 1 IMU reading loss if it runs at 100Hz, so it should not be too bad. Up to you since you're the state estimation specialist to determine how bad this may be. Brief research into this (this kinda looks like a rabbit hole) shows that even explicitely writing a multithreadexecutor for ROS2 does not always solve the node being bottlenecked into a single core.

Secondly, from a failure point, if the depth sensor section of the node crashes it will take down the rest of the sensors with it. Since we made a big point on modularity of our code, I think we should stay consistent on that end. I also think that for beginners reading our code, having each sensor being its own node and thus its own file makes things more "digestible" for a new member.

Thirdly, I do not know how memory is managed for each node as a process. However, as a general rule of thumb, try not too bundle up too much into one node. If ROS2 actually operates like a proper operating system, it decides how much memory is given for each node, which if your node is too big it may overflow (unlikely but good to know). This node written does not seem that heavy but I think its cool and useful to know.

Finally, I also want to say I am no expert in all of this. Having just gotten an internship dealing with OS shenanigans, I am being introduced to all these OS tasks sharing and memory allocations so I think raising these questions to be pondered is valid. There are also other possible issues concerning data sharing, which I don't feel qualified enough to talk about. All that talk to say that I think doing it in one node introduces many error points that can be simply solved by writing different nodes.

[nadimasmar]

Thanks for the throrough comment, what you are saying makes esnse.
I'll try to split it up the processors when I find some free time.

[nadimasmar]

Refactored the package and split up the nodes

[WilliamZhang2205]

Minor stuff in comments I left. Great BOOMIN work overall. I agree with what the big boss JP man say, what he said