diff --git a/CMakeLists.txt b/CMakeLists.txt index 23d83b5..1135cf7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -195,6 +195,20 @@ if(ROS_VERSION STREQUAL "ROS1") ${PCL_LIBRARIES} ) + add_library(cloud_reprojector src/cloud_reprojector.cpp) + target_link_libraries(cloud_reprojector + ${OpenCV_LIBS} + ${PCL_LIBRARIES} + ) + + add_executable(cloud_reprojection_node src/cloud_reprojection_ros.cpp) + target_link_libraries(cloud_reprojection_node + cloud_reprojector + ${catkin_LIBRARIES} + ${OpenCV_LIBS} + ${PCL_LIBRARIES} + ) + # Installation rules install(TARGETS host_sdk_sample RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -301,11 +315,37 @@ elseif(ROS_VERSION STREQUAL "ROS2") message_filters ) + add_library(cloud_reprojector_ros2 src/cloud_reprojector.cpp) + target_compile_definitions(cloud_reprojector_ros2 PRIVATE ROS2) + target_link_libraries(cloud_reprojector_ros2 + ${OpenCV_LIBS} + ${PCL_LIBRARIES} + ) + + add_executable(cloud_reprojection_ros2_node src/cloud_reprojection_ros.cpp) + target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ROS2) + target_link_libraries(cloud_reprojection_ros2_node + cloud_reprojector_ros2 + ${OpenCV_LIBS} + ${PCL_LIBRARIES} + yaml-cpp + ) + ament_target_dependencies(cloud_reprojection_ros2_node + rclcpp + sensor_msgs + nav_msgs + cv_bridge + image_transport + pcl_conversions + message_filters + ) + # Installation rules - ensure all install targets are defined before ament_package() # Install executable install(TARGETS host_sdk_sample pcd2depth_ros2_node + cloud_reprojection_ros2_node EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/README.md b/README.md index 48bdf5e..0a0d5cf 100644 --- a/README.md +++ b/README.md @@ -18,9 +18,9 @@ This driver package provides core functionality for point cloud SLAM application ## 1. Version -Current version: v0.8.0 +Current version: v0.9.0 -Required device firmware version: v0.9.0 +Required device firmware version: v0.10.0 ## 2. Preparation @@ -198,6 +198,8 @@ Odin_ROS_Driver/ // ROS1/ROS2 driver package pcd2depth_ros.cpp //Source code for pcd2depth_ros pcd2depth_ros2.cpp //Source code for pcd2depth_ros2 pointcloud_depth_converter.cpp //Source code for pointcloud_depth_converter + cloud_reprojection_ros.cpp //Source code for cloud reprojection node (ROS1/ROS2) + cloud_reprojector.cpp //Core logic for cloud reprojection lib/ liblydHostApi_amd.a // Static library for AMD platform liblydHostApi_arm.a // Static library for ARM platform @@ -211,6 +213,8 @@ Odin_ROS_Driver/ // ROS1/ROS2 driver package depth_image_ros_node.hpp // depth_image_ros_node depth_image_ros2_node.hpp // depth_image_ros2_node pointcloud_depth_converter.hpp // pointcloud_depth_convert + cloud_reprojection_ros_node.hpp // cloud_reprojection_ros_node (ROS1/ROS2) + cloud_reprojector.hpp // Core class for cloud reprojection config/ control_command.yaml // Control parameter file for driver calib.yaml // Machine calibration yaml，differ for each individual device. Retrieved from the device everytime it connects to ROS driver @@ -255,6 +259,7 @@ Internal parameters of the Odin ROS driver are defined in config/control_command | tf | sendodom | tf tree 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 | +| odin1/reprojected_image | sendreprojection | Reprojected cloud to image Topic. Projects cloud_slam to camera image using odometry. Processed on host device. | ### 4.4 Data format @@ -310,6 +315,7 @@ float32 rgb // RGB value |control_command.yaml | Detailed Description | |-----------------------|----------------------| +| use_host_ros_time | Time synchronization mode: 0 - use odin internal system time as data timestamp (typical and recommended); 1 - use host ROS time upon receive (not recommended for most users); 2 - align odin1 time to host time via NTP-like synchronization, timestamp is the sensor data reception time on host time axis. | | strict_usb3.0_check | Strict USB3.0 check, if off, allow connection even if usb connection is below usb 3.0 | | recorddata | Record data in specific format that can be imported into MindCloud(TM) for post-processing. Please be aware that this will consume a lot of storage space. Testing shows 9.5G for 10mins of data. | | devstatuslog | Device status logging, currently save device status (soc temperature, cpu usage, ram usage, dtof sensor temp .etc) and data tx & rx rate to devstatus.csv under log folder. A new file will be created every time the driver is started. | diff --git a/config/control_command.yaml b/config/control_command.yaml index 63b1325..7bdd2cc 100644 --- a/config/control_command.yaml +++ b/config/control_command.yaml @@ -3,9 +3,10 @@ register_keys: # ATTENTION: usb 3.0 is always recommended, as advance functionality like SLAM mode requires usb 3.0 for reliable map file transfer strict_usb3.0_check: 0 # 0: off: 1: on; - # 0: use odin internal system time as data time stamp as before, typical and recommanded; - # 1: use host ros time (upon recieve) as data time stamp, only use if you specifically require this setup, not recommanded for most user - use_host_ros_time: 0 + # 0: use odin internal system time as data time stamp, typical and recommended; + # 1: use host ros time (upon receive) as data time stamp, only use if you specifically require this setup, not recommended for most users + # 2: align odin1 time to host time, timestamp is the sensor data reception time on host time axis + use_host_ros_time: 2 streamctrl: 1 # 0: off; 1: on @@ -33,6 +34,12 @@ register_keys: # raw dtof data senddtof: 1 # 0: off; 1: on + cloud_raw_confidence_threshold: 35 # please refer to readme for more details + + # dtof sensor frame rate. Supported values: 100 (10fps) or 145 (14.5fps) + # Higher frame rate provides smoother point cloud data but may increase data bandwidth + # Note: value is multiplied by 10 (e.g., 145 means 14.5fps) + dtof_fps: 100 # 100: 10fps; 145: 14.5fps # slam cloud data sendcloudslam: 1 # 0: off; 1: on @@ -46,10 +53,14 @@ register_keys: # Processed on host device senddepth: 0 # 0: off; 1: on + # cloud reprojection demo, projects cloud_slam to camera image using odometry + # Processed on host device + sendreprojection: 1 # 0: off; 1: on + # record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software. # save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/ # ATTENTION: please copy the full folder for post-processing. - recorddata: 0 # 0: off; 1: on + recorddata: 1 # 0: off; 1: on # Save device runtime status info to ws/src/odin_ros_driver/log/Driver_{drvier_start_time}/Conn_{device_connection_time}/dev_status.csv devstatuslog: 1 # 0: off; 1: on. diff --git a/config/odin_ros.rviz b/config/odin_ros.rviz index baca4e4..20634c7 100644 --- a/config/odin_ros.rviz +++ b/config/odin_ros.rviz @@ -9,7 +9,7 @@ Panels: - /slam1 - /TF1/Frames1 Splitter Ratio: 0.4993045926094055 - Tree Height: 837 + Tree Height: 495 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -77,6 +77,18 @@ Visualization Manager: Transport Hint: raw Unreliable: false Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /odin1/reprojected_image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: cloudslam_reprojected + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -334,8 +346,6 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - map: - Value: true odin1_base_link: Value: true odom: @@ -348,8 +358,6 @@ Visualization Manager: Show Names: true Tree: odom: - map: - {} odin1_base_link: {} Update Interval: 0 @@ -382,7 +390,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 15.013533592224121 + Distance: 16.25591278076172 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -398,21 +406,21 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7853981852531433 + Pitch: 1.010398030281067 Target Frame: odom - Yaw: 0.7853981852531433 + Yaw: 0.8753980994224548 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1672 + Height: 1016 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false Image_undistort: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000025f00000582fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000003b30000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000042d000001c30000002600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000002600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f007200740000000568000000df0000002600ffffff000000010000015f00000582fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000005820000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ab00000005efc0100000002fb0000000800540069006d0065010000000000000ab0000006dc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006da0000058200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001e70000033afc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000022c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000026f000001080000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f007200740000000568000000df0000001600fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a0065006300740065006400000002b2000000c50000001600ffffff000000010000015f0000033afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e60000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -421,8 +429,10 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2736 - X: 144 - Y: 54 + Width: 1848 + X: 72 + Y: 27 + cloudslam_reprojected: + collapsed: false dense_depth_image: - collapsed: false \ No newline at end of file + collapsed: false diff --git a/config/odin_ros2.rviz b/config/odin_ros2.rviz index 124933e..48d42c3 100644 --- a/config/odin_ros2.rviz +++ b/config/odin_ros2.rviz @@ -7,11 +7,12 @@ Panels: - /Global Options1 - /Status1 - /Image1/Topic1 + - /cloudslam_reprojected1 - /Odometry1 - /slam1 - /dense_depth_demo1 Splitter Ratio: 0.5 - Tree Height: 667 + Tree Height: 593 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -74,6 +75,20 @@ Visualization Manager: Reliability Policy: Reliable Value: /odin1/image/undistorted Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: cloudslam_reprojected + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odin1/reprojected_image + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -377,8 +392,6 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - map: - Value: true odin1_base_link: Value: true odom: @@ -390,8 +403,6 @@ Visualization Manager: Show Names: false Tree: odom: - map: - {} odin1_base_link: {} Update Interval: 0 @@ -449,18 +460,18 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 0.2568470537662506 + Y: 2.1451337337493896 + Z: 0.3774382472038269 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3203984200954437 + Pitch: 0.3953983187675476 Target Frame: odom Value: Orbit (rviz) - Yaw: 3.130404233932495 + Yaw: 3.230407953262329 Saved: ~ Window Geometry: Displays: @@ -472,7 +483,7 @@ Window Geometry: collapsed: false Image_undistort: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f50000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000031b000000c00000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004700000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004910000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: @@ -482,5 +493,7 @@ Window Geometry: Width: 1920 X: 540 Y: 124 + cloudslam_reprojected: + collapsed: false dense_depth_image: collapsed: false diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp new file mode 100644 index 0000000..4408255 --- /dev/null +++ b/include/cloud_reprojection_ros_node.hpp @@ -0,0 +1,104 @@ +/* +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 + #include + #include + #include + #include +#else + #include + #include + #include + #include + #include + #include + #include + #include +#endif + +#include +#include +#include + +#include "cloud_reprojector.hpp" + +#include +#include + +#ifdef ROS2 +class CloudReprojectionRosNode : public rclcpp::Node +{ +public: + CloudReprojectionRosNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + +private: + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Odometry = nav_msgs::msg::Odometry; + using Image = sensor_msgs::msg::Image; + + std::string cloud_slam_topic_; + std::string odometry_topic_; + std::string reprojected_image_topic_; + + message_filters::Subscriber cloud_sub_; + message_filters::Subscriber odom_sub_; + + typedef message_filters::sync_policies::ExactTime MySyncPolicy; + typedef message_filters::Synchronizer Sync; + std::shared_ptr sync_; + + image_transport::Publisher reprojected_image_pub_; + + std::unique_ptr reprojector_; + + void loadParameters(); + void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg, + const Odometry::ConstSharedPtr& odom_msg); +}; +#else +class CloudReprojectionRosNode +{ +public: + CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh); + +private: + ros::NodeHandle nh_, pnh_; + + std::string cloud_slam_topic_; + std::string odometry_topic_; + std::string reprojected_image_topic_; + + message_filters::Subscriber cloud_sub_; + message_filters::Subscriber odom_sub_; + + typedef message_filters::sync_policies::ExactTime MySyncPolicy; + typedef message_filters::Synchronizer Sync; + std::shared_ptr sync_; + + ros::Publisher reprojected_image_pub_; + + std::unique_ptr reprojector_; + + void loadParameters(); + void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg, + const nav_msgs::OdometryConstPtr& odom_msg); +}; +#endif diff --git a/include/cloud_reprojector.hpp b/include/cloud_reprojector.hpp new file mode 100644 index 0000000..e7ff884 --- /dev/null +++ b/include/cloud_reprojector.hpp @@ -0,0 +1,79 @@ +/* +Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co) +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + http://www.apache.org/licenses/LICENSE-2.0 +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "polynomial_camera.hpp" + +#include + +class CloudReprojector +{ +public: + struct CameraParams + { + int image_width = 1600; + int image_height = 1296; + double A11 = 0.0, A12 = 0.0, A22 = 0.0; + double u0 = 0.0, v0 = 0.0; + double k2 = 0.0, k3 = 0.0, k4 = 0.0, k5 = 0.0, k6 = 0.0, k7 = 0.0; + }; + + struct ExtrinsicParams + { + Eigen::Matrix4d Tcl = Eigen::Matrix4d::Identity(); // camera to lidar + Eigen::Matrix4d Til = Eigen::Matrix4d::Identity(); // lidar to imu (fixed) + Eigen::Matrix4d Tic = Eigen::Matrix4d::Identity(); // camera to imu (calculated) + }; + + struct OdomPose + { + Eigen::Quaterniond orientation = Eigen::Quaterniond::Identity(); + Eigen::Vector3d position = Eigen::Vector3d::Zero(); + }; + + CloudReprojector(); + ~CloudReprojector() = default; + + bool initialize(const CameraParams& cam_params, const ExtrinsicParams& ext_params); + + cv::Mat reprojectCloud(const pcl::PointCloud& cloud_odom, + const OdomPose& odom_pose); + + void setPointRadius(int radius) { point_radius_ = radius; } + int getPointRadius() const { return point_radius_; } + + const CameraParams& getCameraParams() const { return camera_params_; } + const ExtrinsicParams& getExtrinsicParams() const { return extrinsic_params_; } + + static Eigen::Matrix4d calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til); + +private: + Eigen::Matrix4d odomPoseToMatrix(const OdomPose& odom) const; + + cv::Mat projectCloudToImage(const pcl::PointCloud& cloud_in_cam) const; + + CameraParams camera_params_; + ExtrinsicParams extrinsic_params_; + std::unique_ptr camera_model_; + + int point_radius_ = 4; + bool initialized_ = false; +}; diff --git a/include/data_logger.h b/include/data_logger.h index 1366440..7311790 100644 --- a/include/data_logger.h +++ b/include/data_logger.h @@ -79,6 +79,7 @@ public: //cloud_writer_ = std::make_unique(root_dir_ / "OdinPointCloud.olx", opts.batch_size); image_writer_ = std::make_unique(root_dir_ / "OdinImage.bin", opts.batch_size); roatation_writer_ = std::make_unique(root_dir_ / "OdinRotate.bin", opts.batch_size); + imu_writer_ = std::make_unique(root_dir_ / "OdinIMU.bin", opts.batch_size); } ~BinaryDataLogger() { @@ -87,6 +88,7 @@ public: if (cloud_writer_) cloud_writer_->shutdown(); if (image_writer_) image_writer_->shutdown(); if (roatation_writer_) roatation_writer_->shutdown(); + if (imu_writer_) imu_writer_->shutdown(); } const std::filesystem::path& root_dir() const { return root_dir_; } @@ -104,6 +106,9 @@ public: void enqueueRotateFrame(std::vector&& blob) { if (roatation_writer_) roatation_writer_->enqueue(std::move(blob)); } + void enqueueIMUFrame(std::vector&& blob) { + if (imu_writer_) imu_writer_->enqueue(std::move(blob)); + } private: struct Writer { @@ -194,4 +199,5 @@ private: std::unique_ptr cloud_writer_; std::unique_ptr image_writer_; std::unique_ptr roatation_writer_; + std::unique_ptr imu_writer_; }; diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h index c6c92bb..5909823 100644 --- a/include/host_sdk_sample.h +++ b/include/host_sdk_sample.h @@ -70,6 +70,9 @@ enum class OdometryType { extern int g_log_level; extern int g_sendcloudrender; +extern int g_use_host_ros_time; +double get_ptp_smoothed_delay(); +double get_ptp_smoothed_offset(); #ifdef ROS2 #include "rclcpp/rclcpp.hpp" @@ -177,15 +180,40 @@ inline uint64_t ros_time_to_ns(const ros::Time &t) { #endif } +inline ros::Time make_aligned_stamp(uint64_t sensor_timestamp_ns +#ifdef ROS2 + , const rclcpp::Node::SharedPtr& node +#endif + ) { + if (g_use_host_ros_time == 1) { + #ifdef ROS2 + return node->now(); + #else + return ros::Time::now(); + #endif + } + + uint64_t ts_ns = sensor_timestamp_ns; + if (g_use_host_ros_time == 2) { + const double offset_s = get_ptp_smoothed_offset(); + const int64_t offset_ns = static_cast(offset_s * 1e9); + const int64_t base_ns = static_cast(sensor_timestamp_ns); + const int64_t aligned_ns = base_ns - offset_ns; + ts_ns = (aligned_ns < 0) ? 0ULL : static_cast(aligned_ns); + } + + return ns_to_ros_time(ts_ns); +} + class RosNodeControlInterface { public: virtual ~RosNodeControlInterface() = default; virtual void setDtofSubframeODR(int odr) = 0; virtual int getDtofSubframeODR() const = 0; - virtual void setUseHostRosTime(bool use_host_ros_time) = 0; - virtual bool useHostRosTime() const = 0; virtual void setSendOdomBaseLinkTF(bool send_odom_baselink_tf) = 0; virtual bool sendOdomBaseLinkTF() const = 0; + virtual void setCloudRawConfidenceThreshold(int threshold) = 0; + virtual int cloudRawConfidenceThreshold() const = 0; }; RosNodeControlInterface* getRosNodeControl(); @@ -241,15 +269,11 @@ public: ros::Imu imu_msg; #endif - if (getRosNodeControl()->useHostRosTime()) { - #ifdef ROS2 - imu_msg.header.stamp = node_->now(); - #else - imu_msg.header.stamp = ros::Time::now(); - #endif - } else { - imu_msg.header.stamp = ns_to_ros_time(stream->stamp); - } + #ifdef ROS2 + imu_msg.header.stamp = make_aligned_stamp(stream->stamp, node_); + #else + imu_msg.header.stamp = make_aligned_stamp(stream->stamp); + #endif imu_msg.header.frame_id = "imu_link"; imu_msg.linear_acceleration.y = -1 * stream->accel_x; @@ -270,6 +294,31 @@ public: #else imu_pub_.publish(imu_msg); #endif + + if(data_logger_) { + const double ts_sec = static_cast(stream->stamp) / 1e9; + float ax = imu_msg.linear_acceleration.x; + float ay = imu_msg.linear_acceleration.y; + float az = imu_msg.linear_acceleration.z; + float wx = imu_msg.angular_velocity.x; + float wy = imu_msg.angular_velocity.y; + float wz = imu_msg.angular_velocity.z; + + std::vector blob; + blob.reserve(sizeof(double) + sizeof(float) * 6); + auto append_pod = [&](const auto& v) { + const uint8_t* p = reinterpret_cast(&v); + blob.insert(blob.end(), p, p + sizeof(v)); + }; + append_pod(ts_sec); + append_pod(ax); + append_pod(ay); + append_pod(az); + append_pod(wx); + append_pod(wy); + append_pod(wz); + data_logger_->enqueueIMUFrame(std::move(blob)); + } } #ifdef ROS2 using ImageMsg = sensor_msgs::msg::Image; @@ -550,15 +599,11 @@ void publishIntensityCloud(capture_Image_List_t* stream, int idx) // Set message header msg->header.frame_id = "odin1_base_link"; - if (getRosNodeControl()->useHostRosTime()) { - #ifdef ROS2 - msg->header.stamp = node_->now(); - #else - msg->header.stamp = ros::Time::now(); - #endif - } else { - msg->header.stamp = ns_to_ros_time(cloud.timestamp); - } + #ifdef ROS2 + msg->header.stamp = make_aligned_stamp(cloud.timestamp, node_); + #else + msg->header.stamp = make_aligned_stamp(cloud.timestamp); + #endif msg->height = cloud.height; msg->width = cloud.width; @@ -596,9 +641,9 @@ void publishIntensityCloud(capture_Image_List_t* stream, int idx) uint8_t* intensity_data = static_cast(stream->imageList[2].pAddr); uint16_t* confidence_data = static_cast(stream->imageList[3].pAddr); - + int confidence_threshold = getRosNodeControl()->cloudRawConfidenceThreshold(); for (int i = 0; i < total_points; ++i) { - if (confidence_data[i] < 35) { + if (confidence_data[i] < confidence_threshold) { *iter_x = 0.0f; ++iter_x; *iter_y = 0.0f; ++iter_y; *iter_z = 0.0f; ++iter_z; @@ -647,11 +692,10 @@ void publishIntensityCloud(capture_Image_List_t* stream, int idx) } } - { + // Only cache point cloud if cloud_render is enabled + if (g_sendcloudrender) { std::lock_guard lock(pcd_queue_mutex_); - // Get actual point count - const int real_point_count = cloud.width * cloud.height; // Create deep copy of point cloud #ifdef ROS2 auto msg_copy = std::make_shared(*msg); @@ -679,15 +723,11 @@ void publishIntensityCloud(capture_Image_List_t* stream, int idx) void publishGrayUInt8(capture_Image_List_t *stream, int idx) { ImageMsg msg; - if (getRosNodeControl()->useHostRosTime()) { - #ifdef ROS2 - msg.header.stamp = node_->now(); - #else - msg.header.stamp = ros::Time::now(); - #endif - } else { - msg.header.stamp = ns_to_ros_time(stream->imageList[idx].timestamp); - } + #ifdef ROS2 + msg.header.stamp = make_aligned_stamp(stream->imageList[idx].timestamp, node_); + #else + msg.header.stamp = make_aligned_stamp(stream->imageList[idx].timestamp); + #endif msg.header.frame_id = "map"; int width = stream->imageList[idx].width; @@ -731,15 +771,11 @@ void publishRgb(capture_Image_List_t *stream) { cv::Mat decoded_image = cv::imdecode(jpeg_data, cv::IMREAD_COLOR); cv_bridge::CvImage cv_image; - if (getRosNodeControl()->useHostRosTime()) { - #ifdef ROS2 - cv_image.header.stamp = node_->now(); - #else - cv_image.header.stamp = ros::Time::now(); - #endif - } else { - cv_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); - } + #ifdef ROS2 + cv_image.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp, node_); + #else + cv_image.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp); + #endif cv_image.encoding = "bgr8"; cv_image.image = decoded_image; @@ -775,15 +811,11 @@ void publishRgb(capture_Image_List_t *stream) { if (m_undistort_map_init_success) { cv::remap(decoded_image, undistorted_image, m_undistort_map_x, m_undistort_map_y, cv::INTER_LINEAR); - if (getRosNodeControl()->useHostRosTime()) { - #ifdef ROS2 - cv_undistorted_image.header.stamp = node_->now(); - #else - cv_undistorted_image.header.stamp = ros::Time::now(); - #endif - } else { - cv_undistorted_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); - } + #ifdef ROS2 + cv_undistorted_image.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp, node_); + #else + cv_undistorted_image.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp); + #endif cv_undistorted_image.encoding = "bgr8"; cv_undistorted_image.image = undistorted_image; } @@ -795,13 +827,9 @@ void publishRgb(capture_Image_List_t *stream) { undistort_rgb_pub_->publish(*cv_undistorted_image.toImageMsg()); } - // original jpeg + // original jpeg - always publish as it's small sensor_msgs::msg::CompressedImage jpeg_msg; - if (getRosNodeControl()->useHostRosTime()) { - jpeg_msg.header.stamp = node_->now(); - } else { - jpeg_msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); - } + jpeg_msg.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp, node_); jpeg_msg.format = "jpeg"; jpeg_msg.data = jpeg_data; @@ -816,11 +844,7 @@ void publishRgb(capture_Image_List_t *stream) { // original jpeg sensor_msgs::CompressedImagePtr jpeg_msg(new sensor_msgs::CompressedImage()); - if (getRosNodeControl()->useHostRosTime()) { - jpeg_msg->header.stamp = ros::Time::now(); - } else { - jpeg_msg->header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); - } + jpeg_msg->header.stamp = make_aligned_stamp(stream->imageList[0].timestamp); jpeg_msg->format = "jpeg"; jpeg_msg->data = jpeg_data; @@ -837,11 +861,7 @@ void publishRgb(capture_Image_List_t *stream) { #ifdef ROS2 sensor_msgs::msg::PointCloud2 msg; msg.header.frame_id = "odom"; - if (getRosNodeControl()->useHostRosTime()) { - msg.header.stamp = node_->now(); - } else { - msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); - } + msg.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp, node_); //RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Point cloudrgba %ld",stream->imageList[0].timestamp); @@ -869,11 +889,7 @@ void publishRgb(capture_Image_List_t *stream) { #else sensor_msgs::PointCloud2 msg; msg.header.frame_id = "odom"; - if (getRosNodeControl()->useHostRosTime()) { - msg.header.stamp = ros::Time::now(); - } else { - msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp); - } + msg.header.stamp = make_aligned_stamp(stream->imageList[0].timestamp); size_t pt_size = sizeof(int32_t) * 3 + sizeof(int32_t) * 4; uint32_t points = stream->imageList[idx].length / pt_size; @@ -1098,15 +1114,11 @@ void publishRgb(capture_Image_List_t *stream) { if (data_len == sizeof(ros_odom_convert_complete_t)) { ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr; - if (getRosNodeControl()->useHostRosTime()) { - #ifdef ROS2 - msg.header.stamp = node_->now(); - #else - msg.header.stamp = ros::Time::now(); - #endif - } else { - msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); - } + #ifdef ROS2 + msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns, node_); + #else + msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns); + #endif msg.pose.pose.position.x = static_cast(odom_data->pos[0]) / 1e6; msg.pose.pose.position.y = static_cast(odom_data->pos[1]) / 1e6; @@ -1160,15 +1172,11 @@ void publishRgb(capture_Image_List_t *stream) { } else if (data_len == sizeof(ros2_odom_convert_t)) { ros2_odom_convert_t* odom_data = (ros2_odom_convert_t*)stream->imageList[0].pAddr; - if (getRosNodeControl()->useHostRosTime()) { - #ifdef ROS2 - msg.header.stamp = node_->now(); - #else - msg.header.stamp = ros::Time::now(); - #endif - } else { - msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); - } + #ifdef ROS2 + msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns, node_); + #else + msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns); + #endif msg.pose.pose.position.x = static_cast(odom_data->pos[0]) / 1e6; msg.pose.pose.position.y = static_cast(odom_data->pos[1]) / 1e6; @@ -1186,11 +1194,7 @@ void publishRgb(capture_Image_List_t *stream) { { if (getRosNodeControl()->sendOdomBaseLinkTF()) { geometry_msgs::msg::TransformStamped transformStamped; - if (getRosNodeControl()->useHostRosTime()) { - transformStamped.header.stamp = node_->now(); - } else { - transformStamped.header.stamp = msg.header.stamp; - } + transformStamped.header.stamp = msg.header.stamp; transformStamped.header.frame_id = "odom"; transformStamped.child_frame_id = "odin1_base_link"; transformStamped.transform.translation.x = msg.pose.pose.position.x; @@ -1266,11 +1270,7 @@ void publishRgb(capture_Image_List_t *stream) { case OdometryType::TRANSFORM: { geometry_msgs::msg::TransformStamped transformStamped; - if (getRosNodeControl()->useHostRosTime()) { - transformStamped.header.stamp = node_->now(); - } else { - transformStamped.header.stamp = msg.header.stamp; - } + transformStamped.header.stamp = msg.header.stamp; transformStamped.header.frame_id = "odom"; transformStamped.child_frame_id = "map"; transformStamped.transform.translation.x = msg.pose.pose.position.x; @@ -1290,11 +1290,7 @@ void publishRgb(capture_Image_List_t *stream) { { if (getRosNodeControl()->sendOdomBaseLinkTF()) { geometry_msgs::TransformStamped transformStamped; - if (getRosNodeControl()->useHostRosTime()) { - transformStamped.header.stamp = ros::Time::now(); - } else { - transformStamped.header.stamp = msg.header.stamp; - } + transformStamped.header.stamp = msg.header.stamp; transformStamped.header.frame_id = "odom"; transformStamped.child_frame_id = "odin1_base_link"; transformStamped.transform.translation.x = msg.pose.pose.position.x; @@ -1371,11 +1367,7 @@ void publishRgb(capture_Image_List_t *stream) { case OdometryType::TRANSFORM: { geometry_msgs::TransformStamped transformStamped; - if (getRosNodeControl()->useHostRosTime()) { - transformStamped.header.stamp = ros::Time::now(); - } else { - transformStamped.header.stamp = msg.header.stamp; - } + transformStamped.header.stamp = msg.header.stamp; transformStamped.header.frame_id = "odom"; transformStamped.child_frame_id = "map"; transformStamped.transform.translation.x = msg.pose.pose.position.x; @@ -1592,22 +1584,28 @@ private: void initialize_publishers() { #ifdef ROS2 - auto qos_profile = rclcpp::QoS(1) + // Small data with queue depth 1 + auto qos_small = rclcpp::QoS(1) .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - imu_pub_ = node_->create_publisher("odin1/imu", qos_profile); - rgb_pub_ = node_->create_publisher("odin1/image", qos_profile); - cloud_pub_ = node_->create_publisher("odin1/cloud_raw", qos_profile); - xyzrgbacloud_pub_ = node_->create_publisher("odin1/cloud_slam", qos_profile); - odom_publisher_ = node_->create_publisher("odin1/odometry", qos_profile); - odom_highfreq_publisher_ = node_->create_publisher("odin1/odometry_highfreq", qos_profile); - path_publisher_ = node_->create_publisher("odin1/path", qos_profile); - pub_camera_pose_visual_ = node_->create_publisher("odin1/camera_pose_visual", qos_profile); - rgbcloud_pub_ = node_->create_publisher("odin1/cloud_render", qos_profile); - compressed_rgb_pub_ = node_->create_publisher("odin1/image/compressed", qos_profile); - undistort_rgb_pub_ = node_->create_publisher("odin1/image/undistorted", qos_profile); - intensity_gray_pub_ = node_->create_publisher("odin1/image/intensity_gray", qos_profile); + // Large sensor data with larger queue to avoid blocking + auto qos_sensor = rclcpp::QoS(10) + .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE) + .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + imu_pub_ = node_->create_publisher("odin1/imu", qos_small); + rgb_pub_ = node_->create_publisher("odin1/image", qos_sensor); + cloud_pub_ = node_->create_publisher("odin1/cloud_raw", qos_sensor); + xyzrgbacloud_pub_ = node_->create_publisher("odin1/cloud_slam", qos_sensor); + odom_publisher_ = node_->create_publisher("odin1/odometry", qos_small); + odom_highfreq_publisher_ = node_->create_publisher("odin1/odometry_highfreq", qos_small); + path_publisher_ = node_->create_publisher("odin1/path", qos_sensor); + pub_camera_pose_visual_ = node_->create_publisher("odin1/camera_pose_visual", qos_sensor); + rgbcloud_pub_ = node_->create_publisher("odin1/cloud_render", qos_sensor); + compressed_rgb_pub_ = node_->create_publisher("odin1/image/compressed", qos_small); + undistort_rgb_pub_ = node_->create_publisher("odin1/image/undistorted", qos_sensor); + intensity_gray_pub_ = node_->create_publisher("odin1/image/intensity_gray", qos_sensor); tf_broadcaster = std::make_unique(node_); #endif } diff --git a/include/lidar_api.h b/include/lidar_api.h index c7bc9c1..21268a0 100644 --- a/include/lidar_api.h +++ b/include/lidar_api.h @@ -128,7 +128,6 @@ int lidar_set_mode(device_handle device, int mode); * * @param device Handle to the target device * @param type Type of data stream to start (see stream type definitions in lidar_api_type.h) - * @param dtof_subframe_odr DTOF subframe ODR from device, used for raw point cloud per-point time offset estimation * @return int 0 on success, negative error code on failure */ int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr); @@ -255,6 +254,18 @@ int lidar_get_custom_parameter(device_handle device, const char* param_name, int */ int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir); + +/** + * @brief Set the depth parameters for the device + * + * This function must be called before starting data stream. + * + * @param device Handle to the target device + * @param params Pointer to the depth parameters to set + * @return int 0 on success, negative error code on failure + */ +int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params); + #ifdef __cplusplus } #endif diff --git a/include/lidar_api_type.h b/include/lidar_api_type.h index 7b4cc61..4d127ae 100644 --- a/include/lidar_api_type.h +++ b/include/lidar_api_type.h @@ -56,7 +56,8 @@ typedef enum { LIDAR_DT_DEV_STATUS, LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ, LIDAR_DT_SLAM_ODOMETRY_TF, - LIDAR_DT_SLAM_WIWC + LIDAR_DT_SLAM_WIWC, + LIDAR_DT_NTP } lidar_data_type_e; typedef struct { @@ -117,6 +118,11 @@ typedef struct { uint32_t height; } buffer_List_t; +typedef struct { + double delay; + double offset; +} ptp_sync_data_t; + typedef struct capture_Image_List_t { uint32_t imageCount; buffer_List_t imageList[DEVICE_MAX_CH_NUMBER]; @@ -220,6 +226,15 @@ typedef enum { LIDAR_DEVICE_STREAM_STOPPED, } lidar_device_initial_state_e; +typedef enum { + LIDAR_DEPTH_ODR_10HZ = 0, + LIDAR_DEPTH_ODR_14_5HZ, +} lidar_depth_odr_e; + +typedef struct { + lidar_depth_odr_e odr; +} lidar_depth_para_t; + #ifdef __cplusplus } #endif diff --git a/launch_ROS1/odin1_ros1.launch b/launch_ROS1/odin1_ros1.launch index da14c97..e9790f2 100644 --- a/launch_ROS1/odin1_ros1.launch +++ b/launch_ROS1/odin1_ros1.launch @@ -21,6 +21,11 @@ + + + + + diff --git a/launch_ROS2/odin1_ros2.launch.py b/launch_ROS2/odin1_ros2.launch.py index 34224f0..af0f4cd 100644 --- a/launch_ROS2/odin1_ros2.launch.py +++ b/launch_ROS2/odin1_ros2.launch.py @@ -50,6 +50,21 @@ def generate_launch_description(): output='screen', parameters=[pcd2depth_params] ) + + # Cloud reprojection node + reprojection_config_path = os.path.join(package_dir, 'config', 'control_command.yaml') + with open(reprojection_config_path, 'r') as f: + reprojection_params = yaml.safe_load(f) + reprojection_calib_path = os.path.join(package_dir, 'config', 'calib.yaml') + reprojection_params['calib_file_path'] = reprojection_calib_path + cloud_reprojection_node = Node( + package='odin_ros_driver', + executable='cloud_reprojection_ros2_node', + name='cloud_reprojection_ros2_node', + output='screen', + parameters=[reprojection_params] + ) + # Create RViz2 node - loads specified configuration file rviz_node = Node( package='rviz2', @@ -65,6 +80,7 @@ def generate_launch_description(): ld.add_action(rviz_config_arg) # Add RViz configuration argument ld.add_action(host_sdk_node) ld.add_action(pcd2depth_node) + ld.add_action(cloud_reprojection_node) ld.add_action(rviz_node) # Add RViz node return ld diff --git a/lib/liblydHostApi_amd.a b/lib/liblydHostApi_amd.a index 8660301..9a25888 100644 Binary files a/lib/liblydHostApi_amd.a and b/lib/liblydHostApi_amd.a differ diff --git a/lib/liblydHostApi_arm.a b/lib/liblydHostApi_arm.a index 232990c..981b1d4 100644 Binary files a/lib/liblydHostApi_arm.a and b/lib/liblydHostApi_arm.a differ diff --git a/package.xml b/package.xml index 17f851d..b277f0b 100755 --- a/package.xml +++ b/package.xml @@ -2,28 +2,27 @@ odin_ros_driver 0.0.1 - ROS driver for Odin sensor + ROS2 driver for Odin sensor rlk Apache 2.0 - - - catkin - - - roscpp + + ament_cmake + + rclcpp + std_msgs sensor_msgs nav_msgs + geometry_msgs cv_bridge image_transport - - - eigen - opencv - yaml-cpp - - - - catkin - - + pcl_conversions + message_filters + tf2 + tf2_ros + tf2_geometry_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp new file mode 100644 index 0000000..9207c53 --- /dev/null +++ b/src/cloud_reprojection_ros.cpp @@ -0,0 +1,435 @@ +/* +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 "cloud_reprojection_ros_node.hpp" + +#include +#include +#include +#include + +#ifdef ROS2 + #include + #include + #include +#else + #include +#endif + +// Fixed Til (T_imu_lidar): lidar position in imu frame, transforms from lidar to imu +// TODO: Fill in the actual Til values for your sensor setup +static Eigen::Matrix4d getFixedTil() +{ + Eigen::Matrix4d Til = Eigen::Matrix4d::Identity(); + Til(0, 3) = -0.02663; + Til(1, 3) = 0.03447; + Til(2, 3) = 0.02174; + return Til; +} + +static bool fileExists(const std::string& filename) { + struct stat buffer; + return (stat(filename.c_str(), &buffer) == 0); +} + +#ifdef ROS2 +// Helper function to get package source directory for ROS2 +static std::string get_package_source_directory() { + std::string current_file = __FILE__; + size_t pos = current_file.find("/src/cloud_reprojection_ros.cpp"); + if (pos != std::string::npos) { + return current_file.substr(0, pos); + } + return ""; +} + +// ==================== ROS2 Implementation ==================== + +CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& options) + : Node("cloud_reprojection_node", options) +{ + loadParameters(); + + RCLCPP_INFO_STREAM(this->get_logger(), + "\n cloud_slam_topic: " << cloud_slam_topic_ + << "\n odometry_topic: " << odometry_topic_ + << "\n reprojected_image_topic: " << reprojected_image_topic_); + + cloud_sub_.subscribe(this, cloud_slam_topic_); + odom_sub_.subscribe(this, odometry_topic_); + + sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_); + sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_); + + RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized successfully"); +} + +void CloudReprojectionRosNode::loadParameters() +{ + // Declare and get parameters + this->declare_parameter("cloud_slam_topic", "/odin1/cloud_slam"); + this->declare_parameter("odometry_topic", "/odin1/odometry"); + this->declare_parameter("reprojected_image_topic", "/odin1/reprojected_image"); + + cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string(); + odometry_topic_ = this->get_parameter("odometry_topic").as_string(); + reprojected_image_topic_ = this->get_parameter("reprojected_image_topic").as_string(); + + // Load camera parameters from calib.yaml file directly + std::string package_path = get_package_source_directory(); + std::string calib_file = package_path + "/config/calib.yaml"; + + YAML::Node calib_config; + try { + calib_config = YAML::LoadFile(calib_file); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Failed to load calib.yaml: %s", e.what()); + rclcpp::shutdown(); + return; + } + + CloudReprojector::CameraParams cam_params; + try { + cam_params.image_width = calib_config["cam_0"]["image_width"].as(); + cam_params.image_height = calib_config["cam_0"]["image_height"].as(); + cam_params.A11 = calib_config["cam_0"]["A11"].as(); + cam_params.A12 = calib_config["cam_0"]["A12"].as(); + cam_params.A22 = calib_config["cam_0"]["A22"].as(); + cam_params.u0 = calib_config["cam_0"]["u0"].as(); + cam_params.v0 = calib_config["cam_0"]["v0"].as(); + cam_params.k2 = calib_config["cam_0"]["k2"].as(); + cam_params.k3 = calib_config["cam_0"]["k3"].as(); + cam_params.k4 = calib_config["cam_0"]["k4"].as(); + cam_params.k5 = calib_config["cam_0"]["k5"].as(); + cam_params.k6 = calib_config["cam_0"]["k6"].as(); + cam_params.k7 = calib_config["cam_0"]["k7"].as(); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Failed to parse camera parameters: %s", e.what()); + rclcpp::shutdown(); + return; + } + + // Load extrinsic parameters + CloudReprojector::ExtrinsicParams ext_params; + try { + auto Tcl_vec = calib_config["Tcl_0"].as>(); + + if (Tcl_vec.size() == 16) + { + for (int i = 0; i < 4; ++i) + for (int j = 0; j < 4; ++j) + ext_params.Tcl(i, j) = Tcl_vec[i * 4 + j]; + } + else + { + RCLCPP_ERROR(this->get_logger(), "Tcl_0 has invalid size: %zu (expected 16)", Tcl_vec.size()); + rclcpp::shutdown(); + return; + } + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Failed to parse Tcl_0: %s", e.what()); + rclcpp::shutdown(); + return; + } + + ext_params.Til = getFixedTil(); + ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til); + + RCLCPP_INFO_STREAM(this->get_logger(), "Loaded Tcl (camera to lidar):\n" << ext_params.Tcl); + RCLCPP_INFO_STREAM(this->get_logger(), "Fixed Til (lidar to imu):\n" << ext_params.Til); + RCLCPP_INFO_STREAM(this->get_logger(), "Calculated Tic:\n" << ext_params.Tic); + + RCLCPP_INFO(this->get_logger(), "Camera intrinsics:"); + RCLCPP_INFO(this->get_logger(), "Image size: %dx%d", cam_params.image_width, cam_params.image_height); + RCLCPP_INFO(this->get_logger(), "Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f", + cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0); + + reprojector_ = std::make_unique(); + if (!reprojector_->initialize(cam_params, ext_params)) + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize CloudReprojector"); + rclcpp::shutdown(); + } +} + +void CloudReprojectionRosNode::syncCallback( + const PointCloud2::ConstSharedPtr& cloud_msg, + const Odometry::ConstSharedPtr& odom_msg) +{ + pcl::PointCloud cloud_odom; + pcl::fromROSMsg(*cloud_msg, cloud_odom); + + if (cloud_odom.empty()) + { + RCLCPP_WARN(this->get_logger(), "Empty cloud_slam received"); + return; + } + + CloudReprojector::OdomPose odom_pose; + odom_pose.orientation = Eigen::Quaterniond( + odom_msg->pose.pose.orientation.w, + odom_msg->pose.pose.orientation.x, + odom_msg->pose.pose.orientation.y, + odom_msg->pose.pose.orientation.z + ); + odom_pose.position = Eigen::Vector3d( + odom_msg->pose.pose.position.x, + odom_msg->pose.pose.position.y, + odom_msg->pose.pose.position.z + ); + + cv::Mat reprojected_img = reprojector_->reprojectCloud(cloud_odom, odom_pose); + + auto img_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", reprojected_img).toImageMsg(); + reprojected_image_pub_.publish(*img_msg); +} + +// ==================== ROS2 Main ==================== +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + auto temp_node = std::make_shared("cloud_reprojection_check"); + + // Check if reprojection is enabled from control_command.yaml + std::string package_path = get_package_source_directory(); + std::string config_file = package_path + "/config/control_command.yaml"; + + try { + YAML::Node config = YAML::LoadFile(config_file); + std::cout << "config: " << config_file << std::endl; + if (!config["register_keys"] || !config["register_keys"]["sendreprojection"]) { + RCLCPP_INFO(temp_node->get_logger(), "sendreprojection parameter not found, cloud reprojection disabled."); + rclcpp::shutdown(); + return 0; + } + + int sendreprojection = config["register_keys"]["sendreprojection"].as(); + if (sendreprojection == 0) { + RCLCPP_INFO(temp_node->get_logger(), "Cloud reprojection will not be published."); + rclcpp::shutdown(); + return 0; + } + } catch (const std::exception& e) { + RCLCPP_ERROR(temp_node->get_logger(), "Failed to read config: %s", e.what()); + rclcpp::shutdown(); + return 1; + } + + // Wait for calib.yaml file to be generated by host_sdk_sample + std::string calib_file = package_path + "/config/calib.yaml"; + RCLCPP_INFO(temp_node->get_logger(), "Waiting for calib.yaml file at: %s", calib_file.c_str()); + + int wait_count = 0; + while (rclcpp::ok() && !fileExists(calib_file)) { + if (wait_count % 10 == 0) { + RCLCPP_INFO(temp_node->get_logger(), "Still waiting for calib.yaml file..."); + } + std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + wait_count++; + + // Timeout after 5 seconds + if (wait_count > 10) { + RCLCPP_ERROR(temp_node->get_logger(), "Timeout waiting for calib.yaml file"); + rclcpp::shutdown(); + return 1; + } + } + + if (!rclcpp::ok()) { + RCLCPP_INFO(temp_node->get_logger(), "Node shutdown before calib.yaml file was found."); + return 0; + } + + RCLCPP_INFO(temp_node->get_logger(), "Found calib.yaml file! Starting cloud reprojection node..."); + + auto node = std::make_shared(); + + RCLCPP_INFO(node->get_logger(), "CloudReprojectionRosNode started"); + + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} + +#else +// ==================== ROS1 Implementation ==================== + +CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh) + : nh_(nh), pnh_(pnh) +{ + loadParameters(); + + ROS_INFO_STREAM("\n cloud_slam_topic: " << cloud_slam_topic_ + << "\n odometry_topic: " << odometry_topic_ + << "\n reprojected_image_topic: " << reprojected_image_topic_); + + cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1); + odom_sub_.subscribe(nh_, odometry_topic_, 1); + + sync_ = std::make_shared(MySyncPolicy(10), cloud_sub_, odom_sub_); + sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2)); + + reprojected_image_pub_ = nh_.advertise(reprojected_image_topic_, 1); + + ROS_INFO("CloudReprojectionRosNode initialized successfully"); +} + +void CloudReprojectionRosNode::loadParameters() +{ + pnh_.param("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam")); + pnh_.param("odometry_topic", odometry_topic_, std::string("/odin1/odometry")); + pnh_.param("reprojected_image_topic", reprojected_image_topic_, std::string("/odin1/reprojected_image")); + + // Load camera parameters + CloudReprojector::CameraParams cam_params; + pnh_.param("cam_0/image_width", cam_params.image_width, 1600); + pnh_.param("cam_0/image_height", cam_params.image_height, 1296); + pnh_.param("cam_0/A11", cam_params.A11, 0.0); + pnh_.param("cam_0/A12", cam_params.A12, 0.0); + pnh_.param("cam_0/A22", cam_params.A22, 0.0); + pnh_.param("cam_0/u0", cam_params.u0, 0.0); + pnh_.param("cam_0/v0", cam_params.v0, 0.0); + pnh_.param("cam_0/k2", cam_params.k2, 0.0); + pnh_.param("cam_0/k3", cam_params.k3, 0.0); + pnh_.param("cam_0/k4", cam_params.k4, 0.0); + pnh_.param("cam_0/k5", cam_params.k5, 0.0); + pnh_.param("cam_0/k6", cam_params.k6, 0.0); + pnh_.param("cam_0/k7", cam_params.k7, 0.0); + + // Load extrinsic parameters + CloudReprojector::ExtrinsicParams ext_params; + std::vector Tcl_vec_param; + if (pnh_.getParam("Tcl_0", Tcl_vec_param) && Tcl_vec_param.size() == 16) + { + for (int i = 0; i < 4; ++i) + for (int j = 0; j < 4; ++j) + ext_params.Tcl(i, j) = Tcl_vec_param[i * 4 + j]; + } + else + { + ROS_ERROR("Tcl_0 param missing or invalid."); + ros::shutdown(); + return; + } + + ext_params.Til = getFixedTil(); + ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til); + + ROS_INFO_STREAM("Loaded Tcl (camera to lidar):\n" << ext_params.Tcl); + ROS_INFO_STREAM("Fixed Til (lidar to imu):\n" << ext_params.Til); + ROS_INFO_STREAM("Calculated Tic:\n" << ext_params.Tic); + + ROS_INFO("Camera intrinsics:"); + ROS_INFO("Image size: %dx%d", cam_params.image_width, cam_params.image_height); + ROS_INFO("Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f", + cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0); + + reprojector_ = std::make_unique(); + if (!reprojector_->initialize(cam_params, ext_params)) + { + ROS_ERROR("Failed to initialize CloudReprojector"); + ros::shutdown(); + } +} + +void CloudReprojectionRosNode::syncCallback( + const sensor_msgs::PointCloud2ConstPtr& cloud_msg, + const nav_msgs::OdometryConstPtr& odom_msg) +{ + pcl::PointCloud cloud_odom; + pcl::fromROSMsg(*cloud_msg, cloud_odom); + + if (cloud_odom.empty()) + { + ROS_WARN("Empty cloud_slam received"); + return; + } + + CloudReprojector::OdomPose odom_pose; + odom_pose.orientation = Eigen::Quaterniond( + odom_msg->pose.pose.orientation.w, + odom_msg->pose.pose.orientation.x, + odom_msg->pose.pose.orientation.y, + odom_msg->pose.pose.orientation.z + ); + odom_pose.position = Eigen::Vector3d( + odom_msg->pose.pose.position.x, + odom_msg->pose.pose.position.y, + odom_msg->pose.pose.position.z + ); + + cv::Mat reprojected_img = reprojector_->reprojectCloud(cloud_odom, odom_pose); + + sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", reprojected_img).toImageMsg(); + reprojected_image_pub_.publish(img_msg); +} + +// ==================== ROS1 Main ==================== +int main(int argc, char **argv) +{ + ros::init(argc, argv, "cloud_reprojection"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + + // Check if reprojection is enabled + int sendreprojection = 0; + pnh.param("register_keys/sendreprojection", sendreprojection, 0); + if(sendreprojection == 0) + { + ROS_INFO("Cloud reprojection will not be published."); + return 0; + } + + std::string calib_file_path; + pnh.param("calib_file_path", calib_file_path, ""); + + ROS_INFO("Waiting for calib.yaml file at: %s", calib_file_path.c_str()); + while(ros::ok() && !fileExists(calib_file_path)) + { + ROS_INFO_THROTTLE(5, "Still waiting for calib.yaml file..."); + ros::Duration(0.5).sleep(); + ros::spinOnce(); + } + + if(!ros::ok()) + { + ROS_INFO("Node shutdown before calib.yaml file was found."); + return 0; + } + + ROS_INFO("Found calib.yaml file! Loading parameters..."); + + std::string node_name = ros::this_node::getName(); + std::string rosparam_command = "rosparam load " + calib_file_path + " " + node_name; + int result = system(rosparam_command.c_str()); + + if(result == 0) + { + ROS_INFO("Successfully loaded parameters from calib.yaml to namespace: %s", node_name.c_str()); + } + else + { + ROS_ERROR("Failed to load parameters from calib.yaml"); + return 1; + } + + CloudReprojectionRosNode reprojection_node(nh, pnh); + ros::spin(); + return 0; +} +#endif diff --git a/src/cloud_reprojector.cpp b/src/cloud_reprojector.cpp new file mode 100644 index 0000000..7ede028 --- /dev/null +++ b/src/cloud_reprojector.cpp @@ -0,0 +1,113 @@ +/* +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 "cloud_reprojector.hpp" +#include + +CloudReprojector::CloudReprojector() +{ +} + +bool CloudReprojector::initialize(const CameraParams& cam_params, const ExtrinsicParams& ext_params) +{ + camera_params_ = cam_params; + extrinsic_params_ = ext_params; + + if (camera_params_.A11 < 1e-6 || camera_params_.A22 < 1e-6 || + camera_params_.u0 < 1e-6 || camera_params_.v0 < 1e-6) + { + return false; + } + + camera_model_ = std::make_unique( + camera_params_.image_width, camera_params_.image_height, + camera_params_.A11, camera_params_.A22, + camera_params_.u0, camera_params_.v0, + camera_params_.A12, + camera_params_.k2, camera_params_.k3, camera_params_.k4, + camera_params_.k5, camera_params_.k6, camera_params_.k7 + ); + + initialized_ = true; + return true; +} + +Eigen::Matrix4d CloudReprojector::calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til) +{ + // Tic = Til * Tlc = Til * Tcl.inverse() + Eigen::Matrix4d Tlc = Tcl.inverse(); + return Til * Tlc; +} + +Eigen::Matrix4d CloudReprojector::odomPoseToMatrix(const OdomPose& odom) const +{ + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + T.block<3, 3>(0, 0) = odom.orientation.toRotationMatrix(); + T(0, 3) = odom.position.x(); + T(1, 3) = odom.position.y(); + T(2, 3) = odom.position.z(); + return T; +} + +cv::Mat CloudReprojector::reprojectCloud(const pcl::PointCloud& cloud_odom, + const OdomPose& odom_pose) +{ + if (!initialized_) + { + return cv::Mat(); + } + + // T_odom_imu: imu pose in odom frame + Eigen::Matrix4d T_odom_imu = odomPoseToMatrix(odom_pose); + + // T_imu_odom: transforms points from odom frame to imu frame + Eigen::Matrix4d T_imu_odom = T_odom_imu.inverse(); + + // T_cam_imu = Tic.inverse(): transforms from imu to camera + Eigen::Matrix4d T_cam_imu = extrinsic_params_.Tic.inverse(); + + // T_cam_odom: transforms points from odom frame to camera frame + Eigen::Matrix4d T_cam_odom = T_cam_imu * T_imu_odom; + + pcl::PointCloud cloud_in_cam; + pcl::transformPointCloud(cloud_odom, cloud_in_cam, T_cam_odom); + + return projectCloudToImage(cloud_in_cam); +} + +cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud& cloud_in_cam) const +{ + cv::Mat img = cv::Mat::zeros(camera_params_.image_height, camera_params_.image_width, CV_8UC3); + img.setTo(cv::Scalar(255, 255, 255)); + + for (const auto& pt : cloud_in_cam) + { + if (pt.z <= 0.01) + continue; + + Eigen::Vector3d pt_cam(pt.x, pt.y, pt.z); + Eigen::Vector2d uv = camera_model_->world2cam(pt_cam); + + int u_int = static_cast(std::round(uv[0])); + int v_int = static_cast(std::round(uv[1])); + + if (u_int >= 0 && u_int < camera_params_.image_width && + v_int >= 0 && v_int < camera_params_.image_height) + { + cv::circle(img, cv::Point(u_int, v_int), point_radius_, + cv::Scalar(pt.b, pt.g, pt.r), -1); + } + } + + return img; +} diff --git a/src/host_sdk_sample.cpp b/src/host_sdk_sample.cpp index 136e0a0..3640d9e 100644 --- a/src/host_sdk_sample.cpp +++ b/src/host_sdk_sample.cpp @@ -46,9 +46,9 @@ limitations under the License. #include #include #endif -#define ros_driver_version "0.8.0" +#define ros_driver_version "0.9.0" #define required_firmware_version_major 0 -#define required_firmware_version_minor 9 +#define required_firmware_version_minor 10 #define required_firmware_version_patch 0 // Global variable declarations @@ -85,6 +85,21 @@ static std::shared_ptr g_renderer = nullptr; std::string calib_file_ = ""; static std::shared_ptr g_parser = nullptr; +static constexpr size_t PTP_SMOOTH_WINDOW_SIZE = 30; +static std::mutex g_ptp_mutex; +static std::deque g_ptp_delay_buf; +static std::deque g_ptp_offset_buf; +static std::atomic g_ptp_delay_smooth{0.0}; +static std::atomic g_ptp_offset_smooth{0.0}; + +double get_ptp_smoothed_delay() { + return g_ptp_delay_smooth.load(std::memory_order_relaxed); +} + +double get_ptp_smoothed_offset() { + return g_ptp_offset_smooth.load(std::memory_order_relaxed); +} + // usb device static std::string TARGET_VENDOR = "2207"; static std::string TARGET_PRODUCT = "0019"; @@ -106,6 +121,8 @@ int g_show_camerapose = 0; int g_strict_usb3_0_check = 0; int g_use_host_ros_time = 0; int g_save_log = 0; +int g_cloud_raw_confidence_threshold = 35; +int g_dtof_fps = 145; // DTOF sensor frame rate: 100 (10fps) or 145 (14.5fps) std::filesystem::path log_root_dir_; int g_custom_map_mode = 0; @@ -178,15 +195,7 @@ class RosNodeControlImpl : public RosNodeControlInterface { int getDtofSubframeODR() const override { return dtof_subframe_interval_time; } - - void setUseHostRosTime(bool use_host_ros_time) override { - pub_use_host_ros_time = use_host_ros_time; - } - - bool useHostRosTime() const override { - return pub_use_host_ros_time; - } - + void setSendOdomBaseLinkTF(bool send_odom_baselink_tf) override { pub_odom_baselink_tf = send_odom_baselink_tf; } @@ -195,10 +204,17 @@ class RosNodeControlImpl : public RosNodeControlInterface { return pub_odom_baselink_tf; } + void setCloudRawConfidenceThreshold(int threshold) { + cloud_raw_confidence_threshold = threshold; + } + int cloudRawConfidenceThreshold() const { + return cloud_raw_confidence_threshold; + } private: int dtof_subframe_interval_time = 0; bool pub_use_host_ros_time = false; bool pub_odom_baselink_tf = false; + int cloud_raw_confidence_threshold = 35; }; static RosNodeControlImpl g_rosNodeControlImpl; @@ -988,6 +1004,45 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data) } } break; + case LIDAR_DT_NTP: + { + uint32_t data_len = data->stream.imageList[0].length; + if (data_len == sizeof(ptp_sync_data_t)) { + ptp_sync_data_t* ptp_data = (ptp_sync_data_t*)data->stream.imageList[0].pAddr; + { + std::lock_guard lock(g_ptp_mutex); + g_ptp_delay_buf.push_back(ptp_data->delay); + g_ptp_offset_buf.push_back(ptp_data->offset); + + if (g_ptp_delay_buf.size() > PTP_SMOOTH_WINDOW_SIZE) { + g_ptp_delay_buf.pop_front(); + } + if (g_ptp_offset_buf.size() > PTP_SMOOTH_WINDOW_SIZE) { + g_ptp_offset_buf.pop_front(); + } + + double delay_sum = 0.0; + for (double v : g_ptp_delay_buf) delay_sum += v; + double offset_sum = 0.0; + for (double v : g_ptp_offset_buf) offset_sum += v; + + if (!g_ptp_delay_buf.empty()) { + g_ptp_delay_smooth.store(delay_sum / static_cast(g_ptp_delay_buf.size()), std::memory_order_relaxed); + } + if (!g_ptp_offset_buf.empty()) { + g_ptp_offset_smooth.store(offset_sum / static_cast(g_ptp_offset_buf.size()), std::memory_order_relaxed); + } + } + + // std::cout << std::setprecision(16) + // << "PTP delay: " << ptp_data->delay + // << " offset:" << ptp_data->offset + // << " smooth_delay:" << get_ptp_smoothed_delay() + // << " smooth_offset:" << get_ptp_smoothed_offset() + // << std::endl; + } + } + break; default: printf("Unknown lidar data type: %x", data->type); return; @@ -1292,6 +1347,44 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach #endif } + + // Set DTOF sensor frame rate based on configuration + // Supported values: 100 (10fps) or 145 (14.5fps) + lidar_depth_para_t dtofpara; + if (g_dtof_fps == 100) { + dtofpara.odr = LIDAR_DEPTH_ODR_10HZ; + } else if (g_dtof_fps == 145) { + dtofpara.odr = LIDAR_DEPTH_ODR_14_5HZ; + } else { + // Default to 14.5Hz if invalid value + dtofpara.odr = LIDAR_DEPTH_ODR_14_5HZ; + #ifdef ROS2 + RCLCPP_WARN(rclcpp::get_logger("ros[host_sdk_sample]"), + "Invalid dtof_fps value: %d, using default 14.5fps", g_dtof_fps); + #else + ROS_WARN("Invalid dtof_fps value: %d, using default 14.5fps", g_dtof_fps); + #endif + } + + if(lidar_set_depth_parameter(odinDevice, &dtofpara)) { + printf("set depth parameter failed.\n"); + #ifdef ROS2 + RCLCPP_WARN(rclcpp::get_logger("ros[host_sdk_sample]"), "set depth parameter failed"); + #else + ROS_WARN("set depth parameter failed"); + #endif + return; + } + + // Log the configured frame rate + #ifdef ROS2 + RCLCPP_INFO(rclcpp::get_logger("ros[host_sdk_sample]"), + "DTOF sensor frame rate set to %.1f fps", g_dtof_fps / 10.0); + #else + ROS_INFO("DTOF sensor frame rate set to %.1f fps", g_dtof_fps / 10.0); + #endif + + if (lidar_set_mode(odinDevice, type)) { #ifdef ROS2 RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Set mode failed"); @@ -1554,6 +1647,9 @@ int main(int argc, char *argv[]) g_sendrgb = get_key_value("sendrgb", 1); g_sendimu = get_key_value("sendimu", 1); g_senddtof = get_key_value("senddtof", 1); + g_cloud_raw_confidence_threshold = get_key_value("cloud_raw_confidence_threshold", 35); + g_rosNodeControlImpl.setCloudRawConfidenceThreshold(g_cloud_raw_confidence_threshold); + g_dtof_fps = get_key_value("dtof_fps", 145); // Read DTOF frame rate from config (100=10fps, 145=14.5fps) g_sendodom = get_key_value("sendodom", 1); g_send_odom_baselink_tf = get_key_value("send_odom_baselink_tf", 0); g_sendcloudslam = get_key_value("sendcloudslam", 0); @@ -1571,10 +1667,6 @@ int main(int argc, char *argv[]) g_use_host_ros_time = get_key_value("use_host_ros_time", 0); g_save_log = get_key_value("save_log", 0); - if (g_use_host_ros_time) { - g_rosNodeControlImpl.setUseHostRosTime(true); - } - if (g_send_odom_baselink_tf) { g_rosNodeControlImpl.setSendOdomBaseLinkTF(true); }