Gravity Aligned Map + ROS REP 105 changes

[nlitz88]

Hi there,

I added some changes to make FAST-LIO2 a bit more plug-and-play for ROS2 systems that conform to ROS REP 105. Namely:

1. Added gravity alignment changes from this repo, so that the roll and pitch of the sensor's first measurement is accounted for in the transform chain. This way, the resulting map will be gravity aligned, and the odometry outputs will be w.r.t. a gravity aligned local frame. Note: This change has nothing to do with ROS REP 105--more just a quality-of-life feature that you find in other estimators. VINS-Mono does this, to cite another example. All credit here goes to @SanghyunPark01
2. Updated the node to publish the odom_from_**base_link**" transform, instead of odom_from_lidar_link-- which is necessary for FAST-LIO to serve as a ROS REP 105 compliant local estimator. Without this, your lidar's link will end up having two parents:base_linkandodom` (assuming you have your URDF set up according to REP 105 and REP 103). Some helpful context for why this is important can be found in these instructions from Nav2.

I recognize that none of these changes are strictly relevant to FAST-LIO as an odometry system. However, I would argue that these changes are important/useful for this module to fit into / be usable in ROS projects / projects that follow some of these conventions.

I tested these changes on a personal dataset of mine, but may be able to provide further validation if desired.

[nlitz88]

Also, FYSA: I am just realizing that these changes may overlap a bit with #74 and #71 .

• W.r.t. 74: I hardcode odom in place of camera_init. If we're being super pedantic, odom is the "right" frame ID here, but I have certainly worked on systems where this frame takes on many different names, so I think parameterizing this could also be helpful.
• W.r.t. 71: I want to say the gravity alignment feature kind of resolves this? The gravity alignment feature basically applies an additional rotation from the initial sensor frame in order that it's z axis would be aligned with the gravitational acceleration measured by your accelerometers. This is helpful in situations where a.) your sensor is mounted on an incline / not-level with your robot's base frame base_link and, more importantly b.) when your robot does not start on a level surface. In both cases, the gravity alignment solves this problem.

[nlitz88]

Update: Made the local frame (typically odom) and robot body frame (typically base_link) configurable via the node's parameters. This should give us the same functionality introduced in #74 . Let me know if I missed anything 🙂

[nlitz88]

Okay, I made one last change: Instead of publishing local_frame_from_lidar_link to /odom (and the corresponding path), it now publishes local_frame_from_body_frame.

So, in the default case where local_frame_id is set to odom and body_frame_id is set to base_link, the node would publish an odometry message with the pose odom_from_base_link. Under the hood, fast_lio is still just estimating the local_frame_from_lidar_link pose/transform, and then we just compose that pose with the lidar_link_from_body_frame transform.

And, if you don't have extrinsics between your robot's body frame the lidar frame, you can just specify the lidar frame id as the body frame.

The motivation for adding this was ease of integration. Ideally, whatever downstream node (like a local planner or trajectory tracking controller) is consuming the /odom messages should be able to transform those into whatever body-fixed frame it needs. However, many folks don't want to worry about doing this, so having this node take care of that might make integrating it that much easier for simple uses cases.