Support for Livox CustomMsg?

[ubicray]

Hey again! I'm working on implementing and testing this with my jetson based project. Any intention to add support for the custom msg from Livox?

https://github.com/Livox-SDK/livox_ros_driver2/blob/master/msg/CustomMsg.msg

If you have any pointers I can look into implementing support myself too, just might take a while

[fetty31]

Hello @ubicray,

Right now I don't think I will add any support for Livox custom msgs. This project has been implemented in order to be as agnostic as possible to the type of LiDAR being used. Thus, the input pointcloud topic should be of type sensor_msgs::PointCloud2 and the parameter sensor_type will define the logic behind extracting the timestamp of each point.

Having said that, if you would want to add some support for Livox msgs the best way would be to handle the type translation (from livox_ros_driver2/CustomMsg to fast_limo::PointType) in the ROS wrapper (main.cpp), leaving the fast_limo library unchanged.

This lines are the ones to be changed: https://github.com/fetty31/fast_LIMO/blob/bc84bfd19bb8d2614ec1380c94e330fa879a90da/src/main.cpp#L13-L19

Change the lidar_callback in order to receive a _livox_rosdriver2::CustomMsg and fill the pcl::PointCloud::Ptr object as expected. I'm realizing now that _livox_rosdriver2::CustomPoint has an offset time relative to the msg timestamp, so you could for example set the sensor_type param to 0 (OUSTER) and set each _fastlimo::PointType::t field equal to _livox_ros_driver2::CustomPoint::offsettime field (which I'm assuming is in nanoseconds).

[ubicray]

Thanks for assistance, @fetty31 Making these changes (and changes to the driver of the built-in IMU of mid360 so that it'd report in m/s2 instead of g) worked well! Previously you asked about the performance on Jetson, and I can report that with Orin NX, it uses around 1 core fully with mostly default settings.

1 more question... I'm using the scancontext branch for loop_closure and see that the added loop_closure subscriber to main.cpp is commented out. Should that be brought back in for odometry output of the main node to consider loop_closure output?

[fetty31]

Sounds nice!

If you are using the loop/scancontext branch please make sure to update it because I made some critical changes minutes ago. Yes, if you would want the iKFoM to take into account the back-end estimation (pose-graph) you could uncomment the subscriber you mention. However, I don't think it's a good idea taking into account that this LiDAR odometry is filter-based. This subscriber basically tells the fast_limo::Localizer to update the current state estimate to match with the pose-graph optimization output. If the difference between both states is little (which is normal in open-loop) it can work fine (although is not optimal), but when the error gets bigger (normally when the loop is closed) you could destabilize the iKFoM and at some point make it diverge if the filter is not capable of recovering.

If you could explain your use-case maybe I'll be able to offer some more help.

[ubicray]

I have a usual localization system with 2 ekfs from robot_localization running, with 1 providing georeferencing as the project is an outdoor robot. Would like to fuse the result of loop closure into the georeferencing ekf, but as it currently outputs only on closure it wouldn't be a good fit.

[fetty31]

The loop closure node doesn't only output on closure; it outputs every time the pose-graph is solved (which right now is after 3 new keyframes have been added to the graph). All this behaviour can be tuned to your needs modifying config/looper.yaml. The param PoseGraph/MinNumStates controls the min number of received states in order to solve the graph. In KeyFrames/Odom and KeyFrames/GPS you can also tune the distance and orientation norm thresholds that decide when to add a new keyframe to the graph (state or GPS landmark).

If you set all norms to 0 and the MinNumStates to 1 the fast_limo::Looper node will publish its state estimate at its highest frequency (which by default is set to 10Hz).

[woshinidiesb]

Thanks for assistance, @fetty31 Making these changes (and changes to the driver of the built-in IMU of mid360 so that it'd report in m/s2 instead of g) worked well! Previously you asked about the performance on Jetson, and I can report that with Orin NX, it uses around 1 core fully with mostly default settings.

1 more question... I'm using the scancontext branch for loop_closure and see that the added loop_closure subscriber to main.cpp is commented out. Should that be brought back in for odometry output of the main node to consider loop_closure output?

We share the same goal; could we share the modifications made to receive signals from the mid360? I am using the same solution as yours, with LIO for local pose estimation, and GPS as the reference coordinate system or the correct position when there is no occlusion. Perhaps we could discuss this together; I have currently purchased two mid360s and am trying to fuse the radar data and run fast_limo.