adding lidar mount configuration

CMakeLists.txt

@@ -124,6 +124,11 @@ install(TARGETS fastlio_mapping
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(PROGRAMS
+  fast_lio_localization/odom_topic.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 install(
   DIRECTORY config launch rviz
   DESTINATION share/${PROJECT_NAME}

README.md

@@ -114,6 +114,21 @@ Note that, during the initialization stage, it's better to keep the robot still
 3. [FAST-LIO-SLAM](https://github.com/gisbi-kim/FAST_LIO_SLAM): The integration of FAST-LIO with [Scan-Context](https://github.com/irapkaist/scancontext) **loop closure** module.
 4. [LIO-SAM_based_relocalization](https://github.com/Gaochao-hit/LIO-SAM_based_relocalization): A simple system that can relocalize a robot on a built map based on LIO-SAM.
 
+## 5.Additional Features
+Added configuration to allow different lidar mounting orientation. Can be used if the lidar mounting is not parallel to the floor, such as in unitree g1 or other similar robots.
+Modify mid360.yaml:
+```
+use_odom_transform: true  # true: transform scan to odom frame before matching (must match mapping config) -> change this to false if your lidar is already mounted parallel to the floor
+odom_roll: 0.0        # Roll angle (degrees) from camera_init to odom
+odom_pitch: 0.0        # Pitch angle (degrees) from camera_init to odom
+odom_yaw: 0.0           # Yaw angle (degrees) from camera_init to odom
+```
+
+Transformation tree will become:
+```
+odom -> camera_init -> body -> base_link (where base_link is the frame parallel to the floor)
+```
+
 
 ## Acknowledgments
 Thanks for the authors of [FAST-LIO](https://github.com/hku-mars/FAST_LIO) and [LIO-SAM_based_relocalization](https://github.com/Gaochao-hit/LIO-SAM_based_relocalization). This package is build on top of the work done by the ROS1 package of Fast-Lio-Localization - https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION

config/mid360.yaml

@@ -43,8 +43,12 @@
             scan_publish_en:  true       # false: close all the point cloud output
             dense_publish_en: false      # false: low down the points number in a global-frame point clouds scan.
             scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
+            use_odom_transform: false  # true: transform scan to odom frame before matching (must match mapping config)
+            odom_roll: 0.0        # Roll angle (degrees) from odom to camera_init (body->base_link will be inverse)
+            odom_pitch: 0.0        # Pitch angle (degrees) from odom to camera_init (body->base_link will be inverse)
+            odom_yaw: 0.0           # Yaw angle (degrees) from odom to camera_init (body->base_link will be inverse)
 
         pcd_save:
-            pcd_save_en: true
+            pcd_save_en: false
             interval: -1                 # how many LiDAR frames saved in each pcd file; 
                                         # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

fast_lio_localization/invert_livox_scan.py

@@ -1,10 +1,14 @@
 #!/usr/bin/env python3
+
 import rclpy
 from rclpy.node import Node
+import rclpy.parameter
+import rclpy.parameter_service
 from sensor_msgs.msg import PointCloud2, Imu
 from livox_ros_driver2.msg import CustomMsg
+import ros2_numpy
+# from rcl_interfaces.srv import GetParameters
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
-import numpy as np
 
 qos_profile = QoSProfile(
     reliability=ReliabilityPolicy.RELIABLE,  # Ensure reliable message delivery
@@ -13,15 +17,6 @@
 )
 
 class LivoxLaserToPointcloud(Node):
-    LIVOX_DTYPE = np.dtype([
-        ('x', 'f4'),         # offset 0
-        ('y', 'f4'),         # offset 4
-        ('z', 'f4'),         # offset 8
-        ('intensity', 'f4'), # offset 12
-        ('tag', 'u1'),       # offset 16
-        ('line', 'u1'),      # offset 17
-        ('timestamp', 'f8')  # offset 18
-        ])
     def __init__(self):
         super().__init__("Invert_Livox_Scan")
 
@@ -43,22 +38,25 @@ def __init__(self):
 
         self.pub_imu = self.create_publisher(Imu, "/livox/imu", qos_profile=qos_profile)
         self.sub_imu = self.create_subscription(Imu, "/livox/inverted_imu", self.imu_callback, qos_profile=qos_profile)
+
 
     def pointcloud2_callback(self, msg: PointCloud2):
-        # 1. Map the buffer to our structure (Zero-copy view)
-        # We use frombuffer to interpret the raw bytes using our dtype
-        data = np.frombuffer(msg.data, dtype=self.LIVOX_DTYPE).copy()
-
-        # 2. Modify spatial coordinates
-        # These operations are vectorized and extremely fast
-        data['y'] = -data['y']
-        data['z'] = -data['z']
-
-        # 3. Reconstruct message
-        # We copy the original message to keep all metadata (header, fields, etc.)
-        out_msg = msg 
-        out_msg.data = data.tobytes()
+        data = ros2_numpy.numpify(msg)
+        # print(data)
 
+        pc = data['xyz']
+        # print(pc)
+        pc[:, 1] = -pc[:, 1]
+        pc[:, 2] = -pc[:, 2]
+        # print(pc)
+
+        data = {"xyz": pc}  # Invert Y, Z
+        # print(data)
+
+        out_msg = ros2_numpy.msgify(PointCloud2, data)
+        out_msg.header = msg.header
+        # out_msg.header.stamp = self.get_clock().now().to_msg()
+        out_msg.point_step = 12
         self.pub_scan.publish(out_msg)
 
     def custom_msg_callback(self, msg: CustomMsg):

fast_lio_localization/odom_topic.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Point, Quaternion
+import numpy as np
+import tf_transformations
+
+
+class FastLioOdomConverter(Node):
+    def __init__(self):
+        super().__init__("fastlio_odom_converter")
+
+        # Declare ROS2 parameters
+        self.declare_parameters(
+            namespace="",
+            parameters=[
+                ("publish.use_odom_transform", False),
+                ("publish.odom_roll", 0.0),
+                ("publish.odom_pitch", 0.0),
+                ("publish.odom_yaw", 0.0),
+            ],
+        )
+
+        # Check if odom transformation is enabled
+        self.use_odom_transform = self.get_parameter("publish.use_odom_transform").value
+
+        if not self.use_odom_transform:
+            self.get_logger().info("Odom transform disabled - node will not process odometry")
+            # Still create subscriber/publisher but won't publish
+            self.sub = self.create_subscription(Odometry, "/Odometry", self.cb, 10)
+            self.pub = self.create_publisher(Odometry, "/odom", 10)
+            return
+
+        # --- STATIC TRANSFORMS ---------------------------------------
+        # 1. odom -> camera_init
+        odom_roll = np.radians(self.get_parameter("publish.odom_roll").value)
+        odom_pitch = np.radians(self.get_parameter("publish.odom_pitch").value)
+        odom_yaw = np.radians(self.get_parameter("publish.odom_yaw").value)
+        R1 = self.rpy_to_matrix(odom_roll, odom_pitch, odom_yaw)
+
+        # 2. body -> base_link (inverse of odom -> camera_init)
+        # Since odom has the same orientation as base_link, and camera_init has the same orientation as body,
+        # body -> base_link is simply the inverse of odom -> camera_init
+        R2 = np.linalg.inv(R1)
+
+        # Combined static transform:
+        # odom -> base_link = (odom->camera_init) * (body->base_link)
+        self.T_static = R1 @ R2
+
+        # Subscribers and publishers
+        self.sub = self.create_subscription(Odometry, "/Odometry", self.cb, 10)
+        self.pub = self.create_publisher(Odometry, "/odom", 10)
+
+        self.get_logger().info(f"FAST-LIO odom converter started with parameters:")
+        self.get_logger().info(f"  odom->camera_init: roll={self.get_parameter('publish.odom_roll').value}°, "
+                              f"pitch={self.get_parameter('publish.odom_pitch').value}°, "
+                              f"yaw={self.get_parameter('publish.odom_yaw').value}°")
+        self.get_logger().info(f"  body->base_link: computed as inverse (to flip back)")
+
+    # Utility: Convert RPY to 4x4 matrix
+    def rpy_to_matrix(self, roll, pitch, yaw):
+        T = tf_transformations.euler_matrix(roll, pitch, yaw)
+        return T
+
+    # Utility: FAST-LIO odometry: camera_init -> body -> matrix
+    def pose_to_mat(self, pose):
+        T = np.eye(4)
+        T[0, 3] = pose.position.x
+        T[1, 3] = pose.position.y
+        T[2, 3] = pose.position.z
+        q = [pose.orientation.x, pose.orientation.y,
+             pose.orientation.z, pose.orientation.w]
+        T[:3, :3] = tf_transformations.quaternion_matrix(q)[:3, :3]
+        return T
+
+    def cb(self, msg):
+        # If odom transform is disabled, do nothing
+        if not self.use_odom_transform:
+            return
+
+        # camera_init -> body (dynamic odometry)
+        T_cam_to_body = self.pose_to_mat(msg.pose.pose)
+
+        # odom -> base_link = static ⨉ dynamic
+        T_odom_to_base = self.T_static @ T_cam_to_body
+
+        # Extract xyz and quaternion
+        xyz = T_odom_to_base[:3, 3]
+        quat = tf_transformations.quaternion_from_matrix(T_odom_to_base)
+
+        # Construct new odometry
+        odom = Odometry()
+        odom.header.stamp = msg.header.stamp    # OK to reuse
+        odom.header.frame_id = "odom"
+        odom.child_frame_id = "base_link"
+
+        odom.pose.pose.position = Point(
+            x=xyz[0], y=xyz[1], z=xyz[2]
+        )
+        odom.pose.pose.orientation = Quaternion(
+            x=quat[0], y=quat[1], z=quat[2], w=quat[3]
+        )
+
+        odom.twist = msg.twist  # same twist
+
+        self.pub.publish(odom)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FastLioOdomConverter()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

fast_lio_localization/publish_initial_pose.py

@@ -22,7 +22,7 @@ def publish_pose(self, x, y, z, roll, pitch, yaw):
         initial_pose.header.frame_id = "map"
         self.pub_pose.publish(initial_pose)
 
-        self.get_logger().info(f"Initial Pose: {x} {y} {z} {yaw} {pitch} {roll}")
+        self.get_logger().info(f"Initial Pose: {x} {y} {z} {roll} {pitch} {yaw}")
 
 
 def main(args=None):
@@ -33,14 +33,14 @@ def main(args=None):
     parser.add_argument("x", type=float)
     parser.add_argument("y", type=float)
     parser.add_argument("z", type=float)
-    parser.add_argument("yaw", type=float)
-    parser.add_argument("pitch", type=float)
     parser.add_argument("roll", type=float)
+    parser.add_argument("pitch", type=float)
+    parser.add_argument("yaw", type=float)
     args = parser.parse_args()
 
     node.publish_pose(args.x, args.y, args.z, args.roll, args.pitch, args.yaw)
     rclpy.shutdown()
 
 
 if __name__ == "__main__":
-    main()
+    main()