diff --git a/CMakeLists.txt b/CMakeLists.txt index b8236c7..46ea021 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/README.md b/README.md index b57ebbd..7e35cc7 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.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 @@ -204,17 +207,17 @@ 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_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 | +| odin1/imu | sendimu | Imu Topic | +| 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 | +| 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. 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_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: diff --git a/config/control_command.yaml b/config/control_command.yaml index 2b1c002..f71117f 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -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 diff --git a/config/odin_ros.rviz b/config/odin_ros.rviz index 12d7107..ca57624 100644 --- a/config/odin_ros.rviz +++ b/config/odin_ros.rviz @@ -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,76 +76,6 @@ Visualization Manager: Transport Hint: raw Unreliable: false Value: false - - 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: false - Enabled: true - Keep: 1 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Queue Size: 1 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - 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: @@ -161,9 +91,7 @@ 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 @@ -204,6 +132,88 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz/Group + Displays: + - 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: false + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 1 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + 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 + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /odin1/camera_pose_visual + Name: camera_view + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: 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: - 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 \ No newline at end of file diff --git a/config/odin_ros2.rviz b/config/odin_ros2.rviz index 15edc8c..798eb08 100644 --- a/config/odin_ros2.rviz +++ b/config/odin_ros2.rviz @@ -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,84 +74,6 @@ Visualization Manager: Reliability Policy: Reliable Value: /odin1/image/undistorted Value: false - - 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: true - Keep: 1 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - 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: @@ -220,6 +142,100 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_common/Group + Displays: + - 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: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + 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 + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: camera_view + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odin1/camera_pose_visual + Value: false + Enabled: 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: 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 \ No newline at end of file diff --git a/include/camera_pose_visualization.h b/include/camera_pose_visualization.h new file mode 100755 index 0000000..8317215 --- /dev/null +++ b/include/camera_pose_visualization.h @@ -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 +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif +#include +#include + +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; + #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 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 ; +}; diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h index e4b48ce..b4dfa3a 100644 --- a/include/host_sdk_sample.h +++ b/include/host_sdk_sample.h @@ -42,9 +42,11 @@ limitations under the License. #include #include #include +#include #include #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 + #include #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 #include #include + #include #include 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 #include #include + #include #include + 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 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 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(); + 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 getIntensityCloudQueueSnapshot() { std::lock_guard lock(pcd_queue_mutex_); @@ -1267,9 +1442,12 @@ private: 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); + path_publisher_ = node_->create_publisher("odin1/path", 10); + pub_camera_pose_visual_ = node_->create_publisher("odin1/camera_pose_visual", 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); + compressed_rgb_pub_ = node_->create_publisher("odin1/image/compressed", 10); + undistort_rgb_pub_ = node_->create_publisher("odin1/image/undistorted", 10); + intensity_gray_pub_ = node_->create_publisher("odin1/image/intensity_gray", 10); #endif } #ifdef ROS1 @@ -1280,9 +1458,12 @@ private: xyzrgbacloud_pub_ = nh.advertise("odin1/cloud_slam", 10); odom_publisher_ = nh.advertise("odin1/odometry", 10); odom_highfreq_publisher_ = nh.advertise("odin1/odometry_highfreq", 10); + path_publisher_ = nh.advertise("odin1/path", 10); + pub_camera_pose_visual_ = nh.advertise("odin1/camera_pose_visual", 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); + intensity_gray_pub_ = nh.advertise("odin1/image/intensity_gray", 10); } #endif @@ -1294,11 +1475,15 @@ private: rclcpp::Publisher::SharedPtr xyzrgbacloud_pub_; rclcpp::Publisher::SharedPtr odom_publisher_; rclcpp::Publisher::SharedPtr odom_highfreq_publisher_; + rclcpp::Publisher::SharedPtr path_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_; + rclcpp::Publisher::SharedPtr intensity_gray_pub_; + rclcpp::Publisher::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 }; diff --git a/include/pointcloud_depth_converter.hpp b/include/pointcloud_depth_converter.hpp index 94c12a5..ae9226e 100644 --- a/include/pointcloud_depth_converter.hpp +++ b/include/pointcloud_depth_converter.hpp @@ -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_; diff --git a/src/camera_pose_visualization.cpp b/src/camera_pose_visualization.cpp new file mode 100755 index 0000000..7cd5dbe --- /dev/null +++ b/src/camera_pose_visualization.cpp @@ -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); +} \ No newline at end of file diff --git a/src/depth_image_ros2_node.cpp b/src/depth_image_ros2_node.cpp index 82b2492..b2e8c4c 100644 --- a/src/depth_image_ros2_node.cpp +++ b/src/depth_image_ros2_node.cpp @@ -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 diff --git a/src/depth_image_ros_node.cpp b/src/depth_image_ros_node.cpp index 8979d35..fb4aa55 100644 --- a/src/depth_image_ros_node.cpp +++ b/src/depth_image_ros_node.cpp @@ -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 diff --git a/src/host_sdk_sample.cpp b/src/host_sdk_sample.cpp index b29cdd3..ce74c7a 100644 --- a/src/host_sdk_sample.cpp +++ b/src/host_sdk_sample.cpp @@ -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 #include #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 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 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 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); - 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; +double cal_fps(fpsHandle* handle, const char* name, bool print = false) +{ + std::lock_guard 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 (count: %d, elapsed: %f)", name, fps, handle->count, elapsed); + #else + ROS_INFO("%s FPS: %f (count: %d, elapsed: %f)", name, fps, handle->count, elapsed); + #endif + } + 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::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::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& 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 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: @@ -762,6 +861,46 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach odinDevice = nullptr; 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)) { @@ -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); - std::fflush(dev_status_csv_file); - fclose(dev_status_csv_file); + if (dev_status_csv_file) { + std::fflush(dev_status_csv_file); + fclose(dev_status_csv_file); + dev_status_csv_file = nullptr; + } } // lidar_system_deinit(); diff --git a/src/pcd2depth_ros.cpp b/src/pcd2depth_ros.cpp index 2b42bf9..5934faa 100644 --- a/src/pcd2depth_ros.cpp +++ b/src/pcd2depth_ros.cpp @@ -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 #include #include diff --git a/src/pcd2depth_ros2.cpp b/src/pcd2depth_ros2.cpp index 0ac5379..001302c 100644 --- a/src/pcd2depth_ros2.cpp +++ b/src/pcd2depth_ros2.cpp @@ -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 #include #include diff --git a/src/pointcloud_depth_converter.cpp b/src/pointcloud_depth_converter.cpp index 09b200c..cc15533 100644 --- a/src/pointcloud_depth_converter.cpp +++ b/src/pointcloud_depth_converter.cpp @@ -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 #include @@ -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,9 +295,8 @@ pcl::PointCloud 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 cloud_colored; diff --git a/src/rawCloudRender.cpp b/src/rawCloudRender.cpp index ac0c86b..13c47a6 100644 --- a/src/rawCloudRender.cpp +++ b/src/rawCloudRender.cpp @@ -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 #include diff --git a/src/yaml_parser.cpp b/src/yaml_parser.cpp index bdbfe4f..c289f7c 100644 --- a/src/yaml_parser.cpp +++ b/src/yaml_parser.cpp @@ -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 #include