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

feat<all> 1.add cloud_raw confidence filter threshold to config yaml}

Browse Source 
2.update sdk for ntp functions
          3.add set camera FPS function in yaml(10FPS,14.5FPS)
          4.release ros driver 0.9.0
          5.Add time alignment mode for odin1 device synchronization with host
          6.Add new time alignment option (use_host_ros_time=2) to align odin1 sensor timestamps to host time axis using PTP sync data
          7.Implement PTP smoothing with 30-sample moving average window for delay and offset calculations
          8.Refactor timestamp handling with make_aligned_stamp() helper function to centralize time conversion logic across all data types (IMU, point clouds, images, odometry)
          9.Add LIDAR_DT_NTP data type for receiving
          10.add imu data record
          11.add cloud reprojection demo
This commit is contained in:
mt-lifan
2026-02-11 15:47:50 +08:00
parent e51cf98658
commit 4dcf0e0a97
19 changed files with 1143 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+40
View File
@@ -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
README.md
+8 -2
View File
@@ -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. |
config/control_command.yaml
+15 -4
View File
@@ -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.
config/odin_ros.rviz
+24 -14
View File
@@ -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
collapsed: false
config/odin_ros2.rviz
+24 -11
View File
@@ -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
include/cloud_reprojection_ros_node.hpp
+104
View File
@@ -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 <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <image_transport/image_transport.hpp>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#else
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <nav_msgs/Odometry.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#endif
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include "cloud_reprojector.hpp"
#include <string>
#include <memory>
#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<PointCloud2> cloud_sub_;
message_filters::Subscriber<Odometry> odom_sub_;
typedef message_filters::sync_policies::ExactTime<PointCloud2, Odometry> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
image_transport::Publisher reprojected_image_pub_;
std::unique_ptr<CloudReprojector> 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<sensor_msgs::PointCloud2> cloud_sub_;
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
ros::Publisher reprojected_image_pub_;
std::unique_ptr<CloudReprojector> reprojector_;
void loadParameters();
void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const nav_msgs::OdometryConstPtr& odom_msg);
};
#endif
include/cloud_reprojector.hpp
+79
View File
@@ -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 <Eigen/Dense>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include "polynomial_camera.hpp"
#include <memory>
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<pcl::PointXYZRGB>& 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<pcl::PointXYZRGB>& cloud_in_cam) const;
CameraParams camera_params_;
ExtrinsicParams extrinsic_params_;
std::unique_ptr<mini_vikit::PolynomialCamera> camera_model_;
int point_radius_ = 4;
bool initialized_ = false;
};
include/data_logger.h
+6
View File
@@ -79,6 +79,7 @@ public:
//cloud_writer_ = std::make_unique<Writer>(root_dir_ / "OdinPointCloud.olx", opts.batch_size);
image_writer_ = std::make_unique<Writer>(root_dir_ / "OdinImage.bin", opts.batch_size);
roatation_writer_ = std::make_unique<Writer>(root_dir_ / "OdinRotate.bin", opts.batch_size);
imu_writer_ = std::make_unique<Writer>(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<uint8_t>&& blob) {
if (roatation_writer_) roatation_writer_->enqueue(std::move(blob));
}
void enqueueIMUFrame(std::vector<uint8_t>&& blob) {
if (imu_writer_) imu_writer_->enqueue(std::move(blob));
}
private:
struct Writer {
@@ -194,4 +199,5 @@ private:
std::unique_ptr<Writer> cloud_writer_;
std::unique_ptr<Writer> image_writer_;
std::unique_ptr<Writer> roatation_writer_;
std::unique_ptr<Writer> imu_writer_;
};
include/host_sdk_sample.h
+122 -124
View File
@@ -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<int64_t>(offset_s * 1e9);
const int64_t base_ns = static_cast<int64_t>(sensor_timestamp_ns);
const int64_t aligned_ns = base_ns - offset_ns;
ts_ns = (aligned_ns < 0) ? 0ULL : static_cast<uint64_t>(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<double>(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<uint8_t> blob;
blob.reserve(sizeof(double) + sizeof(float) * 6);
auto append_pod = [&](const auto& v) {
const uint8_t* p = reinterpret_cast<const uint8_t*>(&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<uint8_t*>(stream->imageList[2].pAddr);
uint16_t* confidence_data = static_cast<uint16_t*>(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<std::mutex> 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<sensor_msgs::msg::PointCloud2>(*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<double>(odom_data->pos[0]) / 1e6;
msg.pose.pose.position.y = static_cast<double>(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<double>(odom_data->pos[0]) / 1e6;
msg.pose.pose.position.y = static_cast<double>(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<ros::Imu>("odin1/imu", qos_profile);
rgb_pub_ = node_->create_publisher<ros::Image>("odin1/image", qos_profile);
cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", qos_profile);
xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", qos_profile);
odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", qos_profile);
odom_highfreq_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_highfreq", qos_profile);
path_publisher_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/path", qos_profile);
pub_camera_pose_visual_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/camera_pose_visual", qos_profile);
rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", qos_profile);
compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", qos_profile);
undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", qos_profile);
intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("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<ros::Imu>("odin1/imu", qos_small);
rgb_pub_ = node_->create_publisher<ros::Image>("odin1/image", qos_sensor);
cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", qos_sensor);
xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", qos_sensor);
odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", qos_small);
odom_highfreq_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_highfreq", qos_small);
path_publisher_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/path", qos_sensor);
pub_camera_pose_visual_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/camera_pose_visual", qos_sensor);
rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", qos_sensor);
compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", qos_small);
undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", qos_sensor);
intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/intensity_gray", qos_sensor);
tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(node_);
#endif
}
include/lidar_api.h
+12 -1
View File
@@ -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
include/lidar_api_type.h
+16 -1
View File
@@ -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
launch_ROS1/odin1_ros1.launch
+5
View File
@@ -21,6 +21,11 @@
<rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
<param name="calib_file_path" value="$(find odin_ros_driver)/config/calib.yaml"/>
</node>
<node pkg="odin_ros_driver" type="cloud_reprojection_node" name="cloud_reprojection_node" output="screen" >
<rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
<param name="calib_file_path" value="$(find odin_ros_driver)/config/calib.yaml"/>
</node>
<!-- Launch RViz with configuration -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)" output="screen"/>
launch_ROS2/odin1_ros2.launch.py
+16
View File
@@ -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
lib/liblydHostApi_amd.a
BIN
View File
Binary file not shown.
lib/liblydHostApi_arm.a
BIN
View File
Binary file not shown.
package.xml
+17 -18
View File
@@ -2,28 +2,27 @@
<package format="3">
<name>odin_ros_driver</name>
<version>0.0.1</version>
<description>ROS driver for Odin sensor</description>
<description>ROS2 driver for Odin sensor</description>
<maintainer email="[email protected]">rlk</maintainer>
<license>Apache 2.0</license>
<!-- ROS1 uses catkin as the build tool -->
<buildtool_depend>catkin</buildtool_depend>
<!-- ROS1 dependencies -->
<depend>roscpp</depend>
<!-- ROS2 uses colcon as the build tool -->
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- ROS2 dependencies -->
<depend>rclcpp</depend>
<!-- System dependencies -->
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<!-- System dependencies -->
<depend>eigen</depend>
<depend>opencv</depend>
<depend>yaml-cpp</depend>
<!-- Specify build type as catkin -->
<export>
<build_type>catkin</build_type>
</export>
</package>
<depend>pcl_conversions</depend>
<depend>message_filters</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<!-- Specify build type as ament -->
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
src/cloud_reprojection_ros.cpp
+435
View File
@@ -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 <fstream>
#include <sys/stat.h>
#include <thread>
#include <chrono>
#ifdef ROS2
#include <functional>
#include <yaml-cpp/yaml.h>
#include <rcpputils/filesystem_helper.hpp>
#else
#include <boost/bind.hpp>
#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<Sync>(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<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
this->declare_parameter<std::string>("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<int>();
cam_params.image_height = calib_config["cam_0"]["image_height"].as<int>();
cam_params.A11 = calib_config["cam_0"]["A11"].as<double>();
cam_params.A12 = calib_config["cam_0"]["A12"].as<double>();
cam_params.A22 = calib_config["cam_0"]["A22"].as<double>();
cam_params.u0 = calib_config["cam_0"]["u0"].as<double>();
cam_params.v0 = calib_config["cam_0"]["v0"].as<double>();
cam_params.k2 = calib_config["cam_0"]["k2"].as<double>();
cam_params.k3 = calib_config["cam_0"]["k3"].as<double>();
cam_params.k4 = calib_config["cam_0"]["k4"].as<double>();
cam_params.k5 = calib_config["cam_0"]["k5"].as<double>();
cam_params.k6 = calib_config["cam_0"]["k6"].as<double>();
cam_params.k7 = calib_config["cam_0"]["k7"].as<double>();
} 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<std::vector<double>>();
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<CloudReprojector>();
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<pcl::PointXYZRGB> 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<rclcpp::Node>("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<int>();
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<CloudReprojectionRosNode>();
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<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_);
sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2));
reprojected_image_pub_ = nh_.advertise<sensor_msgs::Image>(reprojected_image_topic_, 1);
ROS_INFO("CloudReprojectionRosNode initialized successfully");
}
void CloudReprojectionRosNode::loadParameters()
{
pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
pnh_.param<std::string>("reprojected_image_topic", reprojected_image_topic_, std::string("/odin1/reprojected_image"));
// Load camera parameters
CloudReprojector::CameraParams cam_params;
pnh_.param<int>("cam_0/image_width", cam_params.image_width, 1600);
pnh_.param<int>("cam_0/image_height", cam_params.image_height, 1296);
pnh_.param<double>("cam_0/A11", cam_params.A11, 0.0);
pnh_.param<double>("cam_0/A12", cam_params.A12, 0.0);
pnh_.param<double>("cam_0/A22", cam_params.A22, 0.0);
pnh_.param<double>("cam_0/u0", cam_params.u0, 0.0);
pnh_.param<double>("cam_0/v0", cam_params.v0, 0.0);
pnh_.param<double>("cam_0/k2", cam_params.k2, 0.0);
pnh_.param<double>("cam_0/k3", cam_params.k3, 0.0);
pnh_.param<double>("cam_0/k4", cam_params.k4, 0.0);
pnh_.param<double>("cam_0/k5", cam_params.k5, 0.0);
pnh_.param<double>("cam_0/k6", cam_params.k6, 0.0);
pnh_.param<double>("cam_0/k7", cam_params.k7, 0.0);
// Load extrinsic parameters
CloudReprojector::ExtrinsicParams ext_params;
std::vector<double> 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<CloudReprojector>();
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<pcl::PointXYZRGB> 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<std::string>("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
src/cloud_reprojector.cpp
+113
View File
@@ -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 <cmath>
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<mini_vikit::PolynomialCamera>(
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<pcl::PointXYZRGB>& 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<pcl::PointXYZRGB> 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<pcl::PointXYZRGB>& 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<int>(std::round(uv[0]));
int v_int = static_cast<int>(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;
}
src/host_sdk_sample.cpp
+107 -15
View File
@@ -46,9 +46,9 @@ limitations under the License.
#include <ros/package.h>
#include <ros/ros.h>
#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<rawCloudRender> g_renderer = nullptr;
std::string calib_file_ = "";
static std::shared_ptr<odin_ros_driver::YamlParser> g_parser = nullptr;
static constexpr size_t PTP_SMOOTH_WINDOW_SIZE = 30;
static std::mutex g_ptp_mutex;
static std::deque<double> g_ptp_delay_buf;
static std::deque<double> g_ptp_offset_buf;
static std::atomic<double> g_ptp_delay_smooth{0.0};
static std::atomic<double> 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<std::mutex> 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<double>(g_ptp_delay_buf.size()), std::memory_order_relaxed);
}
if (!g_ptp_offset_buf.empty()) {
g_ptp_offset_smooth.store(offset_sum / static_cast<double>(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);
}