wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

<fix> 1. optimize dense depth demo, now one-to-one with image_undistort

Browse Source 
2. fix per-point time_offset for raw point cloud data
3. optimize how data rx fps is calculated
4. optimize log file structure
This commit is contained in:
mt-lifan
2025-10-21 11:03:31 +08:00
parent bd2f80d82d
commit 28c17e5611
17 changed files with 1084 additions and 290 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+5
View File
@@ -161,6 +161,7 @@ if(ROS_VERSION STREQUAL "ROS1")
src/host_sdk_sample.cpp
src/yaml_parser.cpp
src/rawCloudRender.cpp
src/camera_pose_visualization.cpp
)
target_link_libraries(host_sdk_sample
${catkin_LIBRARIES}
@@ -218,6 +219,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(image_transport REQUIRED)
find_package(pcl_conversions REQUIRED)
@@ -228,6 +230,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
src/host_sdk_sample.cpp
src/yaml_parser.cpp
src/rawCloudRender.cpp
src/camera_pose_visualization.cpp
)
# Link libraries
@@ -244,6 +247,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
std_msgs
sensor_msgs
nav_msgs
visualization_msgs
cv_bridge
image_transport
)
@@ -339,6 +343,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
std_msgs
sensor_msgs
nav_msgs
visualization_msgs
cv_bridge
image_transport
pcl_conversions
README.md
+35 -6
View File
@@ -16,7 +16,7 @@ This driver package provides core functionality for point cloud SLAM application
## 1. Version
Current Version: v0.5.1
Current Version: v0.5.2
## 2. Preparation
@@ -178,7 +178,7 @@ Odin_ROS_Driver/ // ROS1/ROS2 driver package
pointcloud_depth_converter.hpp // pointcloud_depth_convert
config/
control_command.yaml // Control parameter file for driver
calib.yaml //Machine calibration yaml
calib.yaml // Machine calibration yaml，differ for each individual device. Retrieved from the device everytime it connects to ROS driver
launch_ROS1/
odin1_ros1.launch // ROS1 launch file
launch_ROS2/
@@ -188,6 +188,9 @@ Odin_ROS_Driver/ // ROS1/ROS2 driver package
build_ros2.sh // Installation script for ROS2
recorddata/ // holds recorded data that can import into MindCloud
log/ // holds log files
Driver_{timestamp}/ // holds all log folders for each time driver started
Conn_{timestamp}/ // holds all log files for each odin1 device connection
dev_status.csv // device status log file
README.md // Usage instructions
CMakeLists.txt // CMake build file
License // License file
@@ -205,7 +208,7 @@ Internal parameters of the Odin ROS driver are defined in config/control_command
| 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 | sendrgb | RGB Camera Topic, decoded from original jpeg data from device, bgr8 format |
| 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 |
@@ -213,8 +216,8 @@ Internal parameters of the Odin ROS driver are defined in config/control_command
| 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 |
| odin1/depth_img_competetion | senddepth | Dense depth image Topic. Demo, high computing power required. One-to-one with odin1/image_undistort. To utilize the data please directly subscribe to this topic instead of echoing it. Original value is already depth data, no need for further convert. |
| odin1/depth_img_competetion_cloud | senddepth | Dense Depth_Cloud Topic. Demo, high computing power required |
### 4.4 Data format
@@ -225,7 +228,7 @@ 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
float32 offset_time // Time offset relative to the base timestamp unit: s
```
To work with this custom format in PCL, first define the point type:
@@ -257,6 +260,15 @@ Then, you can easily convert a ROS sensor_msgs::PointCloud2 message into a PCL p
pcl::PointCloud<ls_ros::Point> ls_cloud;
pcl::fromROSMsg(*msg, ls_cloud);
```
2. The slam point cloud (cloud_slam) and directly rendered point cloud (cloud_render) has the following fields:
```
float32 x // X axis, in meters
float32 y // Y axis, in meters
float32 z // Z axis, in meters
float32 rgb // RGB value
```
### 4.5 Other functionalities
|control_command.yaml | Detailed Description |
@@ -368,6 +380,23 @@ 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.
### 5.9 ROS Driver died immediately after stream started
**Error Message**
```shell
Device ready and streams activated
[host_sdk_sample-2] process has died ......
```
**Test**
Disable odin1/image with sendrgb = 0 in control_command.yaml and try again. If the driver now works, it is likely that the issue is related to multiple version of opencv is installed on the system.
**Resolution**
Purge the unused version of opencv and maintain a single complete version, then rebuild the driver and try again.
## 6. Contact Information​​
To help diagnose the issue, please provide the following details to our FAE engineer:
config/control_command.yaml
+3 -1
View File
@@ -11,4 +11,6 @@ register_keys:
sendrgbundistort: 0 # 0: off; 1: on
recorddata: 0 # 0: off; 1: on
devstatuslog: 1 # 0: off; 1: on
pubintensitygray: 0 # 0: off; 1: on
showpath: 0 # 0: off; 1: on
showcamerapose: 0 # 0: off; 1: on
config/odin_ros.rviz
+92 -74
View File
@@ -1,14 +1,14 @@
Panels:
- Class: rviz/Displays
Help Height: 138
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Odometry_high1/Shape1
- /Odometry1
- /slam1
- /dense_depth_demo1
Splitter Ratio: 0.4993045926094055
Tree Height: 1052
Tree Height: 549
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -26,7 +26,7 @@ Panels:
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: raw
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
@@ -76,6 +76,64 @@ Visualization Manager:
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: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: raw
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /odin1/cloud_raw
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.8493094444274902
Min Value: -0.13891705870628357
Value: true
Axis: Z
Channel Name: intensity
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: render
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /odin1/cloud_render
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
@@ -146,64 +204,16 @@ Visualization Manager:
Topic: /odin1/odometry_highfreq
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: Intensity
Decay Time: 0
- Class: rviz/MarkerArray
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: Points
Topic: /odin1/cloud_raw
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Marker Topic: /odin1/camera_pose_visual
Name: camera_view
Namespaces:
{}
Queue Size: 100
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.8493094444274902
Min Value: -0.13891705870628357
Value: true
Axis: Z
Channel Name: intensity
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: render
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /odin1/cloud_render
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Name: Odometry
- Class: rviz/Group
Displays:
- Alpha: 1
@@ -246,7 +256,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 5
Enabled: true
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
@@ -261,13 +271,21 @@ Visualization Manager:
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Value: false
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /odin1/path
Name: path
Namespaces:
{}
Queue Size: 100
Value: false
Enabled: false
Name: slam
- Class: rviz/Group
Displays:
- Class: rviz/Image
Enabled: false
Enabled: true
Image Topic: /odin1/depth_img_competetion
Max Value: 1
Median window: 5
@@ -277,7 +295,7 @@ Visualization Manager:
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
@@ -336,7 +354,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 7.223263740539551
Distance: 8.56683349609375
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -344,29 +362,29 @@ Visualization Manager:
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -0.4511165916919708
Y: -0.1217871829867363
Z: 0.8345625996589661
Focal Shape Fixed Size: true
X: 1.4208989143371582
Y: -0.7709289789199829
Z: 0.5377280712127686
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.6153989434242249
Pitch: 0.8353985548019409
Target Frame: <Fixed Frame>
Yaw: 3.0504050254821777
Yaw: 2.530402660369873
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1736
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
Image_undistort:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000001bf0000060afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004e3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000526000001210000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f007200740000000568000000df0000001600ffffff000000010000015f0000060afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000060a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000af80000005efc0100000002fb0000000800540069006d0065010000000000000af8000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007ce0000060a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001bf0000033afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000262000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002a5000000d20000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d00610067006500000001d5000000d70000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f0072007400000002690000010e0000001600ffffff000000010000015f0000033afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d00650100000000000007380000033700fffffffb0000000800540069006d006501000000000000045000000000000000000000040e0000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -375,8 +393,8 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 2808
X: 72
Width: 1848
X: 57
Y: 27
dense_depth_image:
collapsed: false
config/odin_ros2.rviz
+106 -78
View File
@@ -1,17 +1,17 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Image1/Topic1
- /Odometry_high1/Shape1
- /Odometry1
- /slam1
- /dense_depth_demo1
Splitter Ratio: 0.5
Tree Height: 548
Tree Height: 667
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -74,6 +74,76 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /odin1/image/undistorted
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: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 239
Min Color: 0; 0; 0
Min Intensity: 0
Name: raw
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_raw
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.8730175495147705
Min Value: -0.12252448499202728
Value: true
Axis: Z
Channel Name: rgb
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: 2.3509885615147286e-38
Min Color: 0; 0; 0
Min Intensity: 0
Name: render
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_render
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz_common/Group
Displays:
- Angle Tolerance: 0.10000000149011612
Class: rviz_default_plugins/Odometry
Covariance:
@@ -152,74 +222,20 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /odin1/odometry_highfreq
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: Intensity
Decay Time: 0
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 239
Min Color: 0; 0; 0
Min Intensity: 0
Name: raw
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Name: camera_view
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/cloud_raw
Use Fixed Frame: true
Use rainbow: true
Value: /odin1/camera_pose_visual
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.8730175495147705
Min Value: -0.12252448499202728
Value: true
Axis: Z
Channel Name: rgb
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: 2.3509885615147286e-38
Min Color: 0; 0; 0
Min Intensity: 0
Name: render
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_render
Use Fixed Frame: true
Use rainbow: true
Value: true
Name: Odometry
- Class: rviz_common/Group
Displays:
- Alpha: 1
@@ -268,7 +284,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 10
Enabled: true
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
@@ -289,13 +305,25 @@ Visualization Manager:
Value: /odin1/cloud_slam
Use Fixed Frame: true
Use rainbow: true
Value: true
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: path
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/path
Value: false
Enabled: false
Name: slam
- Class: rviz_common/Group
Displays:
- Class: rviz_default_plugins/Image
Enabled: false
Enabled: true
Max Value: 1
Median window: 5
Min Value: 0
@@ -307,7 +335,7 @@ Visualization Manager:
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odin1/depth_img_competetion
Value: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
@@ -390,45 +418,45 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 6.814720153808594
Distance: 9.397579193115234
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.38628554344177246
Y: -1.1808340549468994
Z: 1.2151938676834106
X: 1.1073570251464844
Y: 0.6017969250679016
Z: 0.4314231276512146
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5653982758522034
Pitch: 1.035398006439209
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.0103981494903564
Yaw: 2.960400104522705
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1029
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
Image_undistort:
collapsed: false
QMainWindow State: 000000ff00000000fd000000040000000000000242000003abfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002f2000000f60000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d00610067006500000002cd0000011b0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000031d000000cb0000002800ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003ab000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000423000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001f50000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000031b000000c00000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004280000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1920
X: 498
Y: 112
Width: 1848
X: 392
Y: 165
dense_depth_image:
collapsed: false
include/camera_pose_visualization.h
Executable
+78
View File
@@ -0,0 +1,78 @@
/*
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
#ifdef ROS2
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/color_rgba.hpp>
#include <std_msgs/msg/header.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#else
#include <ros/ros.h>
#include <std_msgs/ColorRGBA.h>
#include <std_msgs/Header.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#endif
#include <Eigen/Dense>
#include <Eigen/Geometry>
class camera_pose_visualization {
public:
std::string m_marker_ns;
camera_pose_visualization(float r, float g, float b, float a);
void setImageBoundaryColor(float r, float g, float b, float a = 1.0);
void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0);
void setScale(double s);
void setLineWidth(double width);
void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);
void reset();
#ifdef ROS2
using ColorRGBA = std_msgs::msg::ColorRGBA;
using Marker = visualization_msgs::msg::Marker;
using MarkerArray = visualization_msgs::msg::MarkerArray;
using Header = std_msgs::msg::Header;
using Publisher = rclcpp::Publisher<MarkerArray>;
#else
using ColorRGBA = std_msgs::ColorRGBA;
using Marker = visualization_msgs::Marker;
using MarkerArray = visualization_msgs::MarkerArray;
using Header = std_msgs::Header;
using Publisher = ros::Publisher;
#endif
void publish_by(Publisher& pub, const Header& header);
void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
private:
std::vector<Marker> m_markers;
ColorRGBA m_image_boundary_color;
ColorRGBA m_optical_center_connector_color;
double m_scale;
double m_line_width;
static const Eigen::Vector3d imlt;
static const Eigen::Vector3d imlb;
static const Eigen::Vector3d imrt;
static const Eigen::Vector3d imrb;
static const Eigen::Vector3d oc ;
static const Eigen::Vector3d lt0 ;
static const Eigen::Vector3d lt1 ;
static const Eigen::Vector3d lt2 ;
};
include/host_sdk_sample.h
+193 -4
View File
@@ -42,9 +42,11 @@ limitations under the License.
#include <unistd.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <filesystem>
#include <yaml-cpp/yaml.h>
#include "polynomial_camera.hpp"
#include "camera_pose_visualization.h"
struct CameraParams {
int width, height;
@@ -67,6 +69,7 @@ extern int g_sendcloudrender;
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <std_msgs/msg/header.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
@@ -74,12 +77,14 @@ extern int g_sendcloudrender;
#include <sensor_msgs/msg/compressed_image.hpp>
#include <builtin_interfaces/msg/time.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <sensor_msgs/msg/point_field.hpp>
namespace ros {
using namespace rclcpp;
using namespace std_msgs::msg;
using namespace sensor_msgs::msg;
using namespace nav_msgs::msg;
using namespace visualization_msgs::msg;
using Time = builtin_interfaces::msg::Time;
}
@@ -99,7 +104,9 @@ extern int g_sendcloudrender;
#include <sensor_msgs/point_cloud2_iterator.h>
#include <sensor_msgs/CompressedImage.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Image.h>
namespace ros {
using namespace ::ros;
using namespace sensor_msgs;
@@ -174,12 +181,13 @@ class MultiSensorPublisher {
public:
#ifdef ROS2
MultiSensorPublisher(rclcpp::Node::SharedPtr node)
: node_(node) {
: node_(node),cameraposevisual_ {1.0f, 0.0f, 0.0f, 1.0f} {
initialize_publishers();
// initialize_data_logger();
}
#else
MultiSensorPublisher(ros::NodeHandle& nh) {
MultiSensorPublisher(ros::NodeHandle& nh)
: cameraposevisual_(1.0f, 0.0f, 0.0f, 1.0f) {
initialize_publishers(nh);
// initialize_data_logger();
}
@@ -573,7 +581,8 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
*iter_confidence = confidence_data[i]; ++iter_confidence;
if (dtof_subframe_odr > 0.0) {
int group = i / DTOF_NUM_ROW_PER_GROUP;
int line_num = i / 256;
int group = line_num / DTOF_NUM_ROW_PER_GROUP;
float timestamp_offset = group * 1.0 / dtof_subframe_odr;
*iter_offsettime = timestamp_offset;
@@ -634,6 +643,33 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
#endif
}
void publishGrayUInt8(capture_Image_List_t *stream, int idx) {
ImageMsg msg;
msg.header.stamp = ns_to_ros_time(stream->imageList[idx].timestamp);
msg.header.frame_id = "map";
int width = stream->imageList[idx].width;
int height = stream->imageList[idx].height;
msg.height = height;
msg.width = width;
msg.encoding = "mono8";
msg.is_bigendian = false;
msg.step = width * sizeof(uint8_t);
size_t image_size = msg.step * height;
msg.data.resize(image_size);
memcpy(msg.data.data(), stream->imageList[idx].pAddr, image_size);
#ifdef ROS2
intensity_gray_pub_->publish(msg);
#else
intensity_gray_pub_.publish(msg);
#endif
}
void publishRgb(capture_Image_List_t *stream) {
buffer_List_t &image = stream->imageList[0];
@@ -991,7 +1027,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
#endif
}
void publishOdometry(capture_Image_List_t* stream, bool is_highfreq) {
void publishOdometry(capture_Image_List_t* stream, bool is_highfreq, bool show_path, bool show_camerapose) {
#ifdef ROS2
auto msg = nav_msgs::msg::Odometry();
@@ -1080,12 +1116,123 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
odom_highfreq_publisher_->publish(std::move(msg));
} else {
odom_publisher_->publish(std::move(msg));
// Publish odom trajectory as visualization markers (green lines connecting adjacent points)
static visualization_msgs::msg::Marker marker;
static std::vector<geometry_msgs::msg::Point> path_points;
if (show_path) {
marker.header = msg.header;
marker.ns = "odom_trajectory";
marker.id = 0;
marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.02; // Line width
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.color.a = 1.0;
geometry_msgs::msg::Point pt;
pt.x = msg.pose.pose.position.x;
pt.y = msg.pose.pose.position.y;
pt.z = msg.pose.pose.position.z;
path_points.push_back(pt);
// Keep only recent points to avoid memory issues (e.g., last 1000 points)
if (path_points.size() > 30000) {
path_points.erase(path_points.begin());
}
marker.points = path_points;
// Publish marker array
static visualization_msgs::msg::MarkerArray marker_array;
marker_array.markers.clear(); // Clear previous markers
marker_array.markers.push_back(marker);
path_publisher_->publish(marker_array);
}
if (show_camerapose) {
// camera pose visualization (ROS2)
Eigen::Vector3d P(msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z);
Eigen::Quaterniond R(msg.pose.pose.orientation.w,
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z);
if (extrinsic_ok_) {
P = P + R * t_ic_;
R = R * R_ic_;
}
cameraposevisual_.reset();
cameraposevisual_.add_pose(P, R);
cameraposevisual_.publish_by(*pub_camera_pose_visual_, msg.header);
}
}
#else
if (is_highfreq) {
odom_highfreq_publisher_.publish(msg);
} else {
odom_publisher_.publish(msg);
if (show_path) {
// Publish odom trajectory as visualization markers (green lines connecting adjacent points)
static visualization_msgs::Marker marker;
static std::vector<geometry_msgs::Point> path_points;
marker.header = msg.header;
marker.ns = "odom_trajectory";
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.02; // Line width
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.color.a = 1.0;
geometry_msgs::Point pt;
pt.x = msg.pose.pose.position.x;
pt.y = msg.pose.pose.position.y;
pt.z = msg.pose.pose.position.z;
path_points.push_back(pt);
// Keep only recent points to avoid memory issues (e.g., last 1000 points)
if (path_points.size() > 30000) {
path_points.erase(path_points.begin());
}
marker.points = path_points;
// Publish marker array
static visualization_msgs::MarkerArray marker_array;
marker_array.markers.clear(); // Clear previous markers
marker_array.markers.push_back(marker);
path_publisher_.publish(marker_array);
}
if (show_camerapose) {
// camera pose visualization (ROS1)
Eigen::Vector3d P(msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z);
Eigen::Quaterniond R(msg.pose.pose.orientation.w,
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z);
if (extrinsic_ok_) {
P = P + R * t_ic_;
R = R * R_ic_;
}
cameraposevisual_.reset();
cameraposevisual_.add_pose(P, R);
cameraposevisual_.publish_by(pub_camera_pose_visual_, msg.header);
}
}
#endif
}
@@ -1153,6 +1300,25 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
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);
// Load extrinsic Tcl_0 (camera to lidar)
extrinsic_ok_ = false;
if (config["Tcl_0"]) {
auto T = config["Tcl_0"];
if (T.IsSequence() && T.size() == 16) {
Eigen::Matrix4d Tcl;
for (int i = 0; i < 16; ++i) Tcl(i/4, i%4) = T[i].as<double>();
T_cl_ = Tcl;
}
std::cout << "Tcl: " << T_cl_ << std::endl;
Eigen::Matrix4d Tic = T_il_ * T_cl_.inverse();
Eigen::Matrix3d Ric = Tic.block<3,3>(0,0);
Eigen::Vector3d tic = Tic.block<3,1>(0,3);
R_ic_ = Eigen::Quaterniond(Ric);
t_ic_ = tic;
extrinsic_ok_ = true;
}
m_cam_init_success = true;
return 0;
} catch (const std::exception& e) {
@@ -1235,6 +1401,15 @@ private:
bool m_undistort_map_init_success = false;
cv::Mat m_undistort_map_x;
cv::Mat m_undistort_map_y;
Eigen::Quaterniond R_ic_ {Eigen::Quaterniond::Identity()};
Eigen::Vector3d t_ic_ {Eigen::Vector3d::Zero()};
bool extrinsic_ok_ {false};
Eigen::Matrix4d T_il_ = (Eigen::Matrix4d() <<
1.0, 0.0, 0.0, 0.00347,
0.0, 1.0, 0.0, 0.03447,
0.0, 0.0, 1.0, 0.02174,
0.0, 0.0, 0.0, 1.0).finished();
Eigen::Matrix4d T_cl_ = Eigen::Matrix4d::Identity(); // Camera->Lidar from YAML
#ifdef ROS2
std::vector<sensor_msgs::msg::PointCloud2> getIntensityCloudQueueSnapshot() {
std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
@@ -1267,9 +1442,12 @@ private:
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);
path_publisher_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/path", 10);
pub_camera_pose_visual_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/camera_pose_visual", 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);
intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/intensity_gray", 10);
#endif
}
#ifdef ROS1
@@ -1280,9 +1458,12 @@ private:
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);
path_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/path", 10);
pub_camera_pose_visual_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/camera_pose_visual", 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);
intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 10);
}
#endif
@@ -1294,11 +1475,15 @@ private:
rclcpp::Publisher<ros::PointCloud2>::SharedPtr xyzrgbacloud_pub_;
rclcpp::Publisher<ros::Odometry>::SharedPtr odom_publisher_;
rclcpp::Publisher<ros::Odometry>::SharedPtr odom_highfreq_publisher_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr path_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_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr intensity_gray_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_camera_pose_visual_;
camera_pose_visualization cameraposevisual_;
#else
ros::Publisher imu_pub_;
ros::Publisher rgb_pub_;
@@ -1306,11 +1491,15 @@ private:
ros::Publisher xyzrgbacloud_pub_;
ros::Publisher odom_publisher_;
ros::Publisher odom_highfreq_publisher_;
ros::Publisher path_publisher_;
ros::Publisher pub_camera_pose_visual_;
camera_pose_visualization cameraposevisual_;
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_;
ros::Publisher intensity_gray_pub_;
#endif
};
include/pointcloud_depth_converter.hpp
-1
View File
@@ -69,7 +69,6 @@ private:
cv::Mat map_x_, map_y_;
cv::Mat inv_map_x_, inv_map_y_;
cv::Mat depth_undistorted_;
int scaled_width_, scaled_height_;
src/camera_pose_visualization.cpp
Executable
+232
View File
@@ -0,0 +1,232 @@
/*
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.
*/
#include "camera_pose_visualization.h"
const Eigen::Vector3d camera_pose_visualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0);
const Eigen::Vector3d camera_pose_visualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0);
const Eigen::Vector3d camera_pose_visualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0);
const Eigen::Vector3d camera_pose_visualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0);
const Eigen::Vector3d camera_pose_visualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0);
const Eigen::Vector3d camera_pose_visualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0);
const Eigen::Vector3d camera_pose_visualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0);
const Eigen::Vector3d camera_pose_visualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0);
#ifdef ROS2
using GeometryPoint = geometry_msgs::msg::Point;
#else
using GeometryPoint = geometry_msgs::Point;
#endif
void Eigen2Point(const Eigen::Vector3d& v, GeometryPoint& p) {
p.x = v.x();
p.y = v.y();
p.z = v.z();
}
camera_pose_visualization::camera_pose_visualization(float r, float g, float b, float a)
: m_marker_ns("camera_pose_visualization"), m_scale(0.3), m_line_width(0.03) {
m_image_boundary_color.r = r;
m_image_boundary_color.g = g;
m_image_boundary_color.b = b;
m_image_boundary_color.a = a;
m_optical_center_connector_color.r = r;
m_optical_center_connector_color.g = g;
m_optical_center_connector_color.b = b;
m_optical_center_connector_color.a = a;
}
void camera_pose_visualization::setImageBoundaryColor(float r, float g, float b, float a) {
m_image_boundary_color.r = r;
m_image_boundary_color.g = g;
m_image_boundary_color.b = b;
m_image_boundary_color.a = a;
}
void camera_pose_visualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) {
m_optical_center_connector_color.r = r;
m_optical_center_connector_color.g = g;
m_optical_center_connector_color.b = b;
m_optical_center_connector_color.a = a;
}
void camera_pose_visualization::setScale(double s) {
m_scale = s;
}
void camera_pose_visualization::setLineWidth(double width) {
m_line_width = width;
}
void camera_pose_visualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
Marker marker;
marker.ns = m_marker_ns;
marker.id = m_markers.size() + 1;
#ifdef ROS2
marker.type = Marker::LINE_LIST;
marker.action = Marker::ADD;
#else
marker.type = visualization_msgs::Marker::LINE_LIST;
marker.action = visualization_msgs::Marker::ADD;
#endif
marker.scale.x = 0.005;
marker.color.g = 1.0f;
marker.color.a = 1.0;
GeometryPoint point0, point1;
Eigen2Point(p0, point0);
Eigen2Point(p1, point1);
marker.points.push_back(point0);
marker.points.push_back(point1);
m_markers.push_back(marker);
}
void camera_pose_visualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
Marker marker;
marker.ns = m_marker_ns;
marker.id = m_markers.size() + 1;
#ifdef ROS2
marker.type = Marker::LINE_LIST;
marker.action = Marker::ADD;
#else
marker.type = visualization_msgs::Marker::LINE_LIST;
marker.action = visualization_msgs::Marker::ADD;
#endif
marker.scale.x = 0.04;
//marker.scale.x = 0.3;
marker.color.r = 1.0f;
marker.color.b = 1.0f;
marker.color.a = 1.0;
GeometryPoint point0, point1;
Eigen2Point(p0, point0);
Eigen2Point(p1, point1);
marker.points.push_back(point0);
marker.points.push_back(point1);
m_markers.push_back(marker);
}
void camera_pose_visualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) {
Marker marker;
marker.ns = m_marker_ns;
marker.id = m_markers.size() + 1;
#ifdef ROS2
marker.type = Marker::LINE_STRIP;
marker.action = Marker::ADD;
#else
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
#endif
marker.scale.x = m_line_width;
marker.pose.position.x = 0.0;
marker.pose.position.y = 0.0;
marker.pose.position.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
GeometryPoint pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2;
Eigen2Point(q * (m_scale * imlt) + p, pt_lt);
Eigen2Point(q * (m_scale * imlb) + p, pt_lb);
Eigen2Point(q * (m_scale * imrt) + p, pt_rt);
Eigen2Point(q * (m_scale * imrb) + p, pt_rb);
Eigen2Point(q * (m_scale * lt0 ) + p, pt_lt0);
Eigen2Point(q * (m_scale * lt1 ) + p, pt_lt1);
Eigen2Point(q * (m_scale * lt2 ) + p, pt_lt2);
Eigen2Point(q * (m_scale * oc ) + p, pt_oc);
// image boundaries
marker.points.push_back(pt_lt);
marker.points.push_back(pt_lb);
marker.colors.push_back(m_image_boundary_color);
marker.colors.push_back(m_image_boundary_color);
marker.points.push_back(pt_lb);
marker.points.push_back(pt_rb);
marker.colors.push_back(m_image_boundary_color);
marker.colors.push_back(m_image_boundary_color);
marker.points.push_back(pt_rb);
marker.points.push_back(pt_rt);
marker.colors.push_back(m_image_boundary_color);
marker.colors.push_back(m_image_boundary_color);
marker.points.push_back(pt_rt);
marker.points.push_back(pt_lt);
marker.colors.push_back(m_image_boundary_color);
marker.colors.push_back(m_image_boundary_color);
// top-left indicator
marker.points.push_back(pt_lt0);
marker.points.push_back(pt_lt1);
marker.colors.push_back(m_image_boundary_color);
marker.colors.push_back(m_image_boundary_color);
marker.points.push_back(pt_lt1);
marker.points.push_back(pt_lt2);
marker.colors.push_back(m_image_boundary_color);
marker.colors.push_back(m_image_boundary_color);
// optical center connector
marker.points.push_back(pt_lt);
marker.points.push_back(pt_oc);
marker.colors.push_back(m_optical_center_connector_color);
marker.colors.push_back(m_optical_center_connector_color);
marker.points.push_back(pt_lb);
marker.points.push_back(pt_oc);
marker.colors.push_back(m_optical_center_connector_color);
marker.colors.push_back(m_optical_center_connector_color);
marker.points.push_back(pt_rt);
marker.points.push_back(pt_oc);
marker.colors.push_back(m_optical_center_connector_color);
marker.colors.push_back(m_optical_center_connector_color);
marker.points.push_back(pt_rb);
marker.points.push_back(pt_oc);
marker.colors.push_back(m_optical_center_connector_color);
marker.colors.push_back(m_optical_center_connector_color);
m_markers.push_back(marker);
}
void camera_pose_visualization::reset() {
m_markers.clear();
}
void camera_pose_visualization::publish_by( Publisher& pub, const Header& header ) {
MarkerArray markerArray_msg;
for (auto& marker : m_markers) {
marker.header = header;
markerArray_msg.markers.push_back(marker);
}
pub.publish(markerArray_msg);
}
src/depth_image_ros2_node.cpp
+13
View File
@@ -1,3 +1,16 @@
/*
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.
*/
#include "depth_image_ros2_node.hpp"
#include <functional>
src/depth_image_ros_node.cpp
+13
View File
@@ -1,3 +1,16 @@
/*
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.
*/
#include "depth_image_ros_node.hpp"
#include <boost/bind.hpp>
src/host_sdk_sample.cpp
+195 -62
View File
@@ -1,3 +1,16 @@
/*
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.
*/
#include "host_sdk_sample.h"
#include "yaml_parser.h"
#include "rawCloudRender.h"
@@ -31,7 +44,7 @@
#include <ros/package.h>
#include <ros/ros.h>
#endif
#define ros_driver_version "0.5.0"
#define ros_driver_version "0.5.2"
// Global variable declarations
static device_handle odinDevice = nullptr;
static std::atomic<bool> deviceConnected(false);
@@ -71,42 +84,52 @@ int g_sendrgb_compressed = 0;
int g_sendrgb_undistort = 0;
int g_record_data = 0;
int g_devstatus_log = 0;
int g_pub_intensity_gray = 0;
int g_show_path = 0;
int g_show_camerapose = 0;
std::filesystem::path log_root_dir_;
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;
struct timespec last = {0, 0};
int count = 0;
std::mutex fps_mutex;
} fpsHandle;
static void sensor_fps(fpsHandle* handle, const char* name, bool print = false)
{
void update_count(fpsHandle* handle) {
struct timespec now;
std::lock_guard<std::mutex> lock(handle->fps_mutex);
clock_gettime(CLOCK_MONOTONIC, &now);
if (handle->start.tv_sec == 0 && handle->start.tv_nsec == 0) {
if (handle->count == 0) {
handle->start = now;
} else {
handle->last = now;
}
handle->count++;
}
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);
double cal_fps(fpsHandle* handle, const char* name, bool print = false)
{
std::lock_guard<std::mutex> lock(handle->fps_mutex);
if (handle->count < 2) {
return 0.0;
}
double elapsed = (handle->last.tv_sec - handle->start.tv_sec)
+ (handle->last.tv_nsec - handle->start.tv_nsec) / 1e9;
double fps = (handle->count - 1) / elapsed;
if (print) {
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "%s FPS: %f", name, handle->fps.load());
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "%s FPS: %f (count: %d, elapsed: %f)", name, fps, handle->count, elapsed);
#else
ROS_INFO("%s FPS: %f", name, handle->fps.load());
ROS_INFO("%s FPS: %f (count: %d, elapsed: %f)", name, fps, handle->count, elapsed);
#endif
}
handle->frame_count = 0;
handle->start = now;
}
handle->start = handle->last;
handle->count = 1;
return fps;
}
static fpsHandle rgb_rx_fps;
@@ -136,6 +159,51 @@ RosNodeControlInterface* getRosNodeControl() {
return &g_rosNodeControlImpl;
}
// Return resident memory (RSS) in **megabytes** for a given PID
double read_rss_mb(pid_t pid) {
std::string path = "/proc/" + std::to_string(pid) + "/status";
std::ifstream in(path);
if (!in) return 0.0;
std::string key;
long kb = 0;
while (in >> key) {
if (key == "VmRSS:") { // VmRSS is reported in kB
in >> kb;
break;
}
in.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}
return kb / 1024.0; // convert to MB
}
double read_pss_mb(pid_t pid) {
std::string path = "/proc/" + std::to_string(pid) + "/smaps_rollup";
std::ifstream in(path);
if (!in) return 0.0;
std::string key;
long kb = 0;
while (in >> key) {
if (key == "Pss:") { // Proportional Set Size in kB
in >> kb;
break;
}
in.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}
return kb / 1024.0; // convert to MB
}
// Recursively collect child PIDs of the given pid
void collect_children(pid_t pid, std::vector<pid_t>& all) {
std::string task_path = "/proc/" + std::to_string(pid) +
"/task/" + std::to_string(pid) + "/children";
std::ifstream in(task_path);
pid_t child;
while (in >> child) {
all.push_back(child);
collect_children(child, all);
}
}
void clear_all_queues();
// detect USB3.0
@@ -403,6 +471,11 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
imu_convert_data_t *imudata = nullptr;
lidar_device_status_t *dev_info_data;
pid_t self = getpid();
std::vector<pid_t> pids;
double total_mb = 0.0;
switch(data->type) {
case LIDAR_DT_NONE:
printf("empty lidar data type: %x\n", data->type);
@@ -411,35 +484,45 @@ 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");
update_count(&rgb_rx_fps);
break;
case LIDAR_DT_RAW_IMU:
if (g_sendimu) {
imudata = (imu_convert_data_t *)data->stream.imageList[0].pAddr;
g_ros_object->publishImu(imudata);
}
sensor_fps(&imu_rx_fps, "imu_rx");
update_count(&imu_rx_fps);
break;
case LIDAR_DT_RAW_DTOF:
if (g_senddtof ) {
g_ros_object->publishIntensityCloud((capture_Image_List_t *)&data->stream, 1);
}
sensor_fps(&dtof_rx_fps, "dtof_rx");
if (g_pub_intensity_gray) {
g_ros_object->publishGrayUInt8((capture_Image_List_t *)&data->stream, 2);
}
update_count(&dtof_rx_fps);
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");
update_count(&slam_cloud_rx_fps);
break;
case LIDAR_DT_SLAM_ODOMETRY:
if (g_sendodom) {
g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, false);
g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, false, g_show_path, g_show_camerapose);
}
sensor_fps(&slam_odom_rx_fps, "slam_odom_rx");
update_count(&slam_odom_rx_fps);
break;
case LIDAR_DT_DEV_STATUS:
dev_info_data = (lidar_device_status_t *)data->stream.imageList[0].pAddr;
pids.push_back(self);
collect_children(self, pids);
for (pid_t p : pids) {
total_mb += read_pss_mb(p); // read_rss_mb(p);
}
if (g_devstatus_log) {
if (dev_status_csv_file) {
// append the data row
@@ -471,7 +554,8 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
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()));
cal_fps(&rgb_rx_fps, "rgb_rx")
);
if (rc < 0) {
printf("Failed to write to dev_status_csv_file\n");
}
@@ -479,7 +563,8 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
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()));
cal_fps(&dtof_rx_fps, "dtof_rx")
);
if (rc < 0) {
printf("Failed to write to dev_status_csv_file\n");
}
@@ -487,18 +572,24 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
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()));
cal_fps(&imu_rx_fps, "imu_rx")
);
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",
rc = std::fprintf(dev_status_csv_file, "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,",
((float)dev_info_data->slam_cloud_tx_odr)/1000,
(slam_cloud_rx_fps.fps.load()),
cal_fps(&slam_cloud_rx_fps, "slam_cloud_rx"),
((float)dev_info_data->slam_odom_tx_odr)/1000,
(slam_odom_rx_fps.fps.load()),
cal_fps(&slam_odom_rx_fps, "slam_odom_rx"),
((float)dev_info_data->slam_odom_highfreq_tx_odr)/1000,
(slam_odom_highfreq_rx_fps.fps.load()));
cal_fps(&slam_odom_highfreq_rx_fps, "slam_odom_highfreq_rx"));
if (rc < 0) {
printf("Failed to write to dev_status_csv_file\n");
}
rc = std::fprintf(dev_status_csv_file, "%.2f\n", total_mb);
if (rc < 0) {
printf("Failed to write to dev_status_csv_file\n");
}
@@ -531,12 +622,14 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
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()));
cal_fps(&rgb_rx_fps, "rgb_rx")
);
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()));
cal_fps(&dtof_rx_fps, "dtof_rx")
);
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);
@@ -544,33 +637,39 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
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())
cal_fps(&imu_rx_fps, "imu_rx")
);
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())
cal_fps(&slam_cloud_rx_fps, "slam_cloud_rx")
);
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())
cal_fps(&slam_odom_rx_fps, "slam_odom_rx")
);
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())
cal_fps(&slam_odom_highfreq_rx_fps, "slam_odom_highfreq_rx")
);
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("dev_info"), "Total RAM (parent + children): %.2f MB", total_mb);
#else
ROS_INFO("Total RAM (parent + children): %.2f MB", total_mb);
#endif
printf("\n------------------------------------------\n");
}
break;
case LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ:
{
if (g_sendodom) {
g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, true);
g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, true, false, false);
}
sensor_fps(&slam_odom_highfreq_rx_fps, "slam_odom_highfreq_rx");
update_count(&slam_odom_highfreq_rx_fps);
}
break;
default:
@@ -763,6 +862,46 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
return;
}
auto con_time = std::chrono::system_clock::now();
std::time_t t = std::chrono::system_clock::to_time_t(con_time);
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::string folder_name = std::string("Conn_") + std::string(buf);
std::filesystem::path per_con_log_root_dir_ = log_root_dir_ / folder_name;
std::filesystem::create_directories(per_con_log_root_dir_);
std::string dev_status_csv_file_path_ = per_con_log_root_dir_ / "dev_status.csv";
if (dev_status_csv_file) {
std::fflush(dev_status_csv_file);
fclose(dev_status_csv_file);
dev_status_csv_file = nullptr;
}
// 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,"
"host_ram_use(mb)\n";
fprintf(dev_status_csv_file, "%s", header);
std::fflush(dev_status_csv_file);
}
uint32_t dtof_subframe_odr = 0;
if (lidar_start_stream(odinDevice, type, dtof_subframe_odr)) {
#ifdef ROS2
@@ -800,7 +939,8 @@ 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) {
bool load_status = g_ros_object->loadCameraParams(calib_config);
if (g_sendrgb_undistort && load_status == 0) {
g_ros_object->buildUndistortMap();
}
@@ -825,6 +965,12 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
clear_all_queues();
if (dev_status_csv_file) {
std::fflush(dev_status_csv_file);
fclose(dev_status_csv_file);
dev_status_csv_file = nullptr;
}
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Waiting for device reconnection...");
#else
@@ -883,6 +1029,9 @@ int main(int argc, char *argv[])
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_pub_intensity_gray = get_key_value("pubintensitygray", 0);
g_show_path = get_key_value("showpath", 0);
g_show_camerapose = get_key_value("showcamerapose", 0);
g_log_level = get_key_value("log_devel", LOG_LEVEL_INFO);
lidar_log_set_level(LIDAR_LOG_INFO);
@@ -925,28 +1074,9 @@ int main(int argc, char *argv[])
#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::string folder_name = std::string("Driver_") + std::string(buf);
log_root_dir_ = std::filesystem::path(log_dir) / folder_name;
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)) {
@@ -1121,8 +1251,11 @@ int main(int argc, char *argv[])
// lidar_close_device(odinDevice);
// lidar_destory_device(odinDevice);
if (dev_status_csv_file) {
std::fflush(dev_status_csv_file);
fclose(dev_status_csv_file);
dev_status_csv_file = nullptr;
}
}
// lidar_system_deinit();
src/pcd2depth_ros.cpp
+13
View File
@@ -1,3 +1,16 @@
/*
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.
*/
#include <ros/ros.h>
#include <fstream>
#include <sys/stat.h>
src/pcd2depth_ros2.cpp
+13
View File
@@ -1,3 +1,16 @@
/*
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.
*/
#include <rclcpp/rclcpp.hpp>
#include <fstream>
#include <sys/stat.h>
src/pointcloud_depth_converter.cpp
+16 -13
View File
@@ -1,3 +1,16 @@
/*
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.
*/
#include "pointcloud_depth_converter.hpp"
#include <cmath>
#include <iostream>
@@ -23,7 +36,7 @@ void PointCloudToDepthConverter::initializeInternalParams()
Kl_ = Eigen::Matrix3d::Identity();
Kl_(0, 0) = params_.A11 / params_.scale;
Kl_(0, 1) = params_.A12 / params_.scale;
Kl_(0, 1) = 0.0;
Kl_(0, 2) = params_.u0 / params_.scale;
Kl_(1, 1) = params_.A22 / params_.scale;
Kl_(1, 2) = params_.v0 / params_.scale;
@@ -241,16 +254,7 @@ cv::Mat PointCloudToDepthConverter::postProcessDepthImage(const cv::Mat &depth_i
return cv::Mat();
}
cv::Mat depth_img_distorted;
try {
depth_undistorted_ = depth_img_upsampled.clone();
cv::remap(depth_img_upsampled, depth_img_distorted, map_x_, map_y_, cv::INTER_LINEAR);
} catch (const cv::Exception& e) {
std::cerr << "ERROR: Remap failed: " << e.what() << std::endl;
return cv::Mat();
}
return depth_img_distorted;
return depth_img_upsampled;
}
cv::Mat PointCloudToDepthConverter::customResize(const cv::Mat& src, const cv::Size& size) {
@@ -291,10 +295,9 @@ pcl::PointCloud<pcl::PointXYZRGB> PointCloudToDepthConverter::generateColoredClo
const cv::Mat &depth_img, const cv::Mat &color_img)
{
cv::Mat depth_undistorted, color_undistorted;
depth_undistorted = depth_undistorted_.clone();
depth_undistorted = depth_img.clone();
cv::remap(color_img, color_undistorted, inv_map_x_, inv_map_y_, cv::INTER_LINEAR);
pcl::PointCloud<pcl::PointXYZRGB> cloud_colored;
Eigen::Matrix4d Tlc = params_.Tcl.inverse();
src/rawCloudRender.cpp
+13
View File
@@ -1,3 +1,16 @@
/*
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.
*/
#include "rawCloudRender.h"
#include <yaml-cpp/yaml.h>
#include <iostream>
src/yaml_parser.cpp
+13
View File
@@ -1,3 +1,16 @@
/*
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.
*/
#include "yaml_parser.h"
#include <fstream>
#include <filesystem>