diff --git a/.gitignore b/.gitignore index ce11a82..c6888f7 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ -recorddata/ \ No newline at end of file +recorddata/ +/config/calib.yaml +/log \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 420a73e..b8236c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/README.md b/README.md index f4cc2d8..542dc6a 100644 --- a/README.md +++ b/README.md @@ -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_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 +: 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: diff --git a/config/control_command.yaml b/config/control_command.yaml index 21e8235..2b1c002 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -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 diff --git a/config/odin_ros.rviz b/config/odin_ros.rviz index bd6b87c..12d7107 100644 --- a/config/odin_ros.rviz +++ b/config/odin_ros.rviz @@ -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: - 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 \ No newline at end of file diff --git a/config/odin_ros2.rviz b/config/odin_ros2.rviz index 6d0c3c1..15edc8c 100644 --- a/config/odin_ros2.rviz +++ b/config/odin_ros2.rviz @@ -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: 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 \ No newline at end of file diff --git a/include/data_logger.h b/include/data_logger.h index 89ff832..1199411 100644 --- a/include/data_logger.h +++ b/include/data_logger.h @@ -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 diff --git a/include/depth_image_ros2_node.hpp b/include/depth_image_ros2_node.hpp index 26d9a7f..64b80f6 100644 --- a/include/depth_image_ros2_node.hpp +++ b/include/depth_image_ros2_node.hpp @@ -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 cloud_sub_; message_filters::Subscriber color_compressed_sub_; + message_filters::Subscriber 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 Sync; std::shared_ptr 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, diff --git a/include/depth_image_ros_node.hpp b/include/depth_image_ros_node.hpp index 84f0105..fb1a816 100644 --- a/include/depth_image_ros_node.hpp +++ b/include/depth_image_ros_node.hpp @@ -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 cloud_sub_; + message_filters::Subscriber color_sub_; message_filters::Subscriber color_compressed_sub_; - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; typedef message_filters::Synchronizer Sync; std::shared_ptr 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, diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h index 11675dd..39e0582 100644 --- a/include/host_sdk_sample.h +++ b/include/host_sdk_sample.h @@ -43,6 +43,15 @@ limitations under the License. #include #include +#include +#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(accel_convert(stream->aacx, ACC_SEN_SCALE)); - imu_msg.linear_acceleration.x = static_cast(accel_convert(stream->aacy, ACC_SEN_SCALE)); - imu_msg.linear_acceleration.z = static_cast(accel_convert(stream->aacz, ACC_SEN_SCALE)); - - imu_msg.angular_velocity.y = -1 * static_cast(gyro_convert(stream->gyrox, GYRO_SEN_SCALE)); - imu_msg.angular_velocity.x = static_cast(gyro_convert(stream->gyroy, GYRO_SEN_SCALE)); - imu_msg.angular_velocity.z = static_cast(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(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(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(); - 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(*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(image.timestamp + 719060) / 1e9; + const double ts_sec = static_cast(image.timestamp) / 1e9; const uint32_t jpeg_size = static_cast(compressed_msg->data.size()); std::vector 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(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(stream->imageList[0].timestamp + 719060) / 1e9; + const double ts_sec = static_cast(stream->imageList[0].timestamp) / 1e9; const uint32_t jpeg_size = static_cast(jpeg_data.size()); std::vector 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(); + m_camera_params.height = cam_node["image_height"].as(); + + double A11 = cam_node["A11"].as(); + double A12 = cam_node["A12"].as(); + double A22 = cam_node["A22"].as(); + double u0 = cam_node["u0"].as(); + double v0 = cam_node["v0"].as(); + + 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(); + m_camera_params.k3 = cam_node["k3"].as(); + m_camera_params.k4 = cam_node["k4"].as(); + m_camera_params.k5 = cam_node["k5"].as(); + m_camera_params.k6 = cam_node["k6"].as(); + m_camera_params.k7 = cam_node["k7"].as(); + m_camera_params.p1 = cam_node["p1"].as(); + m_camera_params.p2 = cam_node["p2"].as(); +#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(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(v_out, u_out) = static_cast(distorted_pixel[0]); + m_undistort_map_y.at(v_out, u_out) = static_cast(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 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 getIntensityCloudQueueSnapshot() { std::lock_guard lock(pcd_queue_mutex_); @@ -1164,8 +1266,10 @@ private: cloud_pub_ = node_->create_publisher("odin1/cloud_raw", 10); xyzrgbacloud_pub_ = node_->create_publisher("odin1/cloud_slam", 10); odom_publisher_ = node_->create_publisher("odin1/odometry", 10); + odom_highfreq_publisher_ = node_->create_publisher("odin1/odometry_highfreq", 10); rgbcloud_pub_ = node_->create_publisher("odin1/cloud_render", 10); compressed_rgb_pub_ = node_->create_publisher("odin1/image/compressed", 10); + undistort_rgb_pub_ = node_->create_publisher("odin1/image/undistorted", 10); #endif } #ifdef ROS1 @@ -1175,8 +1279,10 @@ private: cloud_pub_ = nh.advertise("odin1/cloud_raw", 10); xyzrgbacloud_pub_ = nh.advertise("odin1/cloud_slam", 10); odom_publisher_ = nh.advertise("odin1/odometry", 10); + odom_highfreq_publisher_ = nh.advertise("odin1/odometry_highfreq", 10); rgbcloud_pub_ = nh.advertise("odin1/cloud_render", 10); compressed_rgb_pub_ = nh.advertise("odin1/image/compressed", 10); + undistort_rgb_pub_ = nh.advertise("odin1/image/undistorted", 10); } #endif @@ -1187,20 +1293,24 @@ private: rclcpp::Publisher::SharedPtr cloud_pub_; rclcpp::Publisher::SharedPtr xyzrgbacloud_pub_; rclcpp::Publisher::SharedPtr odom_publisher_; + rclcpp::Publisher::SharedPtr odom_highfreq_publisher_; rclcpp::Publisher::SharedPtr rendered_cloud_pub_; rclcpp::Publisher::SharedPtr rgbcloud_pub_; rclcpp::Publisher::SharedPtr rgbFromnv12_pub_; rclcpp::Publisher::SharedPtr compressed_rgb_pub_; // New compressed image publisher + rclcpp::Publisher::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 }; diff --git a/include/lidar_api_type.h b/include/lidar_api_type.h index a37006b..20039b6 100644 --- a/include/lidar_api_type.h +++ b/include/lidar_api_type.h @@ -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 diff --git a/include/polynomial_camera.hpp b/include/polynomial_camera.hpp new file mode 100644 index 0000000..1de15cb --- /dev/null +++ b/include/polynomial_camera.hpp @@ -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 +#include + +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 diff --git a/lib/liblydHostApi_amd.a b/lib/liblydHostApi_amd.a index d707ee2..81e9368 100644 Binary files a/lib/liblydHostApi_amd.a and b/lib/liblydHostApi_amd.a differ diff --git a/lib/liblydHostApi_arm.a b/lib/liblydHostApi_arm.a index ed1b302..2bad4fa 100644 Binary files a/lib/liblydHostApi_arm.a and b/lib/liblydHostApi_arm.a differ diff --git a/package.xml b/package.xml new file mode 100755 index 0000000..17f851d --- /dev/null +++ b/package.xml @@ -0,0 +1,29 @@ + + + odin_ros_driver + 0.0.1 + ROS driver for Odin sensor + rlk + Apache 2.0 + + + catkin + + + roscpp + std_msgs + sensor_msgs + nav_msgs + cv_bridge + image_transport + + + eigen + opencv + yaml-cpp + + + + catkin + + diff --git a/src/depth_image_ros2_node.cpp b/src/depth_image_ros2_node.cpp index 6e7b138..82b2492 100644 --- a/src/depth_image_ros2_node.cpp +++ b/src/depth_image_ros2_node.cpp @@ -10,12 +10,14 @@ DepthImageRos2Node::DepthImageRos2Node(const rclcpp::NodeOptions & options) cloud_raw_topic_ = this->declare_parameter("cloud_raw_topic", "/odin1/cloud_raw"); color_compressed_topic_ = this->declare_parameter("color_compressed_topic", "/odin1/image/compressed"); + color_raw_topic_ = this->declare_parameter("color_raw_topic", "/odin1/image"); depth_image_topic_ = this->declare_parameter("depth_image_topic", "/odin1/depth_img_competetion"); depth_cloud_topic_ = this->declare_parameter("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(MySyncPolicy(10), cloud_sub_, color_compressed_sub_); + sync_ = std::make_shared(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 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"); diff --git a/src/depth_image_ros_node.cpp b/src/depth_image_ros_node.cpp index 99d55e2..8979d35 100644 --- a/src/depth_image_ros_node.cpp +++ b/src/depth_image_ros_node.cpp @@ -8,19 +8,22 @@ DepthImageRosNode::DepthImageRosNode(ros::NodeHandle &nh, ros::NodeHandle &pnh) depth_converter_ = std::make_unique(camera_params); pnh_.param("cloud_raw_topic", cloud_raw_topic_, std::string("/odin1/cloud_raw")); + pnh_.param("color_raw_topic", color_raw_topic_, std::string("/odin1/image")); pnh_.param("color_compressed_topic_", color_compressed_topic_, std::string("/odin1/image/compressed")); pnh_.param("depth_image_topic", depth_image_topic_, std::string("/odin1/depth_img_competetion")); pnh_.param("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(MySyncPolicy(10), cloud_sub_, color_compressed_sub_); + sync_ = std::make_shared(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 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"); diff --git a/src/host_sdk_sample.cpp b/src/host_sdk_sample.cpp index d76217d..b29cdd3 100644 --- a/src/host_sdk_sample.cpp +++ b/src/host_sdk_sample.cpp @@ -17,11 +17,12 @@ #include #include #include +#include #include #include #include #include -#include +// #include #include #ifdef ROS2 #include @@ -30,7 +31,7 @@ #include #include #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 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 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::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();