diff --git a/sensor_flip/CMakeLists.txt b/sensor_flip/CMakeLists.txt new file mode 100644 index 0000000..f96d0e1 --- /dev/null +++ b/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 + $ + $) + +install(TARGETS flip_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/sensor_flip/README.md b/sensor_flip/README.md new file mode 100644 index 0000000..3964132 --- /dev/null +++ b/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 diff --git a/sensor_flip/launch/flip.launch.py b/sensor_flip/launch/flip.launch.py new file mode 100644 index 0000000..e165548 --- /dev/null +++ b/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, + }] + ) + ]) diff --git a/sensor_flip/package.xml b/sensor_flip/package.xml new file mode 100644 index 0000000..38a5167 --- /dev/null +++ b/sensor_flip/package.xml @@ -0,0 +1,23 @@ + + + sensor_flip + 0.1.0 + Flip LiDAR point cloud and IMU measurements (e.g., 180 deg roll) for SuperOdom. + SuperOdom + BSD-3-Clause + + ament_cmake + + rclcpp + sensor_msgs + tf2 + tf2_ros + tf2_eigen + tf2_geometry_msgs + Eigen3 + livox_ros_driver2 + + + ament_cmake + + diff --git a/sensor_flip/src/flip_node.cpp b/sensor_flip/src/flip_node.cpp new file mode 100644 index 0000000..24beae8 --- /dev/null +++ b/sensor_flip/src/flip_node.cpp @@ -0,0 +1,125 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SensorFlipNode : public rclcpp::Node { +public: + SensorFlipNode() : Node("sensor_flip_node") { + // Parameters + lidar_in_ = declare_parameter("lidar_in", "/livox/lidar"); + imu_in_ = declare_parameter("imu_in", "/livox/imu"); + lidar_out_ = declare_parameter("lidar_out", "/livox_flipped/lidar"); + imu_out_ = declare_parameter("imu_out", "/livox_flipped/imu"); + double roll_deg = declare_parameter("roll_deg", 180.0); + double pitch_deg = declare_parameter("pitch_deg", 0.0); + double yaw_deg = declare_parameter("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( + lidar_in_, lidar_qos, + std::bind(&SensorFlipNode::onLivoxCustom, this, std::placeholders::_1)); + imu_sub_ = create_subscription( + imu_in_, imu_qos, + std::bind(&SensorFlipNode::onImu, this, std::placeholders::_1)); + + livox_pub_ = create_publisher(lidar_out_, lidar_qos); + imu_pub_ = create_publisher(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& P){ + Eigen::Map 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::SharedPtr livox_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Publisher::SharedPtr livox_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/super_odometry/config/livox_mid360_flipped.yaml b/super_odometry/config/livox_mid360_flipped.yaml new file mode 100644 index 0000000..eb8d267 --- /dev/null +++ b/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 # + 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 diff --git a/super_odometry/launch/livox_mid360_with_flip.launch.py b/super_odometry/launch/livox_mid360_with_flip.launch.py new file mode 100644 index 0000000..880adc0 --- /dev/null +++ b/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, + ])