<add>1.Add dev_status.csv for device & data tx rx rate monitor;

2. Add high frequency odom data ; 3. Add image undistort functionality; 4. optimized data publish pipeline 5. other optimizations
2025-09-28 19:19:07 +08:00
parent 97686cc034
commit 7d738e12f8
18 changed files with 1246 additions and 291 deletions
@@ -1 +1,3 @@
-recorddata/
+recorddata/
+/config/calib.yaml
+/log
@@ -96,6 +96,9 @@ endif()
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

+# Set optimization flags
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2")
+
 # Find common dependencies
 find_package(PkgConfig REQUIRED)
 find_package(OpenCV REQUIRED)
@@ -16,7 +16,7 @@ This driver package provides core functionality for point cloud SLAM application

 ## 1. Version

-Current Version: v0.4.1
+Current Version: v0.5.0

 ## 2. Preparation

@@ -30,7 +30,7 @@ Current Version: v0.4.1

 ### 2.2 Dependencies

-● Opencv >= 4.5.0(recommand 4.5.5/4.8.0)
+● Opencv >= 4.5.0(recommand 4.5.5/4.8.0. Make sure only one version of opencv is installed)

 ● yaml-cpp

@@ -122,7 +122,7 @@ source /opt/ros/foxy/setup.bash
 #### 3.4.1 ROS1 (Noetic for example):

 ```shell
-source [ros_workspace]/install/setup.bash
+source [ros_workspace]/devel/setup.bash
 roslaunch odin_ros_driver [launch file]
 ```
 ● odin_ros_driver: package name;
@@ -186,6 +186,8 @@ Odin_ROS_Driver/                // ROS1/ROS2 driver package
    script/
        build_ros1.sh           // Installation script for ROS1
        build_ros2.sh           // Installation script for ROS2
+    recorddata/                 // holds recorded data that can import into MindCloud
+    log/                        // holds log files
    README.md                   // Usage instructions
    CMakeLists.txt              // CMake build file
    License                     // License file
@@ -200,17 +202,67 @@ Odin_ROS_Driver/                // ROS1/ROS2 driver package
 ### 4.3 ROS topics
 Internal parameters of the Odin ROS driver are defined in config/control_command.yaml. Below are descriptions of the commonly used parameters:

-| Topic               | Detailed Description |
-|---------------------|----------------------|
-| odin1/imu           | Imu Topic |
-| odin1/image         | RGB Camera Topic |
-| odin1/image/compressed         | RGB Camera compressed Topic |
-| odin1/cloud_raw     | Raw_Cloud Topic |
-| odin1/cloud_render     | Render_Cloud Topic |
-| odin1/cloud_slam    | Slam_PointCloud Topic |
-| odin1/odometry      | Odom Topic |
-| odin1/depth_img_competetion      | Dense depth image Topic |
-| odin1/depth_img_competetion_cloud      | Dense Depth_Cloud Topic |
+| Topic               |control_command.yaml  | Detailed Description |
+|---------------------|----------------------|----------------------|
+| odin1/imu           | sendimu  | Imu Topic |
+| odin1/image         | sendrgb  | RGB Camera Topic, decoded from jpeg data from device |
+| odin1/image_undistort  | sendrgbundistort  | undistorted RGB Camera Topic, processed with calib.yaml from device |
+| odin1/image/compressed | sendrgbcompressed | RGB Camera compressed Topic, original jpeg data from device |
+| odin1/cloud_raw        | senddtof          | Raw_Cloud Topic |
+| odin1/cloud_render     | sendcloudrender  | Render_Cloud Topic, processed with raw point cloud, rgb image, and calib.yaml from device |
+| odin1/cloud_slam    | sendcloudslam    | Slam_PointCloud Topic |
+| odin1/odometry      | sendodom         | Odom Topic |
+| odin1/odometry_high | sendodom         | high frequency Odom Topic |
+| odin1/depth_img_competetion        | senddepth | Dense depth image Topic, demo, high computing power required |
+| odin1/depth_img_competetion_cloud  | senddepth | Dense Depth_Cloud Topic, demo, high computing power required |
+
+### 4.4 Data format
+
+1. The raw point cloud (cloud_raw) has the following fields:
+```
+float32 x             // X axis, in meters
+float32 y             // Y axis, in meters
+float32 z             // Z axis, in meters
+uint8  intensity      // Reflectivity, range 0–255
+uint16 confidence     // Point confidence, range 0–65535
+float32 offset_time   // Time offset relative to the base timestamp 
+```
+
+To work with this custom format in PCL, first define the point type:
+```cpp
+/*** LS ***/
+namespace ls_ros {
+    struct EIGEN_ALIGN16 Point {
+        float x;
+        float y;
+        float z;
+        uint8_t intensity;
+        uint16_t confidence;
+        float offset_time;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
+}  // namespace ls_ros
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(ls_ros::Point,
+      (float, x, x)
+      (float, y, y)
+      (float, z, z)
+      (uint8_t, intensity, intensity)
+      (uint16_t, confidence, confidence)
+      (float offset_time , offset_time)
+)
+```
+Then, you can easily convert a ROS sensor_msgs::PointCloud2 message into a PCL point cloud:
+```
+pcl::PointCloud<ls_ros::Point> ls_cloud;
+pcl::fromROSMsg(*msg, ls_cloud);
+```
+### 4.5 Other functionalities
+
+|control_command.yaml   | Detailed Description |
+|-----------------------|----------------------|
+| recorddata            | Record data in specific format that can be imported into MindCloud(TM) for post-processing. Please be aware that this will consume a lot of storage space. Testing shows 9.5G for 10mins of data. |
+| devstatuslog          | Device status logging, currently save device status (soc temperature, cpu usage, ram usage, dtof sensor temp .etc) and data tx & rx rate to devstatus.csv under log folder. A new file will be created every time the driver is started. |

 ## 5. FAQ
 ### 5.1 Segmentation fault upon re-launching host SDK
@@ -252,7 +304,20 @@ Unable to open X display or No protocol specified
 xhost + #This command enables graphical passthrough to Docker containers
 ``` 

-### 5.4 RVIZ has not responded for a long time
+### 5.4 ROS driver exit with get version failed error
+
+**Error Message**  
+```shell
+<ERROR><api.cpp:lidar_get_version:672>: get device version fail.
+get version failed.
+```
+
+**Resolution** 
+
+Device firmware version is too low, please update to latest version.
+
+
+### 5.5 RVIZ has not responded for a long time

 **Error Message**  
 Rviz does not respond, and after a while the terminal prints Device disconnected, waiting for reconnection...
@@ -261,7 +326,7 @@ Rviz does not respond, and after a while the terminal prints Device disconnected

 Please power on Odin module again

-### 5.5 Device not responding
+### 5.6 Device not responding

 **Error Message**  
 Missed ok response from device,probably wrong interaction procedure.
@@ -270,7 +335,7 @@ Missed ok response from device,probably wrong interaction procedure.

 Please adopt the solution mentioned in 5.1

-### 5.6 Device has no external calibration file 
+### 5.7 Device has no external calibration file 

 **Error Message**  
 ERROR：Missing camera node 'cam_0'
@@ -279,6 +344,30 @@ ERROR：Missing camera node 'cam_0'

 Please plug and unplug the USB again

+### 5.8 ROS Driver report device disconnected immediately after stream started
+
+**Error Message**  
+
+```shell
+Device ready and streams activated
+Device detaching...
+Wating for device reconnection...
+Device disconnected, waiting for reconnection...
+```
+
+**Reason**
+
+Mostly common on ros2 environment and connected to complex network environment, such as office wifi & ethernet. ROS2 default to broadcast, and complex network environment will cause ros2 publish to block, leading to device disconnection.
+
+**Resolution** 
+
+If cross-device communication is not required, please restrict ros2 to localhost only with:
+```shell
+export ROS_LOCALHOST_ONLY=1
+```
+
+If cross-device communication is required, please simplify the network environment as much as possible. Mini local network with only required devices is recommended.
+
 ## 6.  Contact Information
 To help diagnose the issue, please provide the following details to our FAE engineer:

@@ -1,12 +1,14 @@
 register_keys:
-  streamctrl: 1
-  sendrgb: 1
-  sendimu: 1
-  sendodom: 1
-  senddtof: 1
-  sendcloudslam: 1
-  sendcloudrender: 1
-  sendrgbcompressed: 1
-  senddepth: 0
-  recorddata: 0
+  streamctrl: 1         # 0: off; 1: on
+  sendrgb: 1            # 0: off; 1: on
+  sendimu: 1            # 0: off; 1: on
+  sendodom: 1           # 0: off; 1: on
+  senddtof: 1           # 0: off; 1: on
+  sendcloudslam: 1      # 0: off; 1: on
+  sendcloudrender: 1    # 0: off; 1: on
+  sendrgbcompressed: 1  # 0: off; 1: on
+  senddepth: 0          # 0: off; 1: on
+  sendrgbundistort: 0   # 0: off; 1: on
+  recorddata: 0         # 0: off; 1: on
+  devstatuslog: 1       # 0: off; 1: on

@@ -3,9 +3,12 @@ Panels:
    Help Height: 138
    Name: Displays
    Property Tree Widget:
-      Expanded: ~
-      Splitter Ratio: 0.5
-    Tree Height: 767
+      Expanded:
+        - /Odometry_high1/Shape1
+        - /slam1
+        - /dense_depth_demo1
+      Splitter Ratio: 0.4993045926094055
+    Tree Height: 1052
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
@@ -23,7 +26,7 @@ Panels:
  - Class: rviz/Time
    Name: Time
    SyncMode: 0
-    SyncSource: Image
+    SyncSource: raw
 Preferences:
  PromptSaveOnExit: true
 Toolbars:
@@ -63,11 +66,11 @@ Visualization Manager:
      Value: true
    - Class: rviz/Image
      Enabled: false
-      Image Topic: /odin1/depth_img_competetion
+      Image Topic: /odin1/image/undistorted
      Max Value: 1
      Median window: 5
      Min Value: 0
-      Name: dense_depth_image
+      Name: Image_undistort
      Normalize Range: true
      Queue Size: 2
      Transport Hint: raw
@@ -108,6 +111,41 @@ Visualization Manager:
      Topic: /odin1/odometry
      Unreliable: false
      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: false
+      Keep: 100
+      Name: Odometry_high
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 52; 101; 164
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic: /odin1/odometry_highfreq
+      Unreliable: false
+      Value: false
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
@@ -123,14 +161,16 @@ Visualization Manager:
      Enabled: false
      Invert Rainbow: false
      Max Color: 255; 255; 255
+      Max Intensity: 255
      Min Color: 0; 0; 0
+      Min Intensity: 0
      Name: raw
      Position Transformer: XYZ
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
-      Style: Flat Squares
+      Style: Points
      Topic: /odin1/cloud_raw
      Unreliable: false
      Use Fixed Frame: true
@@ -164,62 +204,110 @@ Visualization Manager:
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: rgb
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: rgb
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: slam_current
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /odin1/cloud_slam
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.10000000149011612
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 5
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: slam_decay
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /odin1/cloud_slam
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
      Name: slam
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic: /odin1/cloud_slam
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
+    - Class: rviz/Group
+      Displays:
+        - Class: rviz/Image
+          Enabled: false
+          Image Topic: /odin1/depth_img_competetion
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: dense_depth_image
+          Normalize Range: true
+          Queue Size: 2
+          Transport Hint: raw
+          Unreliable: false
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Min Color: 0; 0; 0
+          Name: dense_depth_cloud
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /odin1/depth_img_competetion_cloud
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: dense_depth_cloud
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic: /odin1/depth_img_competetion_cloud
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
+      Name: dense_depth_demo
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
@@ -248,7 +336,7 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz/Orbit
-      Distance: 4.46931266784668
+      Distance: 7.223263740539551
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
@@ -264,19 +352,21 @@ Visualization Manager:
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3703991174697876
+      Pitch: 0.6153989434242249
      Target Frame: <Fixed Frame>
-      Yaw: 3.3054051399230957
+      Yaw: 3.0504050254821777
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
-  Height: 1672
+  Height: 1736
  Hide Left Dock: false
  Hide Right Dock: false
  Image:
    collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000002d100000582fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000003f70000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000004710000017f0000002600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000002600ffffff000000010000015f00000582fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000005820000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ab00000005efc0100000002fb0000000800540069006d0065010000000000000ab0000006dc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006680000058200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Image_undistort:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001bf0000060afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004e3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000526000001210000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f007200740000000568000000df0000001600ffffff000000010000015f0000060afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000060a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000af80000005efc0100000002fb0000000800540069006d0065010000000000000af8000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007ce0000060a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
@@ -285,8 +375,8 @@ Window Geometry:
    collapsed: false
  Views:
    collapsed: false
-  Width: 2736
-  X: 144
-  Y: 54
+  Width: 2808
+  X: 72
+  Y: 27
  dense_depth_image:
-    collapsed: false
+    collapsed: false
@@ -7,6 +7,9 @@ Panels:
        - /Global Options1
        - /Status1
        - /Image1/Topic1
+        - /Odometry_high1/Shape1
+        - /slam1
+        - /dense_depth_demo1
      Splitter Ratio: 0.5
    Tree Height: 548
  - Class: rviz_common/Selection
@@ -62,14 +65,14 @@ Visualization Manager:
      Max Value: 1
      Median window: 5
      Min Value: 0
-      Name: dense_depth_image
+      Name: Image_undistort
      Normalize Range: true
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
-        Value: /odin1/depth_img_competetion
+        Value: /odin1/image/undistorted
      Value: false
    - Angle Tolerance: 0.10000000149011612
      Class: rviz_default_plugins/Odometry
@@ -110,6 +113,45 @@ Visualization Manager:
        Reliability Policy: Reliable
        Value: /odin1/odometry
      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: false
+      Keep: 10
+      Name: Odometry_high
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 98; 160; 234
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odin1/odometry_highfreq
+      Value: false
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
@@ -167,7 +209,7 @@ Visualization Manager:
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
-      Style: Flat Squares
+      Style: Points
      Topic:
        Depth: 5
        Durability Policy: Volatile
@@ -178,74 +220,130 @@ Visualization Manager:
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: slam_current
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/cloud_slam
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 0.10000000149011612
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 10
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: slam_deacy
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 1
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/cloud_slam
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
      Name: slam
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /odin1/cloud_slam
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/Image
+          Enabled: false
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: dense_depth_image
+          Normalize Range: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/depth_img_competetion
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: dense_depth_cloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/depth_img_competetion_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: dense_depth_cloud
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /odin1/depth_img_competetion_cloud
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
+      Name: dense_depth_demo
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
@@ -292,25 +390,25 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
-      Distance: 7.086822509765625
+      Distance: 6.814720153808594
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
-        X: 0.40898558497428894
-        Y: -0.07609735429286957
-        Z: 0.5482669472694397
+        X: 0.38628554344177246
+        Y: -1.1808340549468994
+        Z: 1.2151938676834106
      Focal Shape Fixed Size: false
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3503986895084381
+      Pitch: 0.5653982758522034
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
-      Yaw: 3.4554057121276855
+      Yaw: 3.0103981494903564
    Saved: ~
 Window Geometry:
  Displays:
@@ -320,7 +418,9 @@ Window Geometry:
  Hide Right Dock: false
  Image:
    collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000242000003abfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002f2000000f60000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d00610067006500000002cd0000011b0000002800ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003ab000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000423000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Image_undistort:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000242000003abfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002f2000000f60000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d00610067006500000002cd0000011b0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000031d000000cb0000002800ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003ab000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000423000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Tool Properties:
@@ -328,7 +428,7 @@ Window Geometry:
  Views:
    collapsed: false
  Width: 1920
-  X: 70
-  Y: 27
+  X: 498
+  Y: 112
  dense_depth_image:
-    collapsed: false
+    collapsed: false
@@ -1,3 +1,15 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
 #pragma once

 #include <cstdint>
@@ -47,15 +47,18 @@ public:
 private:
    std::string cloud_raw_topic_;
    std::string color_compressed_topic_;
+    std::string color_raw_topic_;
    std::string depth_image_topic_;
    std::string depth_cloud_topic_;

    message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloud_sub_;
    message_filters::Subscriber<sensor_msgs::msg::CompressedImage> color_compressed_sub_;
+    message_filters::Subscriber<sensor_msgs::msg::Image> color_sub_;

    typedef message_filters::sync_policies::ApproximateTime<
        sensor_msgs::msg::PointCloud2, 
-        sensor_msgs::msg::CompressedImage> MySyncPolicy;
+        // sensor_msgs::msg::CompressedImage,
+        sensor_msgs::msg::Image> MySyncPolicy;
    typedef message_filters::Synchronizer<MySyncPolicy> Sync;
    std::shared_ptr<Sync> sync_;

@@ -69,7 +72,8 @@ private:
    PointCloudToDepthConverter::CameraParams loadCameraParams();

    void syncCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg,
-                      const sensor_msgs::msg::CompressedImage::ConstSharedPtr image_msg);
+                      // const sensor_msgs::msg::CompressedImage::ConstSharedPtr image_msg,
+                      const sensor_msgs::msg::Image::ConstSharedPtr color_msg);


    void publishDepthImage(const cv::Mat &img,
@@ -48,14 +48,16 @@ private:


    std::string cloud_raw_topic_;
+    std::string color_raw_topic_;
    std::string color_compressed_topic_;
    std::string depth_image_topic_;
    std::string depth_cloud_topic_;

    message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
+    message_filters::Subscriber<sensor_msgs::Image> color_sub_;
    message_filters::Subscriber<sensor_msgs::CompressedImage> color_compressed_sub_;

-    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CompressedImage> MySyncPolicy;
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;
    typedef message_filters::Synchronizer<MySyncPolicy> Sync;
    std::shared_ptr<Sync> sync_;

@@ -68,7 +70,7 @@ private:


    void syncCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
-                      const sensor_msgs::CompressedImageConstPtr &image_msg);
+                      const sensor_msgs::ImageConstPtr &image_msg);


    void publishDepthImage(const cv::Mat &img,
@@ -43,6 +43,15 @@ limitations under the License.
 #include <sys/types.h>
 #include <sys/wait.h>

+#include <yaml-cpp/yaml.h>
+#include "polynomial_camera.hpp"
+
+struct CameraParams {
+    int width, height;
+    double fx, fy, cx, cy, skew;
+    double k2, k3, k4, k5, k6, k7;
+    double p1, p2;
+};

 #define LOG_LEVEL_NONE 0
 #define LOG_LEVEL_ERROR 1
@@ -128,21 +137,9 @@ extern int g_sendcloudrender;
 #endif

 // Common definitions
-#define GD_ACCL_G 9.7833f
-#define ACC_1G_ms2 9.8
-#define ACC_SEN_SCALE 4096
 #define PAI 3.14159265358979323846
-#define GYRO_SEN_SCALE 16.4f
 #define DTOF_NUM_ROW_PER_GROUP 6
 // Common functions
-inline float accel_convert(int16_t raw, int sen_scale) {
-    return (raw * GD_ACCL_G / sen_scale);
-}
-
-inline float gyro_convert(int16_t raw, float sen_scale) {
-    return (raw * PAI) / (sen_scale * 180); 
-}
-
 inline ros::Time ns_to_ros_time(uint64_t timestamp_ns) {
    ros::Time t;
    #ifdef ROS2
@@ -211,7 +208,7 @@ public:
    }
 
    rawCloudRender render_;
-    void publishImu(icm_6aixs_data_t *stream) {
+    void publishImu(imu_convert_data_t *stream) {
        #ifdef ROS2
            sensor_msgs::msg::Imu imu_msg;
        #else
@@ -220,15 +217,15 @@ public:
        
        imu_msg.header.stamp = ns_to_ros_time(stream->stamp);
        imu_msg.header.frame_id = "imu_link";
-        
-        imu_msg.linear_acceleration.y = -1 * static_cast<double>(accel_convert(stream->aacx, ACC_SEN_SCALE));
-        imu_msg.linear_acceleration.x = static_cast<double>(accel_convert(stream->aacy, ACC_SEN_SCALE));
-        imu_msg.linear_acceleration.z = static_cast<double>(accel_convert(stream->aacz, ACC_SEN_SCALE));
-        
-        imu_msg.angular_velocity.y = -1 * static_cast<double>(gyro_convert(stream->gyrox, GYRO_SEN_SCALE));
-        imu_msg.angular_velocity.x = static_cast<double>(gyro_convert(stream->gyroy, GYRO_SEN_SCALE));
-        imu_msg.angular_velocity.z = static_cast<double>(gyro_convert(stream->gyroz, GYRO_SEN_SCALE));
-        
+
+        imu_msg.linear_acceleration.y = -1 * stream->accel_x;
+        imu_msg.linear_acceleration.x = stream->accel_y;
+        imu_msg.linear_acceleration.z = stream->accel_z;
+
+        imu_msg.angular_velocity.y = -1 * stream->gyro_x;
+        imu_msg.angular_velocity.x = stream->gyro_y;
+        imu_msg.angular_velocity.z = stream->gyro_z;
+
        imu_msg.orientation.x = 0.0;
        imu_msg.orientation.y = 0.0;
        imu_msg.orientation.z = 0.0;
@@ -552,8 +549,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
 	//std::cout << stream->imageCount << std::endl;
    float dtof_subframe_odr = getRosNodeControl()->getDtofSubframeODR() / 1000.0f;
    // printf("dtof_subframe_odr: %f\n", dtof_subframe_odr);
-	    
-    int valid_points = 0;
+
    if (stream->imageCount == 4) {

        uint8_t* intensity_data = static_cast<uint8_t*>(stream->imageList[2].pAddr);
@@ -561,25 +557,29 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
    
        for (int i = 0; i < total_points; ++i) {
            if (confidence_data[i] < 35) {
-                continue;
-            }
-            // XYZ point
-            *iter_x = xyz_data_f[i * 3 + 2] / 1000.0f; ++iter_x;
-            *iter_y = -xyz_data_f[i * 3 + 0] / 1000.0f; ++iter_y;
-            *iter_z = xyz_data_f[i * 3 + 1] / 1000.0f; ++iter_z;
-            
-            *iter_intensity = intensity_data[i]; ++iter_intensity;
-            *iter_confidence = confidence_data[i]; ++iter_confidence;
-            
-            if (dtof_subframe_odr > 0.0) {
-                int group = i / DTOF_NUM_ROW_PER_GROUP;
-                float timestamp_offset = group * 1.0 / dtof_subframe_odr;
+                *iter_x = 0.0f; ++iter_x;
+                *iter_y = 0.0f; ++iter_y;
+                *iter_z = 0.0f; ++iter_z;
+                *iter_intensity = 0; ++iter_intensity;
+                *iter_confidence = 0; ++iter_confidence;
+                *iter_offsettime = 0.0f; ++iter_offsettime;
+            } else {
+                // XYZ point
+                *iter_x = xyz_data_f[i * 3 + 2] / 1000.0f; ++iter_x;
+                *iter_y = -xyz_data_f[i * 3 + 0] / 1000.0f; ++iter_y;
+                *iter_z = xyz_data_f[i * 3 + 1] / 1000.0f; ++iter_z;
+                
+                *iter_intensity = intensity_data[i]; ++iter_intensity;
+                *iter_confidence = confidence_data[i]; ++iter_confidence;
+                
+                if (dtof_subframe_odr > 0.0) {
+                    int group = i / DTOF_NUM_ROW_PER_GROUP;
+                    float timestamp_offset = group * 1.0 / dtof_subframe_odr;

-                *iter_offsettime = timestamp_offset;
-                ++iter_offsettime;
+                    *iter_offsettime = timestamp_offset;
+                    ++iter_offsettime;
+                }
            }
-
-            valid_points++;
    	}
    } else {
        uint16_t* intensity_data = static_cast<uint16_t*>(stream->imageList[2].pAddr);
@@ -601,8 +601,6 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
            
            *iter_confidence = 0;
            ++iter_confidence;
-            
-            valid_points++;
        }
    }

@@ -657,9 +655,9 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
            //Create ROS image message
            #ifdef ROS2
                auto header = std::make_shared<std_msgs::msg::Header>();
-                header->stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation
+                header->stamp = ns_to_ros_time(image.timestamp); // Offset compensation

-                //RCLCPP_INFO(rclcpp::get_logger("device_cb"), "image rgb %ld",image.timestamp + 719060);
+                //RCLCPP_INFO(rclcpp::get_logger("device_cb"), "image rgb %ld",image.timestamp);
                header->frame_id = "camera_rgb_frame";
                
                auto cv_image = std::make_shared<cv_bridge::CvImage>(*header, "bgr8", bgr);
@@ -693,7 +691,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
                // Enqueue binary logging for image
                if (data_logger_) {
                    const uint32_t idx_now = image_index_.fetch_add(1, std::memory_order_relaxed);
-                    const double ts_sec = static_cast<double>(image.timestamp + 719060) / 1e9;
+                    const double ts_sec = static_cast<double>(image.timestamp) / 1e9;
                    const uint32_t jpeg_size = static_cast<uint32_t>(compressed_msg->data.size());
                    std::vector<uint8_t> blob;
                    blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t) + jpeg_size);
@@ -713,7 +711,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
            #else
                // ROS1 version
                std_msgs::Header header;
-                header.stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation
+                header.stamp = ns_to_ros_time(image.timestamp); // Offset compensation
                header.frame_id = "camera_rgb_frame";
                
                auto cv_image = boost::make_shared<cv_bridge::CvImage>(header, "bgr8", bgr);
@@ -785,7 +783,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
        cv::Mat decoded_image = cv::imdecode(jpeg_data, cv::IMREAD_COLOR);

        cv_bridge::CvImage cv_image;
-        cv_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp + 719060);
+        cv_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
        cv_image.encoding = "bgr8";
        cv_image.image = decoded_image;

@@ -800,7 +798,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
        // Enqueue binary logging for image
        if (data_logger_) {
            const uint32_t idx_now = image_index_.fetch_add(1, std::memory_order_relaxed);
-            const double ts_sec = static_cast<double>(stream->imageList[0].timestamp + 719060) / 1e9;
+            const double ts_sec = static_cast<double>(stream->imageList[0].timestamp) / 1e9;
            const uint32_t jpeg_size = static_cast<uint32_t>(jpeg_data.size());
            std::vector<uint8_t> blob;
            blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t) + jpeg_size);
@@ -815,13 +813,26 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
            data_logger_->enqueueImageFrame(std::move(blob));
        }

+        // undistort image
+        cv::Mat undistorted_image = cv::Mat::zeros(decoded_image.size(), decoded_image.type());
+        cv_bridge::CvImage cv_undistorted_image;
+
+        if (m_undistort_map_init_success) {
+            cv::remap(decoded_image, undistorted_image, m_undistort_map_x, m_undistort_map_y, cv::INTER_LINEAR);
+            cv_undistorted_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
+            cv_undistorted_image.encoding = "bgr8";
+            cv_undistorted_image.image = undistorted_image;
+        }
        #ifdef ROS2
        {
            rgb_pub_->publish(*cv_image.toImageMsg());
+            if (m_undistort_map_init_success) {
+                undistort_rgb_pub_->publish(*cv_undistorted_image.toImageMsg());
+            }

            // original jpeg
            sensor_msgs::msg::CompressedImage jpeg_msg;
-            jpeg_msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp + 719060);
+            jpeg_msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
            jpeg_msg.format = "jpeg";
            jpeg_msg.data = jpeg_data;

@@ -830,12 +841,15 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
        #else
        {
            rgb_pub_.publish(cv_image.toImageMsg());
+            if (m_undistort_map_init_success) {
+                undistort_rgb_pub_.publish(cv_undistorted_image.toImageMsg());
+            }

            // original jpeg
            sensor_msgs::CompressedImagePtr jpeg_msg(new sensor_msgs::CompressedImage());
            // compressed_msg->header = header;
            // compressed_msg->format = "jpeg";
-            jpeg_msg->header.stamp = ns_to_ros_time(stream->imageList[0].timestamp + 719060);
+            jpeg_msg->header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
            jpeg_msg->format = "jpeg";
            jpeg_msg->data = jpeg_data;

@@ -977,7 +991,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
 #endif
    }

-    void publishOdometry(capture_Image_List_t* stream) {
+    void publishOdometry(capture_Image_List_t* stream, bool is_highfreq) {
        
 #ifdef ROS2
            auto msg = nav_msgs::msg::Odometry();
@@ -1062,9 +1076,17 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
            }

 #ifdef ROS2
-            odom_publisher_->publish(std::move(msg));
+            if (is_highfreq) {
+                odom_highfreq_publisher_->publish(std::move(msg));
+            } else {
+                odom_publisher_->publish(std::move(msg));
+            }
 #else
-            odom_publisher_.publish(msg);
+            if (is_highfreq) {
+                odom_highfreq_publisher_.publish(msg);
+            } else {
+                odom_publisher_.publish(msg);
+            }
 #endif
    }

@@ -1088,6 +1110,81 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
        }
    }

+    int loadCameraParams(const std::string& yaml_file) {
+        try {
+            YAML::Node config = YAML::LoadFile(yaml_file);
+    
+            YAML::Node cam_node = config["cam_0"];
+    
+            m_camera_params.width = cam_node["image_width"].as<int>();
+            m_camera_params.height = cam_node["image_height"].as<int>();
+    
+            double A11 = cam_node["A11"].as<double>();
+            double A12 = cam_node["A12"].as<double>();
+            double A22 = cam_node["A22"].as<double>();
+            double u0 = cam_node["u0"].as<double>();
+            double v0 = cam_node["v0"].as<double>();
+    
+            m_camera_params.fx = A11;
+            m_camera_params.fy = A22;
+            m_camera_params.cx = u0;
+            m_camera_params.cy = v0;
+            m_camera_params.skew = A12;
+    
+            m_camera_params.k2 = cam_node["k2"].as<double>();
+            m_camera_params.k3 = cam_node["k3"].as<double>();
+            m_camera_params.k4 = cam_node["k4"].as<double>();
+            m_camera_params.k5 = cam_node["k5"].as<double>();
+            m_camera_params.k6 = cam_node["k6"].as<double>();
+            m_camera_params.k7 = cam_node["k7"].as<double>();
+            m_camera_params.p1 = cam_node["p1"].as<double>();
+            m_camera_params.p2 = cam_node["p2"].as<double>();
+#if 0 
+            std::cout << "成功读取相机参数:" << std::endl;
+            std::cout << "图像尺寸: " << m_camera_params.width << "x" << m_camera_params.height << std::endl;
+            std::cout << "焦距: fx=" << m_camera_params.fx << ", fy=" << m_camera_params.fy << std::endl;
+            std::cout << "主点: cx=" << m_camera_params.cx << ", cy=" << m_camera_params.cy << std::endl;
+            std::cout << "倾斜: " << m_camera_params.skew << std::endl;
+            std::cout << "畸变系数: k2=" << m_camera_params.k2 << ", k3=" << m_camera_params.k3 
+                      << ", k4=" << m_camera_params.k4 << ", k5=" << m_camera_params.k5 
+                      << ", k6=" << m_camera_params.k6 << ", k7=" << m_camera_params.k7 << std::endl;
+#endif            
+            m_cam = std::make_unique<mini_vikit::PolynomialCamera>(m_camera_params.width, m_camera_params.height,
+                m_camera_params.fx, m_camera_params.fy, m_camera_params.cx, m_camera_params.cy, m_camera_params.skew,
+                m_camera_params.k2, m_camera_params.k3, m_camera_params.k4, m_camera_params.k5, m_camera_params.k6, m_camera_params.k7);
+
+            m_cam_init_success = true;
+            return 0;
+        } catch (const std::exception& e) {
+            std::cerr << "读取YAML文件失败: " << e.what() << std::endl;
+            m_cam_init_success = false;
+            return -1;
+        }
+    }
+
+    void buildUndistortMap()
+    {
+        m_undistort_map_x.create(m_camera_params.height, m_camera_params.width, CV_32F);
+        m_undistort_map_y.create(m_camera_params.height, m_camera_params.width, CV_32F);
+
+        for (int v_out = 0; v_out < m_camera_params.height; ++v_out) {
+            for (int u_out = 0; u_out < m_camera_params.width; ++u_out) {
+                double x_norm = (u_out - m_cam->cx()) / m_cam->fx();
+                double y_norm = (v_out - m_cam->cy()) / m_cam->fy();
+
+                // remove skew
+                x_norm = x_norm - y_norm * m_cam->skew() / m_cam->fx();
+
+                Eigen::Vector2d uv(x_norm, y_norm);
+                Eigen::Vector2d distorted_pixel = m_cam->world2cam(uv);
+
+                m_undistort_map_x.at<float>(v_out, u_out) = static_cast<float>(distorted_pixel[0]);
+                m_undistort_map_y.at<float>(v_out, u_out) = static_cast<float>(distorted_pixel[1]);
+            }
+        }
+        m_undistort_map_init_success = true;
+    }
+
 private:
    // Add the following member variables
    std::mutex rgb_queue_mutex_;
@@ -1132,7 +1229,12 @@ private:
        return images;
    }

-
+    CameraParams m_camera_params;
+    std::unique_ptr<mini_vikit::PolynomialCamera> m_cam;
+    bool m_cam_init_success;
+    bool m_undistort_map_init_success = false;
+    cv::Mat m_undistort_map_x;
+    cv::Mat m_undistort_map_y;
 #ifdef ROS2
    std::vector<sensor_msgs::msg::PointCloud2> getIntensityCloudQueueSnapshot() {
        std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
@@ -1164,8 +1266,10 @@ private:
            cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", 10);
            xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", 10);
            odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", 10);
+            odom_highfreq_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_highfreq", 10);
            rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", 10);
 	    compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", 10);
+        undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", 10);
        #endif
    }
    #ifdef ROS1
@@ -1175,8 +1279,10 @@ private:
            cloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_raw", 10);
            xyzrgbacloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_slam", 10);
            odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry", 10);
+            odom_highfreq_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry_highfreq", 10);
            rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 10);
            compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 10);
+            undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 10);
        }
    #endif

@@ -1187,20 +1293,24 @@ private:
        rclcpp::Publisher<ros::PointCloud2>::SharedPtr cloud_pub_;
        rclcpp::Publisher<ros::PointCloud2>::SharedPtr xyzrgbacloud_pub_;
        rclcpp::Publisher<ros::Odometry>::SharedPtr odom_publisher_;
+        rclcpp::Publisher<ros::Odometry>::SharedPtr odom_highfreq_publisher_;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr rendered_cloud_pub_;
        rclcpp::Publisher<PointCloud2Msg>::SharedPtr rgbcloud_pub_;
        rclcpp::Publisher<ImageMsg>::SharedPtr rgbFromnv12_pub_;
        rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_rgb_pub_; // New compressed image publisher
+        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr undistort_rgb_pub_;
    #else
        ros::Publisher imu_pub_;
        ros::Publisher rgb_pub_;
        ros::Publisher cloud_pub_;
        ros::Publisher xyzrgbacloud_pub_;
        ros::Publisher odom_publisher_;
+        ros::Publisher odom_highfreq_publisher_;
        ros::Publisher rendered_cloud_pub_;
        ros::Publisher rgbcloud_pub_;
        ros::Publisher rgbFromnv12_pub_;
        ros::Publisher compressed_rgb_pub_; // New compressed image publisher
+        ros::Publisher undistort_rgb_pub_;
    #endif
 };

@@ -53,6 +53,8 @@ typedef enum {
    LIDAR_DT_RAW_DTOF           = 1 << 3,
    LIDAR_DT_SLAM_CLOUD         = 1 << 4,
    LIDAR_DT_SLAM_ODOMETRY      = 1 << 5,
+    LIDAR_DT_DEV_STATUS         = 1 << 6,
+    LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ = 1 << 7,
 } lidar_data_type_e;

 typedef struct {
@@ -90,19 +92,16 @@ typedef struct {
    int64_t cov[3 * 3 * 2];
 } ros_odom_convert_complete_t;

-typedef struct icm_6aixs_data_t {
-	int16_t aacx;
-	int16_t aacy; 
-	int16_t aacz;
-	int16_t gyrox;
-	int16_t gyroy;
-	int16_t gyroz;
-	uint8_t valid;
-	uint32_t nums;
-	uint8_t fsync_pack;
-	uint16_t interval;
-	uint64_t stamp;
-} icm_6aixs_data_t;
+typedef struct {
+    float accel_x;
+    float accel_y;
+    float accel_z;
+    float gyro_x;
+    float gyro_y;
+    float gyro_z;
+    uint64_t stamp;
+    uint64_t sequence;
+} imu_convert_data_t;

 typedef struct {
    uint32_t length;
@@ -140,6 +139,68 @@ typedef struct {
    char host_app_version[64];
 } lidar_version_t;

+/**
+ * @brief RGB image sensor frame rate
+ * 
+ */
+ typedef struct{
+
+    int configured_odr; /* rgb image sensor configured output data rate */
+    int tx_odr;         /* rgb image sensor tx output data rate */
+
+} lidar_rgb_sensor_status_t;
+
+/**
+ * @brief DTOF Lidar frame rate
+ * 
+ */
+typedef struct{
+
+    int configured_odr; /* dtof lidar sensor configured output data rate */
+    int tx_odr;         /* dtof lidar sensor tx output data rate */
+    int subframe_odr;   /* dtof lidar sensor subframe output data rate */
+    short tx_temp;      /* dtof lidar tx module temp */
+    short rx_temp;      /* dtof lidar rx module temp */
+
+} lidar_dtof_sensor_status_t;
+
+/**
+ * @brief IMU Sensor
+ * 
+ */
+typedef struct{
+
+    int configured_odr; /* imu sensor configured output data rate */
+    int tx_odr;         /* imu sensor tx output data rate */
+
+} lidar_imu_sensor_status_t;
+
+typedef struct{
+
+    int package_temp;   /* soc package temp */
+    int cpu_temp;       /* cpu temp */
+    int center_temp;    /* center temp */
+    int gpu_temp;       /* gpu temp */
+    int npu_temp;       /* npu temp */
+
+} lidar_soc_thermal_t;
+typedef struct
+{
+    lidar_soc_thermal_t soc_thermal; 
+
+    int cpu_use_rate[8];              /* cpu usage rate */
+    int ram_use_rate;                 /* ram usage rate */
+
+    lidar_rgb_sensor_status_t rgb_sensor;
+    lidar_dtof_sensor_status_t dtof_sensor;
+    lidar_imu_sensor_status_t imu_sensor;
+
+    int slam_cloud_tx_odr;          /* slam cloud tx output data rate */
+    int slam_odom_tx_odr;           /* slam odom tx output data rate */
+    int slam_odom_highfreq_tx_odr;  /* slam odom high freq tx output data rate */
+
+} lidar_device_status_t;
+
 #ifdef __cplusplus
 }
 #endif
@@ -0,0 +1,144 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+#pragma once
+
+#include <Eigen/Dense>
+#include <cmath>
+
+namespace mini_vikit {
+
+using namespace Eigen;
+
+class PolynomialCamera {
+private:
+    const double fx_, fy_;
+    const double cx_, cy_;
+    const double skew_; 
+    bool distortion_; 
+    double k2_, k3_, k4_, k5_, k6_, k7_;
+
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    PolynomialCamera(double width, double height,
+                    double fx, double fy, double cx, double cy, double skew,
+                    double k2=0.0, double k3=0.0, double k4=0.0, 
+                    double k5=0.0, double k6=0.0, double k7=0.0)
+        : fx_(fx), fy_(fy), cx_(cx), cy_(cy), skew_(skew),
+          distortion_(std::abs(k2) > 1e-7) {
+        k2_ = k2; k3_ = k3; k4_ = k4; k5_ = k5; k6_ = k6; k7_ = k7;
+    }
+
+    Vector3d cam2world(const double& u, const double& v) const {
+        Vector3d xyz;
+        if (!distortion_) {
+            double y = (v - cy_) / fy_;
+            double x = (u - cx_ - y * skew_) / fx_;
+            xyz << x, y, 1.0;
+        } else {
+            double y = (v - cy_) / fy_;
+            double x = (u - cx_ - y * skew_) / fx_;
+
+            const double thetad = std::sqrt(x * x + y * y);
+            double theta = thetad;
+            
+            for (int i = 0; i < 7; ++i) {
+                const double theta2 = theta * theta;
+                const double theta3 = theta2 * theta;
+                const double theta4 = theta3 * theta;
+                const double theta5 = theta4 * theta;
+                const double theta6 = theta5 * theta;
+                theta = thetad / (1.0 + k2_ * theta + k3_ * theta2 + k4_ * theta3 + 
+                                 k5_ * theta4 + k6_ * theta5 + k7_ * theta6);
+            }
+            
+            const double scaling = std::tan(theta) / thetad;
+            x *= scaling;
+            y *= scaling;
+            xyz << x, y, 1.0;
+        }
+        return xyz.normalized();
+    }
+
+    Vector3d cam2world(const Vector2d& px) const {
+        return cam2world(px[0], px[1]);
+    }
+
+    Vector2d world2cam(const Vector3d& xyz) const {
+        Vector2d px;
+        if (!distortion_) {
+            px[0] = fx_ * xyz[0] + cx_;
+            px[1] = fy_ * xyz[1] + cy_;
+        } else {
+            double xd, yd;
+            const double r = std::sqrt(xyz(1) * xyz(1) + xyz(0) * xyz(0));
+            const double theta = std::acos(xyz(2) / xyz.norm());
+            const double thetad = thetad_from_theta(theta);
+            const double scaling = thetad / r;
+            xd = xyz[0] * scaling;
+            yd = xyz[1] * scaling;
+            px[0] = xd * fx_ + yd * skew_ + cx_;
+            px[1] = yd * fy_ + cy_;
+        }
+        return px;
+    }
+
+    Vector2d world2cam(const Vector2d& uv) const {
+        Vector2d px;
+        if (!distortion_) {
+            px[0] = fx_ * uv[0] + cx_;
+            px[1] = fy_ * uv[1] + cy_;
+        } else {
+            double xd, yd;
+            const double r = uv.norm();
+            if (r < 1e-8) {
+                return uv;
+            }
+            const double theta = std::atan(r);
+            const double thetad = thetad_from_theta(theta);
+            const double scaling = thetad / r;
+            xd = uv[0] * scaling;
+            yd = uv[1] * scaling;
+            px[0] = xd * fx_ + yd * skew_ + cx_;
+            px[1] = yd * fy_ + cy_;
+        }
+        return px;
+    }
+
+    inline double thetad_from_theta(const double theta) const {
+        const double theta2 = theta * theta;
+        const double theta3 = theta2 * theta;
+        const double theta4 = theta3 * theta;
+        const double theta5 = theta4 * theta;
+        const double theta6 = theta5 * theta;
+        const double theta7 = theta6 * theta;
+        const double thetad = theta + k2_ * theta2 + k3_ * theta3 +
+                             k4_ * theta4 + k5_ * theta5 + k6_ * theta6 + k7_ * theta7;
+        return thetad;
+    }
+
+    double fx() const { return fx_; }
+    double fy() const { return fy_; }
+    double cx() const { return cx_; }
+    double cy() const { return cy_; }
+    double skew() const { return skew_; }
+    bool has_distortion() const { return distortion_; }
+    
+    double k2() const { return k2_; }
+    double k3() const { return k3_; }
+    double k4() const { return k4_; }
+    double k5() const { return k5_; }
+    double k6() const { return k6_; }
+    double k7() const { return k7_; }
+};
+} // namespace mini_vikit
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>odin_ros_driver</name>
+  <version>0.0.1</version>
+  <description>ROS driver for Odin sensor</description>
+  <maintainer email="[email protected]">rlk</maintainer>
+  <license>Apache 2.0</license>
+  
+  <!-- ROS1 uses catkin as the build tool -->
+  <buildtool_depend>catkin</buildtool_depend>
+  
+  <!-- ROS1 dependencies -->
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  
+  <!-- System dependencies -->
+  <depend>eigen</depend>
+  <depend>opencv</depend>
+  <depend>yaml-cpp</depend>
+  
+  <!-- Specify build type as catkin -->
+  <export>
+    <build_type>catkin</build_type>
+  </export>
+</package>
@@ -10,12 +10,14 @@ DepthImageRos2Node::DepthImageRos2Node(const rclcpp::NodeOptions & options)
    
    cloud_raw_topic_ = this->declare_parameter<std::string>("cloud_raw_topic", "/odin1/cloud_raw");
    color_compressed_topic_ = this->declare_parameter<std::string>("color_compressed_topic", "/odin1/image/compressed");
+    color_raw_topic_ = this->declare_parameter<std::string>("color_raw_topic", "/odin1/image");
    depth_image_topic_ = this->declare_parameter<std::string>("depth_image_topic", "/odin1/depth_img_competetion");
    depth_cloud_topic_ = this->declare_parameter<std::string>("depth_cloud_topic", "/odin1/depth_img_competetion_cloud");

    RCLCPP_INFO_STREAM(this->get_logger(), 
                       "\n  cloud_raw_topic: " << cloud_raw_topic_
                       << "\n  color_compressed_topic: " << color_compressed_topic_
+                       << "\n  color_raw_topic: " << color_raw_topic_
                       << "\n  depth_image_topic: " << depth_image_topic_
                       << "\n  depth_cloud_topic: " << depth_cloud_topic_);
 }
@@ -24,8 +26,9 @@ void DepthImageRos2Node::initialize()
 {
    cloud_sub_.subscribe(this, cloud_raw_topic_);
    color_compressed_sub_.subscribe(this, color_compressed_topic_);
+    color_sub_.subscribe(this, color_raw_topic_);

-    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, color_compressed_sub_);
+    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, color_sub_);
    sync_->registerCallback(std::bind(&DepthImageRos2Node::syncCallback, this, 
                                     std::placeholders::_1, std::placeholders::_2));

@@ -94,7 +97,8 @@ PointCloudToDepthConverter::CameraParams DepthImageRos2Node::loadCameraParams()
 }

 void DepthImageRos2Node::syncCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg,
-                                     const sensor_msgs::msg::CompressedImage::ConstSharedPtr image_msg)
+                                     // const sensor_msgs::msg::CompressedImage::ConstSharedPtr image_msg,
+                                     const sensor_msgs::msg::Image::ConstSharedPtr color_msg)
 {
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(*cloud_msg, cloud);
@@ -107,7 +111,9 @@ void DepthImageRos2Node::syncCallback(const sensor_msgs::msg::PointCloud2::Const
    cv::Mat img_raw;
    try
    {
-        img_raw = cv::imdecode(cv::Mat(image_msg->data), cv::IMREAD_COLOR);
+        // img_raw = cv::imdecode(cv::Mat(image_msg->data), cv::IMREAD_COLOR);
+        cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(color_msg, "bgr8");
+        img_raw = cv_ptr->image;
        if (img_raw.empty())
        {
            RCLCPP_WARN(this->get_logger(), "Failed to decode compressed image");
@@ -8,19 +8,22 @@ DepthImageRosNode::DepthImageRosNode(ros::NodeHandle &nh, ros::NodeHandle &pnh)

    depth_converter_ = std::make_unique<PointCloudToDepthConverter>(camera_params);
    pnh_.param<std::string>("cloud_raw_topic", cloud_raw_topic_, std::string("/odin1/cloud_raw"));
+    pnh_.param<std::string>("color_raw_topic", color_raw_topic_, std::string("/odin1/image"));
    pnh_.param<std::string>("color_compressed_topic_", color_compressed_topic_, std::string("/odin1/image/compressed"));
    pnh_.param<std::string>("depth_image_topic", depth_image_topic_, std::string("/odin1/depth_img_competetion"));
    pnh_.param<std::string>("depth_cloud_topic", depth_cloud_topic_, std::string("/odin1/depth_img_competetion_cloud"));

    ROS_INFO_STREAM("\n  cloud_raw_topic: " << cloud_raw_topic_
-                                            << "\n  color_compressed_topic: " << color_compressed_topic_
-                                            << "\n  depth_image_topic: " << depth_image_topic_
-                                            << "\n  depth_cloud_topic: " << depth_cloud_topic_);
+                << "\n  color_raw_topic: " << color_raw_topic_
+                << "\n  color_compressed_topic: " << color_compressed_topic_
+                << "\n  depth_image_topic: " << depth_image_topic_
+                << "\n  depth_cloud_topic: " << depth_cloud_topic_);

    cloud_sub_.subscribe(nh_, cloud_raw_topic_, 1);
+    color_sub_.subscribe(nh_, color_raw_topic_, 1);
    color_compressed_sub_.subscribe(nh_, color_compressed_topic_, 1);

-    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, color_compressed_sub_);
+    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, color_sub_);
    sync_->registerCallback(boost::bind(&DepthImageRosNode::syncCallback, this, _1, _2));

    depth_image_pub_ = it_.advertise(depth_image_topic_, 1);
@@ -88,7 +91,7 @@ PointCloudToDepthConverter::CameraParams DepthImageRosNode::loadCameraParams()
 }

 void DepthImageRosNode::syncCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
-                                     const sensor_msgs::CompressedImageConstPtr &image_msg)
+                                     const sensor_msgs::ImageConstPtr &image_msg)
 {
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(*cloud_msg, cloud);
@@ -101,7 +104,9 @@ void DepthImageRosNode::syncCallback(const sensor_msgs::PointCloud2ConstPtr &clo
    cv::Mat img_raw;
    try
    {
-        img_raw = cv::imdecode(cv::Mat(image_msg->data), cv::IMREAD_COLOR);
+        // img_raw = cv::imdecode(cv::Mat(image_msg->data), cv::IMREAD_COLOR);
+        cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
+        img_raw = cv_ptr->image;
        if (img_raw.empty())
        {
            ROS_WARN("Failed to decode compressed image");
@@ -17,11 +17,12 @@
 #include <sys/wait.h>
 #include <signal.h>
 #include <chrono>
+#include <filesystem>
 #include <fstream>
 #include <vector>
 #include <cstdio>
 #include <array>
-#include <yaml-cpp/yaml.h>
+// #include <yaml-cpp/yaml.h>
 #include <iomanip>
 #ifdef ROS2
    #include <ament_index_cpp/get_package_share_directory.hpp>
@@ -30,7 +31,7 @@
    #include <ros/package.h>
    #include <ros/ros.h> 
 #endif
-#define ros_driver_version "0.4.1"
+#define ros_driver_version "0.5.0"
 // Global variable declarations
 static device_handle odinDevice = nullptr;
 static std::atomic<bool> deviceConnected(false);
@@ -67,7 +68,53 @@ int g_sendodom = 1;
 int g_sendcloudslam = 0;
 int g_sendcloudrender = 0;
 int g_sendrgb_compressed = 0;
+int g_sendrgb_undistort = 0;
 int g_record_data = 0;
+int g_devstatus_log = 0;
+
+const char* DEV_STATUS_CSV_FILE = "dev_status.csv";
+FILE* dev_status_csv_file = nullptr;
+
+typedef struct  {
+    struct timespec start = {0, 0};
+    double frame_count = 0.0;
+    std::atomic<double> fps;
+} fpsHandle;
+
+static void sensor_fps(fpsHandle* handle, const char* name, bool print = false) 
+{
+    struct timespec now;
+    clock_gettime(CLOCK_MONOTONIC, &now);
+
+    if (handle->start.tv_sec == 0 && handle->start.tv_nsec == 0) {
+        handle->start = now;
+    }
+
+    handle->frame_count += 1.0;
+
+    double elapsed = (now.tv_sec - handle->start.tv_sec)
+                   + (now.tv_nsec - handle->start.tv_nsec) / 1e9;
+
+    if (elapsed >= 1.0) {
+        handle->fps.store(handle->frame_count / elapsed);
+        if (print) {
+            #ifdef ROS2
+                RCLCPP_INFO(rclcpp::get_logger("device_cb"), "%s FPS: %f", name, handle->fps.load());
+            #else
+                ROS_INFO("%s FPS: %f", name, handle->fps.load());
+            #endif
+        }
+        handle->frame_count = 0;
+        handle->start = now;
+    }
+}
+
+static fpsHandle rgb_rx_fps;
+static fpsHandle dtof_rx_fps;
+static fpsHandle imu_rx_fps;
+static fpsHandle slam_cloud_rx_fps;
+static fpsHandle slam_odom_rx_fps;
+static fpsHandle slam_odom_highfreq_rx_fps;

 class RosNodeControlImpl : public RosNodeControlInterface {
    public:
@@ -353,6 +400,8 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
        printf("Invalid device handle or data.\n");
        return;
    }
+    imu_convert_data_t *imudata = nullptr;
+    lidar_device_status_t *dev_info_data;
    
    switch(data->type) {
        case LIDAR_DT_NONE:
@@ -362,25 +411,166 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
            if (g_sendrgb) {
                g_ros_object->publishRgb((capture_Image_List_t *)&data->stream);
            }
+            sensor_fps(&rgb_rx_fps, "rgb_rx");
            break;
        case LIDAR_DT_RAW_IMU:
            if (g_sendimu) {
-                g_ros_object->publishImu((icm_6aixs_data_t *)data->stream.imageList[0].pAddr);
+                imudata = (imu_convert_data_t *)data->stream.imageList[0].pAddr;
+                g_ros_object->publishImu(imudata);
            }
+            sensor_fps(&imu_rx_fps, "imu_rx");
            break;
        case LIDAR_DT_RAW_DTOF:
-        if (g_senddtof ) {
-            g_ros_object->publishIntensityCloud((capture_Image_List_t *)&data->stream, 1);
+            if (g_senddtof ) {
+                g_ros_object->publishIntensityCloud((capture_Image_List_t *)&data->stream, 1);
            }
-          break;
+            sensor_fps(&dtof_rx_fps, "dtof_rx");
+            break;
        case LIDAR_DT_SLAM_CLOUD:
            if (g_sendcloudslam) {
                g_ros_object->publishPC2XYZRGBA((capture_Image_List_t *)&data->stream, 0);
            }
+            sensor_fps(&slam_cloud_rx_fps, "slam_cloud_rx");
            break;
        case LIDAR_DT_SLAM_ODOMETRY:
            if (g_sendodom) {
-                g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream);
+                g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, false);
+            }
+            sensor_fps(&slam_odom_rx_fps, "slam_odom_rx");
+            break;
+        case LIDAR_DT_DEV_STATUS:
+            dev_info_data = (lidar_device_status_t *)data->stream.imageList[0].pAddr;
+            if (g_devstatus_log) {
+                if (dev_status_csv_file) {
+                    // append the data row
+                    int rc = 0;
+                    rc = std::fprintf(dev_status_csv_file, "%d,%d,%d,%d,%d,%d,", // %.0f
+                                            // get_uptime_seconds(),
+                                            0,
+                                            dev_info_data->soc_thermal.package_temp,
+                                            dev_info_data->soc_thermal.cpu_temp,
+                                            dev_info_data->soc_thermal.center_temp,
+                                            dev_info_data->soc_thermal.gpu_temp,
+                                            dev_info_data->soc_thermal.npu_temp);
+                    if (rc < 0) { 
+                        printf("Failed to write to dev_status_csv_file\n");
+                    }
+
+                    rc = std::fprintf(dev_status_csv_file, "%d,%d,", 
+                        dev_info_data->dtof_sensor.tx_temp,
+                        dev_info_data->dtof_sensor.rx_temp);
+                    if (rc < 0) { 
+                        printf("Failed to write to dev_status_csv_file\n");
+                    }
+
+                    for (int i = 0; i < 8; i++) {
+                        rc = std::fprintf(dev_status_csv_file, "%d,", dev_info_data->cpu_use_rate[i]);
+                    }
+                    rc = std::fprintf(dev_status_csv_file, "%d,", dev_info_data->ram_use_rate);
+
+                    rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,", 
+                        ((float)dev_info_data->rgb_sensor.configured_odr)/1000,
+                        ((float)dev_info_data->rgb_sensor.tx_odr)/1000,
+                        (rgb_rx_fps.fps.load()));
+                    if (rc < 0) { 
+                        printf("Failed to write to dev_status_csv_file\n");
+                    }
+
+                    rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,",
+                        ((float)dev_info_data->dtof_sensor.configured_odr)/1000,
+                        ((float)dev_info_data->dtof_sensor.tx_odr)/1000,
+                        (dtof_rx_fps.fps.load()));
+                    if (rc < 0) { 
+                        printf("Failed to write to dev_status_csv_file\n");
+                    }
+
+                    rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,",
+                        ((float)dev_info_data->imu_sensor.configured_odr)/1000,
+                        ((float)dev_info_data->imu_sensor.tx_odr)/1000,
+                        (imu_rx_fps.fps.load()));
+                    if (rc < 0) { 
+                        printf("Failed to write to dev_status_csv_file\n");
+                    }
+
+                    rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", 
+                        ((float)dev_info_data->slam_cloud_tx_odr)/1000,
+                        (slam_cloud_rx_fps.fps.load()),
+                        ((float)dev_info_data->slam_odom_tx_odr)/1000,
+                        (slam_odom_rx_fps.fps.load()),
+                        ((float)dev_info_data->slam_odom_highfreq_tx_odr)/1000,
+                        (slam_odom_highfreq_rx_fps.fps.load()));
+                    if (rc < 0) { 
+                        printf("Failed to write to dev_status_csv_file\n");
+                    }
+
+                    std::fflush(dev_status_csv_file);
+                }
+            }
+            if (g_show_fps) {
+                printf("\n [dev_info] [soc_thermal]: package_temp:%dC \n", 
+                    dev_info_data->soc_thermal.package_temp);
+                printf("\n [dev_info] [soc_thermal]: cpu:%dC \n",
+                    dev_info_data->soc_thermal.cpu_temp);                
+                printf("\n [dev_info] [soc_thermal]: center_temp:%dC \n",
+                    dev_info_data->soc_thermal.center_temp);
+                printf("\n [dev_info] [soc_thermal]: gpu_temp:%dC \n",
+                    dev_info_data->soc_thermal.gpu_temp);
+                printf("\n [dev_info] [soc_thermal]: npu_temp:%dC \n",
+                    dev_info_data->soc_thermal.npu_temp);
+
+                for ( int i=0;i<8;i++) 
+                {
+                    printf("\n [dev_info] [cpu]: cpu_use_rate-core[%d]:%d%% \n",
+                                                    i,
+                                                    dev_info_data->cpu_use_rate[i]);
+
+                }
+                printf("\n [dev_info] [cpu]: ram_use_rate:%d%% \n",
+                    dev_info_data->ram_use_rate);
+
+                printf("\n [dev_info] [rgb]: configured_odr: %.2f HZ, tx_odr: %.2f HZ, rx_odr: %.2f HZ \n", 
+                    ((float)dev_info_data->rgb_sensor.configured_odr)/1000,
+                    ((float)dev_info_data->rgb_sensor.tx_odr)/1000,
+                    (rgb_rx_fps.fps.load()));
+
+                printf("\n [dev_info] [dtof]: configured_odr: %.2f HZ, tx_odr: %.2f HZ, rx_odr: %.2f HZ \n", 
+                    ((float)dev_info_data->dtof_sensor.configured_odr)/1000,
+                    ((float)dev_info_data->dtof_sensor.tx_odr)/1000,
+                    (dtof_rx_fps.fps.load()));
+                printf("\n [dev_info] [dtof]: subframe_odr: %.2f \n", 
+                    ((float)dev_info_data->dtof_sensor.subframe_odr)/1000);
+                printf("\n [dev_info] [dtof]: txtemp:%dC, rxtemp:%dC \n", dev_info_data->dtof_sensor.tx_temp, dev_info_data->dtof_sensor.rx_temp);
+
+                printf("\n [dev_info] [imu]: configured_odr: %.2f HZ, tx_odr: %.2f HZ, rx_odr: %.2f HZ\n",
+                    ((float) dev_info_data->imu_sensor.configured_odr)/1000,
+                    ((float) dev_info_data->imu_sensor.tx_odr)/1000,
+                    (imu_rx_fps.fps.load())
+                );
+
+                printf("\n [dev_info] [slam]: slam_cloud_tx_odr: %.2f HZ, rx_odr: %.2f HZ \n",
+                    ((float)dev_info_data->slam_cloud_tx_odr)/1000,
+                    (slam_cloud_rx_fps.fps.load())
+                );
+
+                printf("\n [dev_info] [slam]: slam_odom_tx_odr: %.2f HZ, rx_odr: %.2f HZ \n",
+                    ((float)dev_info_data->slam_odom_tx_odr)/1000,
+                    (slam_odom_rx_fps.fps.load())
+                );
+
+                printf("\n [dev_info] [slam]: slam_odom_highfreq_tx_odr: %.2f HZ, rx_odr: %.2f HZ \n",
+                    ((float)dev_info_data->slam_odom_highfreq_tx_odr)/1000,
+                    (slam_odom_highfreq_rx_fps.fps.load())
+                );
+
+                printf("\n------------------------------------------\n");
+            }
+            break;
+            case LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ:
+            {
+                if (g_sendodom) {
+                    g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, true);
+                }
+                sensor_fps(&slam_odom_highfreq_rx_fps, "slam_odom_highfreq_rx");
            }
            break;
        default:
@@ -392,6 +582,7 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
 static void lidar_device_callback(const lidar_device_info_t* device, bool attach)
 {
    int type = LIDAR_MODE_SLAM;
+    // int type = LIDAR_MODE_RAW;
    static std::chrono::steady_clock::time_point software_connect_start; 
    static bool software_connect_timing = false; 
    
@@ -488,7 +679,13 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
        }
        
        if(lidar_get_version(odinDevice)) {
-            printf("get version failed.\n");
+            #ifdef ROS2
+                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get device firmware version, potential incompatible, please upgrade device firmware and retry.");
+            #else
+                ROS_ERROR("Failed to get device firmware version, potential incompatible, please upgrade device firmware and retry.");
+            #endif
+            system("pkill -f rviz");
+            exit(1);
        }
        else {
            printf("ros_driver_version:%s\n", ros_driver_version);
@@ -565,7 +762,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            odinDevice = nullptr;
            return;
        }
-        
+
        uint32_t dtof_subframe_odr = 0;
        if (lidar_start_stream(odinDevice, type, dtof_subframe_odr)) {
            #ifdef ROS2
@@ -603,6 +800,10 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
        deviceConnected = true;
        deviceDisconnected = false; 
        
+        if (g_sendrgb_undistort && g_ros_object->loadCameraParams(calib_config) == 0) {
+            g_ros_object->buildUndistortMap();
+        }
+
        #ifdef ROS2
            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Software connection successful in %ld seconds", 
                       std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now() - software_connect_start).count());
@@ -631,6 +832,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
        #endif
    }
 }
+
 int main(int argc, char *argv[])
 {
 #ifdef ROS2
@@ -677,33 +879,76 @@ int main(int argc, char *argv[])
        g_sendcloudslam = get_key_value("sendcloudslam", 0);
        g_sendcloudrender = get_key_value("sendcloudrender", 1);
        g_sendrgb_compressed = get_key_value("sendrgbcompressed", 1);
+        g_sendrgb_undistort = get_key_value("sendrgbundistort", 0);
        g_record_data = get_key_value("recorddata", 0);
+        g_show_fps = get_key_value("showfps", 0);
+        g_devstatus_log = get_key_value("devstatuslog", 0);
        g_log_level = get_key_value("log_devel", LOG_LEVEL_INFO);
        lidar_log_set_level(LIDAR_LOG_INFO);

-        if (g_record_data) {
-            const std::string package_name = "odin_ros_driver";
-            std::string data_dir = "";
-            #ifdef ROS2
-                char* ros_workspace = std::getenv("COLCON_PREFIX_PATH");
-                if (ros_workspace) {
-                std::string workspace_path(ros_workspace);
-                size_t pos = workspace_path.find("/install");
-                if (pos != std::string::npos) {
-                    data_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/recorddata";
-                } else {
-                    data_dir = ament_index_cpp::get_package_share_directory(package_name) + "/recorddata";
-                }
-                } else {
+        const std::string package_name = "odin_ros_driver";
+        std::string data_dir = "";
+        std::string log_dir = "";
+        #ifdef ROS2
+            char* ros_workspace = std::getenv("COLCON_PREFIX_PATH");
+            if (ros_workspace) {
+            std::string workspace_path(ros_workspace);
+            size_t pos = workspace_path.find("/install");
+            if (pos != std::string::npos) {
+                data_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/recorddata";
+                log_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/log";
+            } else {
                data_dir = ament_index_cpp::get_package_share_directory(package_name) + "/recorddata";
-                }
-            #else
-                data_dir = ros::package::getPath(package_name) + "/recorddata";
-            #endif
+                log_dir = ament_index_cpp::get_package_share_directory(package_name) + "/log";
+            }
+            } else {
+            data_dir = ament_index_cpp::get_package_share_directory(package_name) + "/recorddata";
+            log_dir = ament_index_cpp::get_package_share_directory(package_name) + "/log";
+            }
+        #else
+            data_dir = ros::package::getPath(package_name) + "/recorddata";
+            log_dir = ros::package::getPath(package_name) + "/log";
+        #endif

+        if (g_record_data) {
            g_ros_object->initialize_data_logger(data_dir);
        }

+        if (g_devstatus_log) {
+            auto now = std::chrono::system_clock::now();
+            std::time_t t = std::chrono::system_clock::to_time_t(now);
+            std::tm tm{};
+            #ifdef _WIN32
+                localtime_s(&tm, &t);
+            #else
+                localtime_r(&t, &tm);
+            #endif
+            char buf[32];
+            std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm);
+
+            std::filesystem::path log_root_dir_ = std::filesystem::path(log_dir) / buf;
+            std::filesystem::create_directories(log_root_dir_);
+            std::string dev_status_csv_file_path_ = log_root_dir_ / "dev_status.csv";
+
+            // Open the file in append mode
+            dev_status_csv_file = fopen(dev_status_csv_file_path_.c_str(), "a");
+            if (!dev_status_csv_file) {
+                #ifdef ROS2
+                    RCLCPP_ERROR(rclcpp::get_logger("init"), "Failed to open dev_status CSV file");
+                #else
+                    ROS_ERROR("Failed to open dev_status CSV file");
+                #endif
+            } else {
+                const char* header =
+                "uptime_seconds,package_temp,cpu_temp,center_temp,gpu_temp,npu_temp,dtof_tx_temp,dtof_rx_temp,"
+                "cpu0,cpu1,cpu2,cpu3,cpu4,cpu5,cpu6,cpu7,ram_use,"
+                "rgb_configured_odr,rgb_tx_odr,rgb_rx_odr,dtof_configured_odr,dtof_tx_odr,dtof_rx_odr,imu_configured_odr,imu_tx_odr,imu_rx_odr,"
+                "slam_cloud_tx_odr,slam_cloud_rx_odr,slam_odom_tx_odr,slam_odom_rx_odr,slam_odom_highfreq_tx_odr,slam_odom_highfreq_rx_odr\n";
+                fprintf(dev_status_csv_file, "%s", header);
+                std::fflush(dev_status_csv_file);
+            }
+        }
+
        if (lidar_system_init(lidar_device_callback)) {
            #ifdef ROS2
                RCLCPP_ERROR(node->get_logger(), "Lidar system init failed");
@@ -717,6 +962,17 @@ int main(int argc, char *argv[])
        bool usbPresent = false;
        bool usbVersionChecked = false; 
        while (!deviceConnected) {
+            #ifdef ROS2
+            if (!rclcpp::ok()) {
+                break;
+            }
+            #else
+            if (!ros::ok())     // ROS1 shutdown check
+            {
+                break;
+            }
+            #endif
+
            usbPresent = isUsbDevicePresent(TARGET_VENDOR, TARGET_PRODUCT); 
            if (usbPresent) { 
                if (!usbVersionChecked) {
@@ -752,19 +1008,36 @@ int main(int argc, char *argv[])
                return -1;
    }

+    if (!deviceConnected) {
+        #ifdef ROS2
+        if (g_ros_object) {
+            g_ros_object.reset();   // destroys all publishers/subscribers
+        }
+        node.reset();              // destroy the node first
+        rclcpp::shutdown();
+        #else
+        if (g_ros_object) {
+            delete g_ros_object;
+            g_ros_object = nullptr;
+        }
+        ros::shutdown();
+        #endif
+        return 1;
+    }
+
+    bool disconnect_msg_printed = false;
    #ifdef ROS2
        // Create 10Hz Rate object
        rclcpp::Rate rate(10);
+        
        while (rclcpp::ok()) {
            rclcpp::spin_some(node);
-
            // Check device disconnection status
            if (deviceDisconnected.load()) {
-                #ifdef ROS2
+                if (!disconnect_msg_printed) {
                    RCLCPP_INFO(node->get_logger(), "Device disconnected, waiting for reconnection...");
-                #else
-                    ROS_INFO("Device disconnected, waiting for reconnection...");
-                #endif
+                    disconnect_msg_printed = true;
+                }
                
                // Wait 0.1 seconds
                rate.sleep();
@@ -775,6 +1048,7 @@ int main(int argc, char *argv[])
            if (g_sendcloudrender) {
                g_ros_object->try_process_pair();  
            }
+            disconnect_msg_printed = false;

            // Wait 0.1 seconds
            rate.sleep();
@@ -788,7 +1062,10 @@ int main(int argc, char *argv[])

            // Check device disconnection status
            if (deviceDisconnected.load()) {
-                ROS_INFO("Device disconnected, waiting for reconnection...");
+                if (!disconnect_msg_printed) {
+                    ROS_INFO("Device disconnected, waiting for reconnection...");
+                    disconnect_msg_printed = true;
+                }
                
                // Wait 0.1 seconds
                rate.sleep();
@@ -799,6 +1076,7 @@ int main(int argc, char *argv[])
            if (g_sendcloudrender) {
                g_ros_object->try_process_pair();  
            }
+            disconnect_msg_printed = false;

            // Wait 0.1 seconds
            rate.sleep();
@@ -823,10 +1101,28 @@ int main(int argc, char *argv[])
            ROS_INFO("image_index: %d", g_ros_object->get_image_index());
        #endif
        // Perform cleanup on normal exit
-        // lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM);
-        // lidar_unregister_stream_callback(odinDevice);
+        // if(lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM))
+        // {
+        //     #ifdef ROS2
+        //         RCLCPP_INFO(rclcpp::get_logger("device_cb"), "lidar_stop_stream failed");
+        //     #else
+        //         ROS_INFO("lidar_stop_stream failed");
+        //     #endif
+        // }
+        
+        if(lidar_unregister_stream_callback(odinDevice))
+        {
+            #ifdef ROS2
+                RCLCPP_INFO(rclcpp::get_logger("device_cb"), "lidar_unregister_stream_callback failed");
+            #else
+                ROS_INFO("lidar_unregister_stream_callback failed");
+            #endif
+        }
        // lidar_close_device(odinDevice);
        // lidar_destory_device(odinDevice);
+
+        std::fflush(dev_status_csv_file);
+        fclose(dev_status_csv_file);
    }
    // lidar_system_deinit();