Uploaded Shaoshu changes

sensor_flip/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.10)
+project(sensor_flip)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(livox_ros_driver2 REQUIRED)
+
+include_directories(
+  include
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+add_executable(flip_node src/flip_node.cpp)
+ament_target_dependencies(flip_node rclcpp sensor_msgs tf2 tf2_ros tf2_eigen tf2_geometry_msgs livox_ros_driver2)
+
+target_include_directories(flip_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+install(TARGETS flip_node
+  DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()

sensor_flip/README.md

@@ -0,0 +1,20 @@
+# sensor_flip
+
+A small ROS 2 node to apply a fixed rotation (e.g., 180° roll) to LiDAR PointCloud2 and IMU messages.
+
+- Input topics (defaults):
+  - /livox/lidar
+  - /livox/imu
+- Output topics (defaults):
+  - /livox_flipped/lidar
+  - /livox_flipped/imu
+
+Parameters:
+- roll_deg, pitch_deg, yaw_deg: degrees, default 180, 0, 0
+- lidar_in, imu_in, lidar_out, imu_out
+
+Launch:
+ros2 launch sensor_flip flip.launch.py
+
+Example (180° roll flip, using Livox driver topics):
+ros2 launch sensor_flip flip.launch.py lidar_in:=/livox/lidar imu_in:=/livox/imu lidar_out:=/livox/lidar_flipped imu_out:=/livox/imu_flipped roll_deg:=180.0 pitch_deg:=0.0 yaw_deg:=0.0

sensor_flip/launch/flip.launch.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+import os
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+
+def generate_launch_description():
+    lidar_in = LaunchConfiguration('lidar_in', default='/livox/lidar')
+    imu_in = LaunchConfiguration('imu_in', default='/livox/imu')
+    lidar_out = LaunchConfiguration('lidar_out', default='/livox_flipped/lidar')
+    lidar_out_pc2 = LaunchConfiguration('lidar_out_pc2', default='')
+    imu_out = LaunchConfiguration('imu_out', default='/livox_flipped/imu')
+    roll_deg = LaunchConfiguration('roll_deg', default='180.0')
+    pitch_deg = LaunchConfiguration('pitch_deg', default='0.0')
+    yaw_deg = LaunchConfiguration('yaw_deg', default='0.0')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('lidar_in', default_value=lidar_in, description='Input LiDAR topic'),
+        DeclareLaunchArgument('imu_in', default_value=imu_in, description='Input IMU topic'),
+        DeclareLaunchArgument('lidar_out', default_value=lidar_out, description='Output LiDAR topic'),
+        DeclareLaunchArgument('imu_out', default_value=imu_out, description='Output IMU topic'),
+        DeclareLaunchArgument('roll_deg', default_value=roll_deg, description='Roll rotation in degrees'),
+        DeclareLaunchArgument('pitch_deg', default_value=pitch_deg, description='Pitch rotation in degrees'),
+        DeclareLaunchArgument('yaw_deg', default_value=yaw_deg, description='Yaw rotation in degrees'),
+
+        Node(
+            package='sensor_flip',
+            executable='flip_node',
+            name='sensor_flip_node',
+            output='screen',
+            parameters=[{
+                'lidar_in': lidar_in,
+                'imu_in': imu_in,
+                'lidar_out': lidar_out,
+                'imu_out': imu_out,
+                'roll_deg': roll_deg,
+                'pitch_deg': pitch_deg,
+                'yaw_deg': yaw_deg,
+            }]
+        )
+    ])

sensor_flip/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>sensor_flip</name>
+  <version>0.1.0</version>
+  <description>Flip LiDAR point cloud and IMU measurements (e.g., 180 deg roll) for SuperOdom.</description>
+  <maintainer email="[email protected]">SuperOdom</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>Eigen3</depend>
+  <depend>livox_ros_driver2</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

sensor_flip/src/flip_node.cpp

@@ -0,0 +1,125 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <livox_ros_driver2/msg/custom_msg.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_eigen/tf2_eigen.hpp>
+#include <Eigen/Dense>
+
+class SensorFlipNode : public rclcpp::Node {
+public:
+  SensorFlipNode() : Node("sensor_flip_node") {
+    // Parameters
+    lidar_in_ = declare_parameter<std::string>("lidar_in", "/livox/lidar");
+    imu_in_ = declare_parameter<std::string>("imu_in", "/livox/imu");
+  lidar_out_ = declare_parameter<std::string>("lidar_out", "/livox_flipped/lidar");
+    imu_out_ = declare_parameter<std::string>("imu_out", "/livox_flipped/imu");
+    double roll_deg = declare_parameter<double>("roll_deg", 180.0);
+    double pitch_deg = declare_parameter<double>("pitch_deg", 0.0);
+    double yaw_deg = declare_parameter<double>("yaw_deg", 0.0);
+
+    // Build rotation matrix R from roll,pitch,yaw degrees
+    const double pi = 3.14159265358979323846;
+    double r = roll_deg * pi / 180.0;
+    double p = pitch_deg * pi / 180.0;
+    double y = yaw_deg * pi / 180.0;
+
+    Eigen::AngleAxisd Rx(r, Eigen::Vector3d::UnitX());
+    Eigen::AngleAxisd Ry(p, Eigen::Vector3d::UnitY());
+    Eigen::AngleAxisd Rz(y, Eigen::Vector3d::UnitZ());
+    // ZYX (yaw-pitch-roll) composition commonly used
+    R_ = (Rz * Ry * Rx).toRotationMatrix();
+
+    // Subscribers and publishers
+    // Use compatible QoS: RELIABLE for LiDAR, BEST_EFFORT for IMU
+    rclcpp::QoS lidar_qos(20);
+    lidar_qos.reliability(rclcpp::ReliabilityPolicy::Reliable);
+    lidar_qos.durability(rclcpp::DurabilityPolicy::Volatile);
+
+    rclcpp::QoS imu_qos(10);
+    imu_qos.reliability(rclcpp::ReliabilityPolicy::BestEffort);
+    imu_qos.durability(rclcpp::DurabilityPolicy::Volatile);
+
+    // Subscribe to Livox CustomMsg only (avoids type conflicts)
+    livox_sub_ = create_subscription<livox_ros_driver2::msg::CustomMsg>(
+        lidar_in_, lidar_qos,
+        std::bind(&SensorFlipNode::onLivoxCustom, this, std::placeholders::_1));
+    imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
+        imu_in_, imu_qos,
+        std::bind(&SensorFlipNode::onImu, this, std::placeholders::_1));
+
+    livox_pub_ = create_publisher<livox_ros_driver2::msg::CustomMsg>(lidar_out_, lidar_qos);
+    imu_pub_ = create_publisher<sensor_msgs::msg::Imu>(imu_out_, imu_qos);    RCLCPP_INFO(get_logger(), "sensor_flip configured: lidar %s -> %s, imu %s -> %s, RPY(deg)=(%.1f, %.1f, %.1f)",
+                lidar_in_.c_str(), lidar_out_.c_str(), imu_in_.c_str(), imu_out_.c_str(),
+                roll_deg, pitch_deg, yaw_deg);
+  }
+
+private:
+  void onLivoxCustom(livox_ros_driver2::msg::CustomMsg::SharedPtr msg) {
+    if (!livox_pub_) return;
+    auto out = *msg;
+    // Flip xyz for each point; timestamp and metadata unchanged
+    for (auto &pt : out.points) {
+      Eigen::Vector3d p(pt.x, pt.y, pt.z);
+      p = R_ * p;
+      pt.x = p.x();
+      pt.y = p.y();
+      pt.z = p.z();
+    }
+    livox_pub_->publish(out);
+  }
+
+
+  void onImu(sensor_msgs::msg::Imu::SharedPtr msg) {
+    if (!imu_pub_) return;
+    sensor_msgs::msg::Imu out = *msg;
+
+    // Rotate orientation, angular velocity, and linear acceleration
+    // Convert quaternion to rotation matrix
+    Eigen::Quaterniond q(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
+    if (!(std::isfinite(q.w()) && std::isfinite(q.x()) && std::isfinite(q.y()) && std::isfinite(q.z()))) {
+      q = Eigen::Quaterniond::Identity();
+    } else {
+      q.normalize();
+    }
+    Eigen::Quaterniond qR(R_);
+    Eigen::Quaterniond q_new = q * qR.conjugate();
+    q_new.normalize();
+    out.orientation.w = q_new.w();
+    out.orientation.x = q_new.x();
+    out.orientation.y = q_new.y();
+    out.orientation.z = q_new.z();
+
+    // Angular velocity and linear acceleration are vectors in IMU frame
+    Eigen::Vector3d w(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
+    Eigen::Vector3d a(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
+    w = R_ * w;  a = R_ * a;
+    out.angular_velocity.x = w.x(); out.angular_velocity.y = w.y(); out.angular_velocity.z = w.z();
+    out.linear_acceleration.x = a.x(); out.linear_acceleration.y = a.y(); out.linear_acceleration.z = a.z();
+
+    auto rotCov = [&](std::array<double, 9>& P){
+      Eigen::Map<Eigen::Matrix3d> M(P.data());
+      if (M(0,0) >= 0.0) { M = R_ * M * R_.transpose(); }
+    };
+
+    out.header.frame_id = msg->header.frame_id;
+    imu_pub_->publish(out);
+  }
+
+  std::string lidar_in_, imu_in_, lidar_out_, imu_out_;
+  Eigen::Matrix3d R_;
+  rclcpp::Subscription<livox_ros_driver2::msg::CustomMsg>::SharedPtr livox_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  rclcpp::Publisher<livox_ros_driver2::msg::CustomMsg>::SharedPtr livox_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
+};
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<SensorFlipNode>());
+  rclcpp::shutdown();
+  return 0;
+}

super_odometry/config/livox_mid360_flipped.yaml

@@ -0,0 +1,48 @@
+/**:
+  ros__parameters:
+    imu_topic: "/livox/imu_flipped"
+    laser_topic: "/livox/lidar_flipped"
+    sensor: "livox"
+    use_imu_roll_pitch: false
+
+    world_frame: "map"
+    world_frame_rot: "map_rot"
+    sensor_frame: "sensor"
+    sensor_frame_rot: "sensor_rot"
+
+    imu_acc_x_limit: 0.5
+    imu_acc_y_limit: 0.2
+    imu_acc_z_limit: 0.4
+
+    # Feature Extraction Params
+    feature_extraction_node:
+        scan_line: 4 
+        min_range: 0.2    #<!-- remove too closed points -->
+        filter_point_size: 3
+
+    # Laser Mapping Params
+    laser_mapping_node:
+        mapping_line_resolution: 0.1
+        mapping_plane_resolution: 0.1
+        max_iterations: 5
+        max_surface_features: 4000
+
+        # For localization mode
+        localization_mode: false
+        read_pose_file: false
+        init_x: 0.0
+        init_y: 0.0
+        init_z: 0.0
+        init_roll: 0.0
+        init_pitch: 0.0
+        init_yaw: 0.0 
+
+    # IMU Preintegration Params
+    imu_preintegration_node:
+        lidar_correction_noise: 0.01
+
+        acc_n: 3.9939570888238808e-03          # accelerometer measurement noise standard deviation. #0.2
+        gyr_n: 1.5636343949698187e-03         # gyroscope measurement noise standard deviation.     #0.05
+        acc_w: 6.4356659353532566e-05          # accelerometer bias random work noise standard deviation.  #0.02
+        gyr_w: 3.5640318696367613e-05       # gyroscope bias random work noise standard deviation.     #4.0e-5
+        g_norm: 9.80511    # 0 means you need to provide both imu-camera and laser-camera extrinsic

super_odometry/launch/livox_mid360_with_flip.launch.py

@@ -0,0 +1,78 @@
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+import launch_ros
+
+def get_share_file(package_name, file_name):
+    return os.path.join(get_package_share_directory(package_name), file_name)
+
+def generate_launch_description():
+    config_path = get_share_file(
+        package_name="super_odometry",
+        file_name="config/livox_mid360_flipped.yaml")
+    calib_path = get_share_file(
+        package_name="super_odometry",
+        file_name="config/livox/livox_mid360_calibration.yaml"
+    )
+
+    # Launch the flip node
+    flip_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('sensor_flip'), 'launch/flip.launch.py')
+        ),
+        launch_arguments={
+            'lidar_in': LaunchConfiguration('lidar_in', default='/livox/lidar'),
+            'imu_in': LaunchConfiguration('imu_in', default='/livox/imu'),
+            'lidar_out': LaunchConfiguration('lidar_out', default='/livox/lidar_flipped'),
+            'imu_out': LaunchConfiguration('imu_out', default='/livox/imu_flipped'),
+            'roll_deg': LaunchConfiguration('roll_deg', default='180.0'),
+            'pitch_deg': LaunchConfiguration('pitch_deg', default='0.0'),
+            'yaw_deg': LaunchConfiguration('yaw_deg', default='0.0'),
+        }.items()
+    )
+
+    feature_extraction_node = Node(
+        package="super_odometry",
+        executable="feature_extraction_node",
+        output={
+            "stdout": "screen",
+            "stderr": "screen",
+        },
+        parameters=[config_path, {"calibration_file": calib_path}],
+    )
+
+    laser_mapping_node = Node(
+        package="super_odometry",
+        executable="laser_mapping_node",
+        output={
+            "stdout": "screen",
+            "stderr": "screen",
+        },
+        parameters=[config_path, {"calibration_file": calib_path}],
+        remappings=[
+            ("laser_odom_to_init", "integrated_to_init"),
+        ]
+    )
+
+    imu_preintegration_node = Node(
+        package="super_odometry",
+        executable="imu_preintegration_node",
+        output={
+            "stdout": "screen",
+            "stderr": "screen",
+        },
+        parameters=[config_path, {"calibration_file": calib_path}],
+    )
+
+    return LaunchDescription([
+        launch_ros.actions.SetParameter(name='use_sim_time', value='false'),
+        flip_launch,
+        feature_extraction_node,
+        laser_mapping_node,
+        imu_preintegration_node,
+    ])