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

test ver

Browse Source
This commit is contained in:
jingyang-huang
2026-04-02 16:11:40 +08:00
parent 9ca6cf61f3
commit 84c6318121
18 changed files with 879 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+20
View File
@@ -209,6 +209,12 @@ if(ROS_VERSION STREQUAL "ROS1")
${PCL_LIBRARIES}
)
add_executable(image_overlay_node src/image_overlay_node.cpp)
target_link_libraries(image_overlay_node
${catkin_LIBRARIES}
${OpenCV_LIBS}
)
# Installation rules
install(TARGETS host_sdk_sample
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -340,12 +346,26 @@ elseif(ROS_VERSION STREQUAL "ROS2")
message_filters
)
add_executable(image_overlay_node src/image_overlay_node.cpp)
target_compile_definitions(image_overlay_node PRIVATE ROS2)
target_link_libraries(image_overlay_node
${OpenCV_LIBS}
)
ament_target_dependencies(image_overlay_node
rclcpp
sensor_msgs
cv_bridge
image_transport
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
image_overlay_node
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
config/control_command.yaml
+24 -1
View File
@@ -6,7 +6,7 @@ register_keys:
# 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
use_host_ros_time: 0 # Changed to 0 to use device timestamp without PTP offset correction
streamctrl: 1 # 0: off; 1: on
@@ -25,6 +25,14 @@ register_keys:
# IMU data
sendimu: 1 # 0: off; 1: on
# SDK IMU smooth sending feature
# When enabled, SDK will send IMU data at precise intervals (default 400Hz)
# using a dedicated high-priority thread to reduce jitter and timing variance
enable_imu_smooth: 0 # 0: disable SDK IMU smooth sending; 1: enable (default)
# SDK IMU smooth sending frequency in Hz (only effective when enable_imu_smooth = 1)
imu_smooth_frequency: 400 # 1-1000 Hz, recommended 400 Hz
# Odometry data
sendodom: 1 # 0: off; 1: on
@@ -57,6 +65,14 @@ register_keys:
# Processed on host device
sendreprojection: 0 # 0: off; 1: on
# image overlay settings - overlays reprojected points on camera image
# Processed on host device
sendoverlay: 0 # 0: off; 1: on
overlay_reprojected_topic: "/odin1/reprojected_image" # reprojected image topic
overlay_camera_topic: "/odin1/image" # camera image topic (undistorted)
overlay_output_topic: "/odin1/overlay_image" # output overlay image topic
overlay_alpha: 0.6 # blend alpha (0.0-1.0, higher = more reproj color)
# 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.
@@ -80,3 +96,10 @@ register_keys:
# To get the mapping result file, please use the set_param.sh script provided: "./set_param.sh save_map 1"
mapping_result_dest_dir: "" # "": if not specified, save to default location of {ws}/src/odin_ros_driver/map/{driver_start_time}/
mapping_result_file_name: "" # "": if not specified, save to location above with default file name of map_{map_save_time}.bin
# Image mask transfer settings
sendimagemask: 0 # 0: off; 1: on - transfer image mask to device on startup
image_mask_abs_path: "" # absolute path to the image mask file (e.g., /path/to/mask.png(1600x1296))
# Algorithm reset settings
resetalgo: 0 # 0: off; 1: on - send algo_reset command to device on startup
config/odin_ros2.rviz
+8 -8
View File
@@ -385,7 +385,7 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Enabled: false
Name: dense_depth_demo
- Class: rviz_default_plugins/TF
Enabled: true
@@ -460,9 +460,9 @@ Visualization Manager:
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.28456413745880127
Y: 1.8771981000900269
Z: 0.38664406538009644
X: 0.2568470537662506
Y: 2.1451337337493896
Z: 0.3774382472038269
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
@@ -483,16 +483,16 @@ Window Geometry:
collapsed: false
Image_undistort:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d0061006700650300000293000000dd000002d400000205fb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000044b0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004910000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1850
X: 70
Y: 27
Width: 1920
X: 540
Y: 124
cloudslam_reprojected:
collapsed: false
dense_depth_image:
include/cloud_reprojection_ros_node.hpp
+12 -4
View File
@@ -23,6 +23,7 @@ limitations under the License.
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#else
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
@@ -32,6 +33,7 @@ limitations under the License.
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#endif
#include <pcl_conversions/pcl_conversions.h>
@@ -56,12 +58,14 @@ private:
std::string cloud_slam_topic_;
std::string odometry_topic_;
std::string wiwc_topic_;
std::string reprojected_image_topic_;
message_filters::Subscriber<PointCloud2> cloud_sub_;
message_filters::Subscriber<Odometry> odom_sub_;
message_filters::Subscriber<Odometry> wiwc_sub_;
typedef message_filters::sync_policies::ExactTime<PointCloud2, Odometry> MySyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
@@ -71,7 +75,8 @@ private:
void loadParameters();
void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg);
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg);
};
#else
class CloudReprojectionRosNode
@@ -84,12 +89,14 @@ private:
std::string cloud_slam_topic_;
std::string odometry_topic_;
std::string wiwc_topic_;
std::string reprojected_image_topic_;
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
message_filters::Subscriber<nav_msgs::Odometry> wiwc_sub_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> MySyncPolicy;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
std::shared_ptr<Sync> sync_;
@@ -99,6 +106,7 @@ private:
void loadParameters();
void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const nav_msgs::OdometryConstPtr& odom_msg);
const nav_msgs::OdometryConstPtr& odom_msg,
const nav_msgs::OdometryConstPtr& wiwc_msg);
};
#endif
include/cloud_reprojector.hpp
+7
View File
@@ -63,6 +63,13 @@ public:
const CameraParams& getCameraParams() const { return camera_params_; }
const ExtrinsicParams& getExtrinsicParams() const { return extrinsic_params_; }
// Update extrinsic parameters at runtime with real-time values from module
void updateExtrinsics(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til) {
extrinsic_params_.Tcl = Tcl;
extrinsic_params_.Til = Til;
extrinsic_params_.Tic = calculateTic(Tcl, Til);
}
static Eigen::Matrix4d calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til);
private:
include/host_sdk_sample.h
+85 -1
View File
@@ -1019,12 +1019,25 @@ void publishRgb(capture_Image_List_t *stream) {
for (int idx = 0; idx < 16; ++idx) {
T_CL(idx / 4, idx % 4) = static_cast<double>(odom_data->pose_cov[idx]);
}
// Force last row to be [0, 0, 0, 1] for valid transformation matrix
T_CL(3, 0) = 0.0; T_CL(3, 1) = 0.0; T_CL(3, 2) = 0.0; T_CL(3, 3) = 1.0;
// Build TIL matrix (4x4) from twist_cov
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
for (int idx = 0; idx < 16; ++idx) {
T_IL(idx / 4, idx % 4) = static_cast<double>(odom_data->twist_cov[idx]);
}
// Force last row to be [0, 0, 0, 1] for valid transformation matrix
T_IL(3, 0) = 0.0; T_IL(3, 1) = 0.0; T_IL(3, 2) = 0.0; T_IL(3, 3) = 1.0;
// Debug print to compare with cloud_reprojection values
// static int host_print_count = 0;
// if (host_print_count++) {
// std::cout << "=== host_sdk_sample T_CL from odom_data->pose_cov ===" << std::endl;
// std::cout << T_CL << std::endl;
// std::cout << "=== host_sdk_sample T_IL from odom_data->twist_cov ===" << std::endl;
// std::cout << T_IL << std::endl;
// }
// Extract rotation and translation from T_CL
Eigen::Matrix3d RCL = T_CL.block<3, 3>(0, 0);
@@ -1033,7 +1046,20 @@ void publishRgb(capture_Image_List_t *stream) {
// Extract rotation and translation from T_IL
Eigen::Matrix3d RIL = T_IL.block<3, 3>(0, 0);
Eigen::Vector3d TIL = T_IL.block<3, 1>(0, 3);
// Debug print extracted RCL and TCL
// static int rcl_print_count = 0;
// if (rcl_print_count++ ) {
// std::cout << "=== host_sdk_sample RCL (3x3 rotation from T_CL) ===" << std::endl;
// std::cout << RCL << std::endl;
// std::cout << "=== host_sdk_sample TCL (3x1 translation from T_CL) ===" << std::endl;
// std::cout << TCL.transpose() << std::endl;
// std::cout << "=== host_sdk_sample RIL (3x3 rotation from T_IL) ===" << std::endl;
// std::cout << RIL << std::endl;
// std::cout << "=== host_sdk_sample TIL (3x1 translation from T_IL) ===" << std::endl;
// std::cout << TIL.transpose() << std::endl;
// }
// Save RIL, TIL, RCL, TCL to YAML file
static int save_count = 0;
static int index_count = 0;
@@ -1096,6 +1122,59 @@ void publishRgb(capture_Image_List_t *stream) {
}
// Publish WIWC data (T_CL and T_IL extrinsics) as a separate topic
void publishWiwc(capture_Image_List_t* stream) {
uint32_t data_len = stream->imageList[0].length;
if (data_len != sizeof(ros_odom_convert_complete_t)) {
return;
}
ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr;
#ifdef ROS2
auto msg = nav_msgs::msg::Odometry();
msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns, node_);
msg.header.frame_id = "odom";
#else
nav_msgs::Odometry msg;
msg.header.stamp = make_aligned_stamp(odom_data->timestamp_ns);
msg.header.frame_id = "odom";
#endif
// Store T_CL in pose.covariance (first 16 elements)
// Force last row to be [0, 0, 0, 1] for valid transformation matrix
for (int i = 0; i < 16; ++i) {
msg.pose.covariance[i] = odom_data->pose_cov[i];
}
msg.pose.covariance[12] = 0.0;
msg.pose.covariance[13] = 0.0;
msg.pose.covariance[14] = 0.0;
msg.pose.covariance[15] = 1.0;
// Fill remaining with zeros
for (int i = 16; i < 36; ++i) {
msg.pose.covariance[i] = 0.0;
}
// Store T_IL in twist.covariance (first 16 elements)
// Force last row to be [0, 0, 0, 1] for valid transformation matrix
for (int i = 0; i < 16; ++i) {
msg.twist.covariance[i] = odom_data->twist_cov[i];
}
msg.twist.covariance[12] = 0.0;
msg.twist.covariance[13] = 0.0;
msg.twist.covariance[14] = 0.0;
msg.twist.covariance[15] = 1.0;
// Fill remaining with zeros
for (int i = 16; i < 36; ++i) {
msg.twist.covariance[i] = 0.0;
}
#ifdef ROS2
wiwc_publisher_->publish(msg);
#else
wiwc_publisher_.publish(msg);
#endif
}
void publishOdometry(capture_Image_List_t* stream, OdometryType odom_type, bool show_path, bool show_camerapose) {
@@ -1161,6 +1240,7 @@ void publishRgb(capture_Image_List_t *stream) {
msg.twist.twist.angular.y = static_cast<double>(odom_data->angular_velocity[1]) / 1e6;
msg.twist.twist.angular.z = static_cast<double>(odom_data->angular_velocity[2]) / 1e6;
// Copy original covariance data
for (int i = 0; i < 36; ++i) {
msg.pose.covariance[i] = odom_data->pose_cov[i];
}
@@ -1606,6 +1686,7 @@ private:
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);
wiwc_publisher_ = node_->create_publisher<ros::Odometry>("odin1/wiwc", qos_small);
tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(node_);
#endif
}
@@ -1623,6 +1704,7 @@ private:
compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 100);
undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 100);
intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 100);
wiwc_publisher_ = nh.advertise<ros::Odometry>("odin1/wiwc", 100);
tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>();
}
#endif
@@ -1643,6 +1725,7 @@ private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr undistort_rgb_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr intensity_gray_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_camera_pose_visual_;
rclcpp::Publisher<ros::Odometry>::SharedPtr wiwc_publisher_;
camera_pose_visualization cameraposevisual_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
#else
@@ -1661,6 +1744,7 @@ private:
ros::Publisher compressed_rgb_pub_; // New compressed image publisher
ros::Publisher undistort_rgb_pub_;
ros::Publisher intensity_gray_pub_;
ros::Publisher wiwc_publisher_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
#endif
};
include/image_overlay_node.hpp
+91
View File
@@ -0,0 +1,91 @@
/*
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/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <mutex>
#else
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#endif
#include <opencv2/opencv.hpp>
#include <string>
#include <memory>
#ifdef ROS2
class ImageOverlayNode : public rclcpp::Node
{
public:
ImageOverlayNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
private:
using Image = sensor_msgs::msg::Image;
std::string reprojected_topic_;
std::string camera_topic_;
std::string overlay_topic_;
double alpha_; // blend alpha for overlay
rclcpp::Subscription<Image>::SharedPtr reproj_sub_;
rclcpp::Subscription<Image>::SharedPtr camera_sub_;
rclcpp::Publisher<Image>::SharedPtr overlay_pub_;
// Cache latest images
cv::Mat latest_reproj_img_;
cv::Mat latest_camera_img_;
std_msgs::msg::Header latest_header_;
std::mutex mutex_;
void reprojCallback(const Image::ConstSharedPtr& msg);
void cameraCallback(const Image::ConstSharedPtr& msg);
void publishOverlay();
};
#else
#include <mutex>
class ImageOverlayNode
{
public:
ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);
private:
ros::NodeHandle nh_, pnh_;
std::string reprojected_topic_;
std::string camera_topic_;
std::string overlay_topic_;
double alpha_; // blend alpha for overlay
ros::Subscriber reproj_sub_;
ros::Subscriber camera_sub_;
ros::Publisher overlay_pub_;
// Cache latest images
cv::Mat latest_reproj_img_;
cv::Mat latest_camera_img_;
std_msgs::Header latest_header_;
std::mutex mutex_;
void reprojCallback(const sensor_msgs::ImageConstPtr& msg);
void cameraCallback(const sensor_msgs::ImageConstPtr& msg);
void publishOverlay();
};
#endif
include/lidar_api.h
+35
View File
@@ -244,6 +244,18 @@ int lidar_get_custom_parameter(device_handle device, const char* param_name, int
*/
int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
/**
* @brief Set the image mask file for the device
*
* Read & send specified image mask file to device
*
* @param device Handle to the target device
* @param abs_path Absolute path to the image mask file (e.g., mask.png)
* @return int 0 on success, -1 on failure, -2 if file transfer in progress
*/
int lidar_set_image_mask(device_handle device, const char* abs_path);
/**
* @brief enable device log
*
@@ -266,6 +278,29 @@ int lidar_get_custom_parameter(device_handle device, const char* param_name, int
*/
int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
/**
* @brief Enable or disable IMU smooth sending feature
*
* When enabled, IMU data will be sent at precise intervals (default 400Hz)
* using a dedicated high-priority thread to reduce jitter and timing variance.
* When disabled, IMU data will be sent immediately upon reception.
*
* @param enable 1 to enable smooth sending, 0 to disable
* @return int 0 on success, -1 on failure
*/
int lidar_enable_imu_smooth_sending(int enable);
/**
* @brief Set IMU smooth sending frequency
*
* Set the target frequency for IMU smooth sending. Only effective when
* smooth sending is enabled via lidar_enable_imu_smooth_sending().
*
* @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
* @return int 0 on success, -1 on failure
*/
int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
#ifdef __cplusplus
}
#endif
include/yaml_parser.h
+1 -1
View File
@@ -87,7 +87,7 @@ private:
std::map<std::string, std::string> register_keys_str_val_;
std::map<std::string, ParameterValue> custom_parameters_;
std::unordered_set<std::string> allowed_key_w_str_val = {"relocalization_map_abs_path", "mapping_result_dest_dir", "mapping_result_file_name"};
std::unordered_set<std::string> allowed_key_w_str_val = {"relocalization_map_abs_path", "mapping_result_dest_dir", "mapping_result_file_name", "image_mask_abs_path"};
};
}
launch_ROS1/odin1_ros1.launch
+5
View File
@@ -26,6 +26,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>
<!-- Image overlay node - overlays reprojected points on camera image -->
<node pkg="odin_ros_driver" type="image_overlay_node" name="image_overlay_node" output="screen" >
<rosparam command="load" file="$(find odin_ros_driver)/config/control_command.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
+14 -1
View File
@@ -65,6 +65,18 @@ def generate_launch_description():
parameters=[reprojection_params]
)
# Image overlay node - overlays reprojected points on camera image
overlay_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
with open(overlay_config_path, 'r') as f:
overlay_params = yaml.safe_load(f)
image_overlay_node = Node(
package='odin_ros_driver',
executable='image_overlay_node',
name='image_overlay_node',
output='screen',
parameters=[overlay_params]
)
# Create RViz2 node - loads specified configuration file
rviz_node = Node(
package='rviz2',
@@ -81,6 +93,7 @@ def generate_launch_description():
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
ld.add_action(image_overlay_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
+18 -17
View File
@@ -2,27 +2,28 @@
<package format="3">
<name>odin_ros_driver</name>
<version>0.0.1</version>
<description>ROS2 driver for Odin sensor</description>
<description>ROS driver for Odin sensor</description>
<maintainer email="[email protected]">rlk</maintainer>
<license>Apache 2.0</license>
<!-- ROS2 uses colcon as the build tool -->
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- ROS2 dependencies -->
<depend>rclcpp</depend>
<!-- System dependencies -->
<!-- ROS1 uses catkin as the build tool -->
<buildtool_depend>catkin</buildtool_depend>
<!-- ROS1 dependencies -->
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<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>
<!-- 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>
src/cloud_reprojection_ros.cpp
+74 -6
View File
@@ -63,14 +63,16 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
RCLCPP_INFO_STREAM(this->get_logger(),
"\n cloud_slam_topic: " << cloud_slam_topic_
<< "\n odometry_topic: " << odometry_topic_
<< "\n wiwc_topic: " << wiwc_topic_
<< "\n reprojected_image_topic: " << reprojected_image_topic_);
cloud_sub_.subscribe(this, cloud_slam_topic_);
odom_sub_.subscribe(this, odometry_topic_);
wiwc_sub_.subscribe(this, wiwc_topic_);
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_);
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_);
sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this,
std::placeholders::_1, std::placeholders::_2));
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_);
@@ -82,10 +84,12 @@ 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>("wiwc_topic", "/odin1/wiwc");
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();
wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
reprojected_image_topic_ = this->get_parameter("reprojected_image_topic").as_string();
// Load camera parameters from calib.yaml file directly
@@ -167,8 +171,13 @@ void CloudReprojectionRosNode::loadParameters()
void CloudReprojectionRosNode::syncCallback(
const PointCloud2::ConstSharedPtr& cloud_msg,
const Odometry::ConstSharedPtr& odom_msg)
const Odometry::ConstSharedPtr& odom_msg,
const Odometry::ConstSharedPtr& wiwc_msg)
{
// Debug: print that syncCallback is called
static int sync_count = 0;
// RCLCPP_INFO(this->get_logger(), "=== syncCallback called, count: %d ===", ++sync_count);
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
pcl::fromROSMsg(*cloud_msg, cloud_odom);
@@ -178,6 +187,45 @@ void CloudReprojectionRosNode::syncCallback(
return;
}
// Extract real-time extrinsics from WIWC message covariance fields
// pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values)
Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
for (int i = 0; i < 16; ++i) {
T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
}
// Update extrinsics if valid (not identity matrix)
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
// // Debug print to compare with host_sdk_sample values
// static int print_count = 0;
// if (print_count++) {
// // Extract rotation (3x3) and translation (3x1) from T_CL
// Eigen::Matrix3d RCL = T_CL.block<3,3>(0,0);
// Eigen::Vector3d TCL = T_CL.block<3,1>(0,3);
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RCL (3x3 rotation from T_CL) ===\n" << RCL);
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TCL (3x1 translation from T_CL) ===\n" << TCL.transpose());
// // Extract rotation (3x3) and translation (3x1) from T_IL
// Eigen::Matrix3d RIL = T_IL.block<3,3>(0,0);
// Eigen::Vector3d TIL = T_IL.block<3,1>(0,3);
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RIL (3x3 rotation from T_IL) ===\n" << RIL);
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TIL (3x1 translation from T_IL) ===\n" << TIL.transpose());
// RCLCPP_INFO(this->get_logger(), "T_CL_valid: %d, T_IL_valid: %d", T_CL_valid, T_IL_valid);
// if (T_CL_valid && T_IL_valid) {
// Eigen::Matrix4d Tic = CloudReprojector::calculateTic(T_CL, T_IL);
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection calculated Tic ===\n" << Tic);
// }
// }
if (T_CL_valid && T_IL_valid) {
reprojector_->updateExtrinsics(T_CL, T_IL);
}
CloudReprojector::OdomPose odom_pose;
odom_pose.orientation = Eigen::Quaterniond(
odom_msg->pose.pose.orientation.w,
@@ -276,13 +324,15 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
ROS_INFO_STREAM("\n cloud_slam_topic: " << cloud_slam_topic_
<< "\n odometry_topic: " << odometry_topic_
<< "\n wiwc_topic: " << wiwc_topic_
<< "\n reprojected_image_topic: " << reprojected_image_topic_);
cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1);
odom_sub_.subscribe(nh_, odometry_topic_, 1);
wiwc_sub_.subscribe(nh_, wiwc_topic_, 1);
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_);
sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2));
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_);
sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3));
reprojected_image_pub_ = nh_.advertise<sensor_msgs::Image>(reprojected_image_topic_, 1);
@@ -293,6 +343,7 @@ 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>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
pnh_.param<std::string>("reprojected_image_topic", reprojected_image_topic_, std::string("/odin1/reprojected_image"));
// Load camera parameters
@@ -349,7 +400,8 @@ void CloudReprojectionRosNode::loadParameters()
void CloudReprojectionRosNode::syncCallback(
const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const nav_msgs::OdometryConstPtr& odom_msg)
const nav_msgs::OdometryConstPtr& odom_msg,
const nav_msgs::OdometryConstPtr& wiwc_msg)
{
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
pcl::fromROSMsg(*cloud_msg, cloud_odom);
@@ -360,6 +412,22 @@ void CloudReprojectionRosNode::syncCallback(
return;
}
// Extract real-time extrinsics from WIWC message covariance fields
// pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values)
Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
for (int i = 0; i < 16; ++i) {
T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
}
// Update extrinsics if valid (not identity matrix)
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
if (T_CL_valid && T_IL_valid) {
reprojector_->updateExtrinsics(T_CL, T_IL);
}
CloudReprojector::OdomPose odom_pose;
odom_pose.orientation = Eigen::Quaterniond(
odom_msg->pose.pose.orientation.w,
src/cloud_reprojector.cpp
+22 -7
View File
@@ -3,7 +3,7 @@ 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
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.
@@ -90,16 +90,31 @@ cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud<pcl::PointXY
cv::Mat img = cv::Mat::zeros(camera_params_.image_height, camera_params_.image_width, CV_8UC3);
img.setTo(cv::Scalar(255, 255, 255));
const double fx = camera_model_->fx();
const double fy = camera_model_->fy();
const double cx = camera_model_->cx();
const double cy = camera_model_->cy();
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]));
int u_int, v_int;
if (0)
{
// Pinhole projection (undistorted image)
u_int = static_cast<int>(std::round(fx * pt.x / pt.z + cx));
v_int = static_cast<int>(std::round(fy * pt.y / pt.z + cy));
}
else
{
// Distorted projection (original image)
Eigen::Vector3d pt_cam(pt.x, pt.y, pt.z);
Eigen::Vector2d uv = camera_model_->world2cam(pt_cam);
u_int = static_cast<int>(std::round(uv[0]));
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)
@@ -110,4 +125,4 @@ cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud<pcl::PointXY
}
return img;
}
}
src/host_sdk_sample.cpp
+209 -3
View File
@@ -23,8 +23,11 @@ limitations under the License.
#include <memory>
#include <opencv2/opencv.hpp>
#include <deque>
#include <queue>
#include <unistd.h>
#include <cstdlib>
#include <sched.h>
#include <pthread.h>
#include <cstring>
#include <sys/types.h>
#include <sys/wait.h>
@@ -46,7 +49,7 @@ limitations under the License.
#include <ros/package.h>
#include <ros/ros.h>
#endif
#define ros_driver_version "0.9.0"
#define ros_driver_version "0.10.0"
#define required_firmware_version_major 0
#define required_firmware_version_minor 10
#define required_firmware_version_patch 0
@@ -85,13 +88,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 constexpr size_t PTP_SMOOTH_WINDOW_SIZE = 300;
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};
// IMU dedicated processing thread
static std::atomic<bool> g_imu_thread_running(false);
static std::thread g_imu_thread;
static std::queue<imu_convert_data_t> g_imu_queue;
static std::mutex g_imu_queue_mutex;
static std::condition_variable g_imu_queue_cv;
static const size_t IMU_QUEUE_MAX_SIZE = 200;
double get_ptp_smoothed_delay() {
return g_ptp_delay_smooth.load(std::memory_order_relaxed);
}
@@ -109,6 +120,10 @@ int g_sendimu = 1;
int g_senddtof = 1;
int g_sendodom = 1;
int g_send_odom_baselink_tf = 0;
// SDK IMU smooth sending configuration
int g_enable_imu_smooth = 0;
int g_imu_smooth_frequency = 400;
int g_sendcloudslam = 0;
int g_sendcloudrender = 0;
int g_sendrgb_compressed = 0;
@@ -132,6 +147,11 @@ std::string g_relocalization_map_abs_path = "";
std::string g_mapping_result_dest_dir = "";
std::string g_mapping_result_file_name = "";
int g_send_image_mask = 0;
std::string g_image_mask_abs_path = "";
int g_reset_algo = 0;
const char* DEV_STATUS_CSV_FILE = "dev_status.csv";
FILE* dev_status_csv_file = nullptr;
@@ -764,6 +784,105 @@ void clear_all_queues() {
g_latest_bgr.reset();
g_latest_rgb_timestamp = 0;
g_has_rgb = false;
// Clear IMU queue
{
std::lock_guard<std::mutex> lock(g_imu_queue_mutex);
while (!g_imu_queue.empty()) {
g_imu_queue.pop();
}
}
}
// IMU dedicated processing thread routine
static void imu_thread_routine()
{
// Try to set higher thread priority for IMU processing
pthread_t this_thread = pthread_self();
struct sched_param param;
param.sched_priority = 70;
int ret = pthread_setschedparam(this_thread, SCHED_FIFO, &param);
if (ret != 0) {
ret = pthread_setschedparam(this_thread, SCHED_RR, &param);
}
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("imu_thread"), "IMU dedicated thread started (priority: %d)", param.sched_priority);
#else
ROS_INFO("IMU dedicated thread started (priority: %d)", param.sched_priority);
#endif
while (g_imu_thread_running) {
std::unique_lock<std::mutex> lock(g_imu_queue_mutex);
// Wait for IMU data
g_imu_queue_cv.wait(lock, []() {
return !g_imu_queue.empty() || !g_imu_thread_running;
});
if (!g_imu_thread_running) {
break;
}
// Process all pending IMU data
while (!g_imu_queue.empty() && g_imu_thread_running) {
imu_convert_data_t imu_data = g_imu_queue.front();
g_imu_queue.pop();
lock.unlock();
// Publish IMU data
if (g_ros_object && g_sendimu) {
g_ros_object->publishImu(&imu_data);
}
lock.lock();
}
}
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("imu_thread"), "IMU dedicated thread exiting");
#else
ROS_INFO("IMU dedicated thread exiting");
#endif
}
// Start IMU dedicated thread
static void start_imu_thread()
{
if (!g_imu_thread_running) {
g_imu_thread_running = true;
g_imu_thread = std::thread(imu_thread_routine);
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("imu_thread"), "IMU dedicated thread created");
#else
ROS_INFO("IMU dedicated thread created");
#endif
}
}
// Stop IMU dedicated thread
static void stop_imu_thread()
{
if (g_imu_thread_running) {
g_imu_thread_running = false;
g_imu_queue_cv.notify_all();
if (g_imu_thread.joinable()) {
g_imu_thread.join();
}
// Clear queue
std::lock_guard<std::mutex> lock(g_imu_queue_mutex);
while (!g_imu_queue.empty()) {
g_imu_queue.pop();
}
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("imu_thread"), "IMU dedicated thread stopped");
#else
ROS_INFO("IMU dedicated thread stopped");
#endif
}
}
// Lidar data callback
@@ -800,7 +919,15 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
case LIDAR_DT_RAW_IMU:
if (g_sendimu) {
imudata = (imu_convert_data_t *)data->stream.imageList[0].pAddr;
g_ros_object->publishImu(imudata);
// Enqueue IMU data for dedicated thread processing
{
std::lock_guard<std::mutex> lock(g_imu_queue_mutex);
if (g_imu_queue.size() >= IMU_QUEUE_MAX_SIZE) {
g_imu_queue.pop(); // Drop oldest if full
}
g_imu_queue.push(*imudata);
}
g_imu_queue_cv.notify_one();
}
update_count(&imu_rx_fps);
break;
@@ -999,6 +1126,9 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
break;
case LIDAR_DT_SLAM_WIWC:
{
// Always publish WIWC data for real-time extrinsics
g_ros_object->publishWiwc((capture_Image_List_t *)&data->stream);
if(g_record_data ) {
g_ros_object->recordrotate((capture_Image_List_t *)&data->stream);
}
@@ -1451,6 +1581,51 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
return;
}
}
// Transfer image mask if enabled
if (g_send_image_mask == 1) {
if (g_image_mask_abs_path != "" && std::filesystem::exists(g_image_mask_abs_path)) {
int ret = lidar_set_image_mask(odinDevice, g_image_mask_abs_path.c_str());
if (ret == 0) {
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Image mask set successfully: %s", g_image_mask_abs_path.c_str());
#else
ROS_INFO("Image mask set successfully: %s", g_image_mask_abs_path.c_str());
#endif
} else {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to set image mask: %s, error: %d", g_image_mask_abs_path.c_str(), ret);
#else
ROS_ERROR("Failed to set image mask: %s, error: %d", g_image_mask_abs_path.c_str(), ret);
#endif
}
} else {
#ifdef ROS2
RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Image mask path not set or file not found: %s", g_image_mask_abs_path.c_str());
#else
ROS_WARN("Image mask path not set or file not found: %s", g_image_mask_abs_path.c_str());
#endif
}
}
// Send algo_reset command if enabled
if (g_reset_algo == 1) {
int reset_value = 1;
int ret = lidar_set_custom_parameter(odinDevice, "algo_reset", &reset_value, sizeof(int));
if (ret == 0) {
#ifdef ROS2
RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Algo reset command sent successfully");
#else
ROS_INFO("Algo reset command sent successfully");
#endif
} else {
#ifdef ROS2
RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to send algo reset command, error: %d", ret);
#else
ROS_ERROR("Failed to send algo reset command, error: %d", ret);
#endif
}
}
lidar_data_callback_info_t data_callback_info;
data_callback_info.data_callback = lidar_data_callback;
@@ -1532,6 +1707,9 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
deviceConnected = true;
deviceDisconnected = false;
// Start IMU dedicated thread
start_imu_thread();
// Start custom parameter monitoring thread
g_param_monitor_running = true;
g_param_monitor_thread = std::thread(custom_parameter_monitor);
@@ -1567,6 +1745,9 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
deviceConnected = false;
deviceDisconnected = true;
// Stop IMU dedicated thread
stop_imu_thread();
// Stop custom parameter monitoring thread
g_param_monitor_running = false;
if (g_param_monitor_thread.joinable()) {
@@ -1647,6 +1828,10 @@ 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);
// SDK IMU smooth sending configuration
g_enable_imu_smooth = get_key_value("enable_imu_smooth", 0);
g_imu_smooth_frequency = get_key_value("imu_smooth_frequency", 400);
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)
@@ -1679,7 +1864,10 @@ int main(int argc, char *argv[])
g_relocalization_map_abs_path = get_key_str_value("relocalization_map_abs_path", "");
g_mapping_result_dest_dir = get_key_str_value("mapping_result_dest_dir", "");
g_mapping_result_file_name = get_key_str_value("mapping_result_file_name", "");
g_image_mask_abs_path = get_key_str_value("image_mask_abs_path", "");
g_send_image_mask = get_key_value("sendimagemask", 0);
g_reset_algo = get_key_value("resetalgo", 0);
g_custom_map_mode = g_parser->getCustomMapMode(2);
lidar_log_set_level(LIDAR_LOG_INFO);
@@ -1747,6 +1935,24 @@ int main(int argc, char *argv[])
return -1;
}
// Configure SDK IMU smooth sending AFTER lidar_system_init
// SDK now defaults to disabled, only enable if configured
if (g_enable_imu_smooth) {
lidar_enable_imu_smooth_sending(1);
lidar_set_imu_smooth_frequency(g_imu_smooth_frequency);
#ifdef ROS2
RCLCPP_INFO(node->get_logger(), "Enabling SDK IMU smooth sending at %d Hz", g_imu_smooth_frequency);
#else
ROS_INFO("Enabling SDK IMU smooth sending at %d Hz", g_imu_smooth_frequency);
#endif
} else {
#ifdef ROS2
RCLCPP_INFO(node->get_logger(), "SDK IMU smooth sending disabled");
#else
ROS_INFO("SDK IMU smooth sending disabled");
#endif
}
bool usbPresent = false;
bool usbVersionChecked = false;
src/image_overlay_node.cpp
+254
View File
@@ -0,0 +1,254 @@
/*
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 "image_overlay_node.hpp"
#ifdef ROS2
// ==================== ROS2 Implementation ====================
ImageOverlayNode::ImageOverlayNode(const rclcpp::NodeOptions& options)
: Node("image_overlay_node", options)
{
// Read from register_keys (same structure as control_command.yaml)
this->declare_parameter<std::string>("register_keys.overlay_reprojected_topic", "/odin1/reprojected_image");
this->declare_parameter<std::string>("register_keys.overlay_camera_topic", "/odin1/image/undistorted");
this->declare_parameter<std::string>("register_keys.overlay_output_topic", "/odin1/overlay_image");
this->declare_parameter<double>("register_keys.overlay_alpha", 0.6);
reprojected_topic_ = this->get_parameter("register_keys.overlay_reprojected_topic").as_string();
camera_topic_ = this->get_parameter("register_keys.overlay_camera_topic").as_string();
overlay_topic_ = this->get_parameter("register_keys.overlay_output_topic").as_string();
alpha_ = this->get_parameter("register_keys.overlay_alpha").as_double();
RCLCPP_INFO(this->get_logger(), "Subscribing to: %s and %s",
reprojected_topic_.c_str(), camera_topic_.c_str());
RCLCPP_INFO(this->get_logger(), "Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_);
// Independent subscriptions - no synchronization needed
reproj_sub_ = this->create_subscription<Image>(
reprojected_topic_, 10,
std::bind(&ImageOverlayNode::reprojCallback, this, std::placeholders::_1));
camera_sub_ = this->create_subscription<Image>(
camera_topic_, 10,
std::bind(&ImageOverlayNode::cameraCallback, this, std::placeholders::_1));
overlay_pub_ = this->create_publisher<sensor_msgs::msg::Image>(overlay_topic_, 10);
RCLCPP_INFO(this->get_logger(), "ImageOverlayNode initialized (no-sync mode)");
}
void ImageOverlayNode::reprojCallback(const Image::ConstSharedPtr& msg)
{
try {
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
{
std::lock_guard<std::mutex> lock(mutex_);
latest_reproj_img_ = cv_ptr->image.clone();
latest_header_ = msg->header;
}
} catch (cv_bridge::Exception& e) {
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (reproj): %s", e.what());
return;
}
publishOverlay();
}
void ImageOverlayNode::cameraCallback(const Image::ConstSharedPtr& msg)
{
try {
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
{
std::lock_guard<std::mutex> lock(mutex_);
latest_camera_img_ = cv_ptr->image.clone();
}
} catch (cv_bridge::Exception& e) {
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (camera): %s", e.what());
return;
}
publishOverlay();
}
void ImageOverlayNode::publishOverlay()
{
cv::Mat reproj_copy, camera_copy;
std_msgs::msg::Header header_copy;
{
std::lock_guard<std::mutex> lock(mutex_);
if (latest_reproj_img_.empty() || latest_camera_img_.empty()) {
return;
}
reproj_copy = latest_reproj_img_.clone();
camera_copy = latest_camera_img_.clone();
header_copy = latest_header_;
}
if (reproj_copy.size() != camera_copy.size()) {
RCLCPP_WARN(this->get_logger(),
"Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)",
reproj_copy.cols, reproj_copy.rows,
camera_copy.cols, camera_copy.rows);
return;
}
// Create overlay using alpha blending
// Replace white background in reproj with camera image, keep colored points
cv::Mat overlay = camera_copy.clone();
// Blend: where reproj has color (non-white), show reproj color semi-transparently
// where reproj is white (background), show camera image
for (int y = 0; y < reproj_copy.rows; ++y) {
for (int x = 0; x < reproj_copy.cols; ++x) {
cv::Vec3b reproj_pixel = reproj_copy.at<cv::Vec3b>(y, x);
// Check if pixel is not white (has point cloud color)
if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) {
// Blend reproj color with camera color
cv::Vec3b cam_pixel = camera_copy.at<cv::Vec3b>(y, x);
overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
static_cast<uchar>(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2])
);
}
// else: keep camera image (already in overlay)
}
}
auto overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg();
overlay_pub_->publish(*overlay_msg);
}
// ==================== ROS2 Main ====================
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<ImageOverlayNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
#else
// ==================== ROS1 Implementation ====================
ImageOverlayNode::ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
: nh_(nh), pnh_(pnh)
{
// Read from register_keys (same structure as control_command.yaml)
pnh_.param<std::string>("register_keys/overlay_reprojected_topic", reprojected_topic_, "/odin1/reprojected_image");
pnh_.param<std::string>("register_keys/overlay_camera_topic", camera_topic_, "/odin1/image/undistorted");
pnh_.param<std::string>("register_keys/overlay_output_topic", overlay_topic_, "/odin1/overlay_image");
pnh_.param<double>("register_keys/overlay_alpha", alpha_, 0.6);
ROS_INFO("Subscribing to: %s and %s", reprojected_topic_.c_str(), camera_topic_.c_str());
ROS_INFO("Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_);
// Independent subscriptions - no synchronization needed
reproj_sub_ = nh_.subscribe(reprojected_topic_, 10, &ImageOverlayNode::reprojCallback, this);
camera_sub_ = nh_.subscribe(camera_topic_, 10, &ImageOverlayNode::cameraCallback, this);
overlay_pub_ = nh_.advertise<sensor_msgs::Image>(overlay_topic_, 10);
ROS_INFO("ImageOverlayNode initialized (no-sync mode)");
}
void ImageOverlayNode::reprojCallback(const sensor_msgs::ImageConstPtr& msg)
{
try {
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
{
std::lock_guard<std::mutex> lock(mutex_);
latest_reproj_img_ = cv_ptr->image.clone();
latest_header_ = msg->header;
}
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception (reproj): %s", e.what());
return;
}
publishOverlay();
}
void ImageOverlayNode::cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
try {
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
{
std::lock_guard<std::mutex> lock(mutex_);
latest_camera_img_ = cv_ptr->image.clone();
}
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception (camera): %s", e.what());
return;
}
publishOverlay();
}
void ImageOverlayNode::publishOverlay()
{
cv::Mat reproj_copy, camera_copy;
std_msgs::Header header_copy;
{
std::lock_guard<std::mutex> lock(mutex_);
if (latest_reproj_img_.empty() || latest_camera_img_.empty()) {
return;
}
reproj_copy = latest_reproj_img_.clone();
camera_copy = latest_camera_img_.clone();
header_copy = latest_header_;
}
if (reproj_copy.size() != camera_copy.size()) {
ROS_WARN_THROTTLE(2, "Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)",
reproj_copy.cols, reproj_copy.rows, camera_copy.cols, camera_copy.rows);
return;
}
// Create overlay using alpha blending
cv::Mat overlay = camera_copy.clone();
for (int y = 0; y < reproj_copy.rows; ++y) {
for (int x = 0; x < reproj_copy.cols; ++x) {
cv::Vec3b reproj_pixel = reproj_copy.at<cv::Vec3b>(y, x);
if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) {
cv::Vec3b cam_pixel = camera_copy.at<cv::Vec3b>(y, x);
overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
static_cast<uchar>(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2])
);
}
}
}
sensor_msgs::ImagePtr overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg();
overlay_pub_.publish(overlay_msg);
}
// ==================== ROS1 Main ====================
int main(int argc, char **argv)
{
ros::init(argc, argv, "image_overlay_node");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
ImageOverlayNode node(nh, pnh);
ros::spin();
return 0;
}
#endif