diff --git a/README.md b/README.md index d68f3a8..9da957a 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ This driver package provides core functionality for point cloud SLAM application ## 1. Version -Current Version: v0.3.1 +Current Version: v0.4.0 ## 2. Preparation @@ -199,7 +199,7 @@ Internal parameters of the Odin ROS driver are defined in config/control_command | odin1/cloud_raw | Raw_Cloud Topic | | odin1/cloud_render | Render_Cloud Topic | | odin1/cloud_slam | Slam_PointCloud Topic | -| odin1/odometry_map | Odom Topic | +| odin1/odometry | Odom Topic | ## 5. FAQ ### 5.1 Segmentation fault upon re-launching host SDK diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h index a406f89..9452497 100644 --- a/include/host_sdk_sample.h +++ b/include/host_sdk_sample.h @@ -131,7 +131,7 @@ extern int g_sendcloudrender; #define ACC_SEN_SCALE 4096 #define PAI 3.14159265358979323846 #define GYRO_SEN_SCALE 16.4f - +#define DTOF_NUM_ROW_PER_GROUP 6 // Common functions inline float accel_convert(int16_t raw, int sen_scale) { return (raw * GD_ACCL_G / sen_scale); @@ -161,6 +161,15 @@ inline uint64_t ros_time_to_ns(const ros::Time &t) { #endif } +class RosNodeControlInterface { + public: + virtual ~RosNodeControlInterface() = default; + virtual void setDtofSubframeODR(int odr) = 0; + virtual int getDtofSubframeODR() const = 0; + }; + +RosNodeControlInterface* getRosNodeControl(); + // Multi-sensor publisher class class MultiSensorPublisher { public: @@ -498,12 +507,13 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m // Set point cloud fields sensor_msgs::PointCloud2Modifier modifier(*msg); modifier.setPointCloud2Fields( - 5, + 6, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, "z", 1, sensor_msgs::PointField::FLOAT32, "intensity", 1, sensor_msgs::PointField::UINT8, - "confidence", 1, sensor_msgs::PointField::UINT16 + "confidence", 1, sensor_msgs::PointField::UINT16, + "offset_time", 1, sensor_msgs::PointField::FLOAT32 ); modifier.resize(msg->height * msg->width); @@ -513,77 +523,88 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); sensor_msgs::PointCloud2Iterator iter_intensity(*msg, "intensity"); sensor_msgs::PointCloud2Iterator iter_confidence(*msg, "confidence"); + sensor_msgs::PointCloud2Iterator iter_offsettime(*msg, "offset_time"); float* xyz_data_f = static_cast(cloud.pAddr); int total_points = cloud.height * cloud.width; //std::cout << stream->imageCount << std::endl; + float dtof_subframe_odr = getRosNodeControl()->getDtofSubframeODR() / 1000.0f; + // printf("dtof_subframe_odr: %f\n", dtof_subframe_odr); int valid_points = 0; if (stream->imageCount == 4) { - uint8_t* intensity_data = static_cast(stream->imageList[2].pAddr); - uint16_t* confidence_data = static_cast(stream->imageList[3].pAddr); + uint8_t* intensity_data = static_cast(stream->imageList[2].pAddr); + uint16_t* confidence_data = static_cast(stream->imageList[3].pAddr); - for (int i = 0; i < total_points; ++i) { - if (confidence_data[i] < 35) { + for (int i = 0; i < total_points; ++i) { + if (confidence_data[i] < 35) { continue; } - // XYZ point - *iter_x = xyz_data_f[i * 3 + 2] / 1000.0f; ++iter_x; - *iter_y = -xyz_data_f[i * 3 + 0] / 1000.0f; ++iter_y; - *iter_z = xyz_data_f[i * 3 + 1] / 1000.0f; ++iter_z; - - *iter_intensity = intensity_data[i]; ++iter_intensity; - - *iter_confidence = confidence_data[i]; ++iter_confidence; - - valid_points++; + // XYZ point + *iter_x = xyz_data_f[i * 3 + 2] / 1000.0f; ++iter_x; + *iter_y = -xyz_data_f[i * 3 + 0] / 1000.0f; ++iter_y; + *iter_z = xyz_data_f[i * 3 + 1] / 1000.0f; ++iter_z; + + *iter_intensity = intensity_data[i]; ++iter_intensity; + *iter_confidence = confidence_data[i]; ++iter_confidence; + + if (dtof_subframe_odr > 0.0) { + int group = i / DTOF_NUM_ROW_PER_GROUP; + float timestamp_offset = group * 1.0 / dtof_subframe_odr; + + *iter_offsettime = timestamp_offset; + ++iter_offsettime; + } + + valid_points++; } } else { - uint16_t* intensity_data = static_cast(stream->imageList[2].pAddr); - - for (int i = 0; i < total_points; ++i) { - *iter_x = xyz_data_f[i * 4 + 2] / 1000.0f; ++iter_x; - *iter_y = -xyz_data_f[i * 4 + 0] / 1000.0f; ++iter_y; - *iter_z = xyz_data_f[i * 4 + 1] / 1000.0f; ++iter_z; + uint16_t* intensity_data = static_cast(stream->imageList[2].pAddr); - float intensity = (intensity_data[i] - 10) * 255.0f / (12500 - 10); - if (intensity > 255) { - *iter_intensity = 255; - } else if (intensity < 0) { - *iter_intensity = 0; - } else { - *iter_intensity = static_cast(intensity); + for (int i = 0; i < total_points; ++i) { + *iter_x = xyz_data_f[i * 4 + 2] / 1000.0f; ++iter_x; + *iter_y = -xyz_data_f[i * 4 + 0] / 1000.0f; ++iter_y; + *iter_z = xyz_data_f[i * 4 + 1] / 1000.0f; ++iter_z; + + float intensity = (intensity_data[i] - 10) * 255.0f / (12500 - 10); + if (intensity > 255) { + *iter_intensity = 255; + } else if (intensity < 0) { + *iter_intensity = 0; + } else { + *iter_intensity = static_cast(intensity); + } + ++iter_intensity; + + *iter_confidence = 0; + ++iter_confidence; + + valid_points++; } - ++iter_intensity; - - *iter_confidence = 0; - ++iter_confidence; - - valid_points++; } -} -{ - std::lock_guard lock(pcd_queue_mutex_); - - // Get actual point count - const int real_point_count = cloud.width * cloud.height; - // Create deep copy of point cloud - #ifdef ROS2 - auto msg_copy = std::make_shared(*msg); - #else - auto msg_copy = boost::make_shared(); - *msg_copy = *msg; // Deep copy - #endif - - // Queue management - if (pcd_queue_.size() >= 10) { - pcd_queue_.pop_front(); + + { + std::lock_guard lock(pcd_queue_mutex_); + + // Get actual point count + const int real_point_count = cloud.width * cloud.height; + // Create deep copy of point cloud + #ifdef ROS2 + auto msg_copy = std::make_shared(*msg); + #else + auto msg_copy = boost::make_shared(); + *msg_copy = *msg; // Deep copy + #endif + + // Queue management + if (pcd_queue_.size() >= 10) { + pcd_queue_.pop_front(); + } + + // Add to queue (using copy) + pcd_queue_.push_back(msg_copy); } - - // Add to queue (using copy) - pcd_queue_.push_back(msg_copy); -} // Publish point cloud #ifdef ROS2 @@ -596,103 +617,155 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m void publishRgb(capture_Image_List_t *stream) { buffer_List_t &image = stream->imageList[0]; - try { - const int height_nv12 = image.height * 3 / 2; - cv::Mat nv12_mat(height_nv12, image.width, CV_8UC1, image.pAddr); - cv::Mat bgr; - cv::cvtColor(nv12_mat, bgr, cv::COLOR_YUV2BGR_NV12); - - if (bgr.empty()) { + // old version yuv data + if (image.length == image.width * image.height * 3 / 2) { + try { + const int height_nv12 = image.height * 3 / 2; + cv::Mat nv12_mat(height_nv12, image.width, CV_8UC1, image.pAddr); + cv::Mat bgr; + cv::cvtColor(nv12_mat, bgr, cv::COLOR_YUV2BGR_NV12); + + if (bgr.empty()) { + #ifndef ROS2 + ROS_ERROR("Failed to convert NV12 to BGR"); + #endif + return; + } + + //Create ROS image message + #ifdef ROS2 + auto header = std::make_shared(); + header->stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation + header->frame_id = "camera_rgb_frame"; + + auto cv_image = std::make_shared(*header, "bgr8", bgr); + auto msg = cv_image->toImageMsg(); + + // Add to unified queue + if (g_sendcloudrender) { + std::lock_guard lock(rgb_queue_mutex_); + if (rgb_image_queue_.size() >= 10) { + rgb_image_queue_.pop_front(); + } + rgb_image_queue_.push_back(msg); + } + + // Publish original image message + rgb_pub_->publish(*msg); + + // Create compressed image message + auto compressed_msg = std::make_shared(); + compressed_msg->header = *header; + compressed_msg->format = "jpeg"; + + // Set compression parameters + std::vector compression_params; + compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); + compression_params.push_back(80); + + // Compress image + cv::imencode(".jpg", bgr, compressed_msg->data, compression_params); + + compressed_rgb_pub_->publish(*compressed_msg); + + #else + // ROS1 version + std_msgs::Header header; + header.stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation + header.frame_id = "camera_rgb_frame"; + + auto cv_image = boost::make_shared(header, "bgr8", bgr); + auto msg = cv_image->toImageMsg(); + + // Add to unified queue + if (g_sendcloudrender) { + std::lock_guard lock(rgb_queue_mutex_); + if (rgb_image_queue_.size() >= 10) { + rgb_image_queue_.pop_front(); + } + rgb_image_queue_.push_back(msg); + } + + // Publish original image message + rgb_pub_.publish(msg); + + // Publish compressed image - always publish + // Create compressed image message + sensor_msgs::CompressedImagePtr compressed_msg(new sensor_msgs::CompressedImage()); + compressed_msg->header = header; + compressed_msg->format = "jpeg"; + + // Set compression parameters + std::vector compression_params; + compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); + compression_params.push_back(80); // JPEG quality 80% + + // Compress image + cv::imencode(".jpg", bgr, compressed_msg->data, compression_params); + + compressed_rgb_pub_.publish(compressed_msg); + + #endif + + } catch (const cv::Exception& e) { #ifndef ROS2 - ROS_ERROR("Failed to convert NV12 to BGR"); + ROS_ERROR("OpenCV error in publishRgb: %s", e.what()); + #endif + } catch (const std::exception& e) { + #ifndef ROS2 + ROS_ERROR("Exception in publishRgb: %s", e.what()); #endif - return; } + } else {// new version jpeg data + + std::vector jpeg_data(static_cast(image.pAddr), + static_cast(image.pAddr) + image.length); + + // convert back to bgr8 + cv::Mat decoded_image = cv::imdecode(jpeg_data, cv::IMREAD_COLOR); + + cv_bridge::CvImage cv_image; + cv_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp + 719060); + cv_image.encoding = "bgr8"; + cv_image.image = decoded_image; + + if (g_sendcloudrender) { + std::lock_guard lock(rgb_queue_mutex_); + if (rgb_image_queue_.size() >= 10) { + rgb_image_queue_.pop_front(); + } + rgb_image_queue_.push_back(cv_image.toImageMsg()); + } - //Create ROS image message #ifdef ROS2 - auto header = std::make_shared(); - header->stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation - header->frame_id = "camera_rgb_frame"; - - auto cv_image = std::make_shared(*header, "bgr8", bgr); - auto msg = cv_image->toImageMsg(); - - // Add to unified queue - if (g_sendcloudrender) { - std::lock_guard lock(rgb_queue_mutex_); - if (rgb_image_queue_.size() >= 10) { - rgb_image_queue_.pop_front(); - } - rgb_image_queue_.push_back(msg); - } + { + rgb_pub_->publish(*cv_image.toImageMsg()); - // Publish original image message - rgb_pub_->publish(*msg); - - // Create compressed image message - auto compressed_msg = std::make_shared(); - compressed_msg->header = *header; - compressed_msg->format = "jpeg"; - - // Set compression parameters - std::vector compression_params; - compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); - compression_params.push_back(80); - - // Compress image - cv::imencode(".jpg", bgr, compressed_msg->data, compression_params); - - compressed_rgb_pub_->publish(*compressed_msg); - + // original jpeg + sensor_msgs::msg::CompressedImage jpeg_msg; + jpeg_msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp + 719060); + jpeg_msg.format = "jpeg"; + jpeg_msg.data = jpeg_data; + + compressed_rgb_pub_->publish(jpeg_msg); + } #else - // ROS1 version - std_msgs::Header header; - header.stamp = ns_to_ros_time(image.timestamp + 719060); // Offset compensation - header.frame_id = "camera_rgb_frame"; - - auto cv_image = boost::make_shared(header, "bgr8", bgr); - auto msg = cv_image->toImageMsg(); - - // Add to unified queue - if (g_sendcloudrender) { - std::lock_guard lock(rgb_queue_mutex_); - if (rgb_image_queue_.size() >= 10) { - rgb_image_queue_.pop_front(); - } - rgb_image_queue_.push_back(msg); - } - - // Publish original image message - rgb_pub_.publish(msg); - - // Publish compressed image - always publish - // Create compressed image message - sensor_msgs::CompressedImagePtr compressed_msg(new sensor_msgs::CompressedImage()); - compressed_msg->header = header; - compressed_msg->format = "jpeg"; - - // Set compression parameters - std::vector compression_params; - compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); - compression_params.push_back(80); // JPEG quality 80% - - // Compress image - cv::imencode(".jpg", bgr, compressed_msg->data, compression_params); - - compressed_rgb_pub_.publish(compressed_msg); - - #endif + { + rgb_pub_.publish(cv_image.toImageMsg()); - } catch (const cv::Exception& e) { - #ifndef ROS2 - ROS_ERROR("OpenCV error in publishRgb: %s", e.what()); - #endif - } catch (const std::exception& e) { - #ifndef ROS2 - ROS_ERROR("Exception in publishRgb: %s", e.what()); + // original jpeg + sensor_msgs::CompressedImagePtr jpeg_msg(new sensor_msgs::CompressedImage()); + // compressed_msg->header = header; + // compressed_msg->format = "jpeg"; + jpeg_msg->header.stamp = ns_to_ros_time(stream->imageList[0].timestamp + 719060); + jpeg_msg->format = "jpeg"; + jpeg_msg->data = jpeg_data; + + compressed_rgb_pub_.publish(jpeg_msg); + } #endif } + } @@ -789,7 +862,6 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m } void publishOdometry(capture_Image_List_t* stream) { - ros2_odom_convert_t* odom_data = (ros2_odom_convert_t*)stream->imageList[0].pAddr; #ifdef ROS2 auto msg = nav_msgs::msg::Odometry(); @@ -797,19 +869,56 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m ros::Odometry msg; #endif - msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); msg.header.frame_id = "map"; msg.child_frame_id = "base_link"; - msg.pose.pose.position.x = static_cast(odom_data->pos[0]) / 1e6; - msg.pose.pose.position.y = static_cast(odom_data->pos[1]) / 1e6; - msg.pose.pose.position.z = static_cast(odom_data->pos[2]) / 1e6; + uint32_t data_len = stream->imageList[0].length; + 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; + msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); + + msg.pose.pose.position.x = static_cast(odom_data->pos[0]) / 1e6; + msg.pose.pose.position.y = static_cast(odom_data->pos[1]) / 1e6; + msg.pose.pose.position.z = static_cast(odom_data->pos[2]) / 1e6; + + msg.pose.pose.orientation.x = static_cast(odom_data->orient[0]) / 1e6; + msg.pose.pose.orientation.y = static_cast(odom_data->orient[1]) / 1e6; + msg.pose.pose.orientation.z = static_cast(odom_data->orient[2]) / 1e6; + msg.pose.pose.orientation.w = static_cast(odom_data->orient[3]) / 1e6; + + msg.twist.twist.linear.x = static_cast(odom_data->linear_velocity[0]) / 1e6; + msg.twist.twist.linear.y = static_cast(odom_data->linear_velocity[1]) / 1e6; + msg.twist.twist.linear.z = static_cast(odom_data->linear_velocity[2]) / 1e6; + + msg.twist.twist.angular.x = static_cast(odom_data->angular_velocity[0]) / 1e6; + msg.twist.twist.angular.y = static_cast(odom_data->angular_velocity[1]) / 1e6; + msg.twist.twist.angular.z = static_cast(odom_data->angular_velocity[2]) / 1e6; + + msg.pose.covariance = { + static_cast(odom_data->cov[0]) / 1e9, static_cast(odom_data->cov[1]) / 1e9, static_cast(odom_data->cov[2]) / 1e9, 0.0, 0.0, 0.0, + static_cast(odom_data->cov[3]) / 1e9, static_cast(odom_data->cov[4]) / 1e9, static_cast(odom_data->cov[5]) / 1e9, 0.0, 0.0, 0.0, + static_cast(odom_data->cov[6]) / 1e9, static_cast(odom_data->cov[7]) / 1e9, static_cast(odom_data->cov[8]) / 1e9, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, static_cast(odom_data->cov[9]) / 1e9, static_cast(odom_data->cov[10]) / 1e9, static_cast(odom_data->cov[11]) / 1e9, + 0.0, 0.0, 0.0, static_cast(odom_data->cov[12]) / 1e9, static_cast(odom_data->cov[13]) / 1e9, static_cast(odom_data->cov[14]) / 1e9, + 0.0, 0.0, 0.0, static_cast(odom_data->cov[15]) / 1e9, static_cast(odom_data->cov[16]) / 1e9, static_cast(odom_data->cov[17]) / 1e9, + }; + + } else if (data_len == sizeof(ros2_odom_convert_t)) { + + ros2_odom_convert_t* odom_data = (ros2_odom_convert_t*)stream->imageList[0].pAddr; + msg.header.stamp = ns_to_ros_time(odom_data->timestamp_ns); + + msg.pose.pose.position.x = static_cast(odom_data->pos[0]) / 1e6; + msg.pose.pose.position.y = static_cast(odom_data->pos[1]) / 1e6; + msg.pose.pose.position.z = static_cast(odom_data->pos[2]) / 1e6; + + msg.pose.pose.orientation.x = static_cast(odom_data->orient[0]) / 1e6; + msg.pose.pose.orientation.y = static_cast(odom_data->orient[1]) / 1e6; + msg.pose.pose.orientation.z = static_cast(odom_data->orient[2]) / 1e6; + msg.pose.pose.orientation.w = static_cast(odom_data->orient[3]) / 1e6; + } - msg.pose.pose.orientation.x = static_cast(odom_data->orient[0]) / 1e6; - msg.pose.pose.orientation.y = static_cast(odom_data->orient[1]) / 1e6; - msg.pose.pose.orientation.z = static_cast(odom_data->orient[2]) / 1e6; - msg.pose.pose.orientation.w = static_cast(odom_data->orient[3]) / 1e6; - #ifdef ROS2 odom_publisher_->publish(std::move(msg)); #else diff --git a/include/lidar_api.h b/include/lidar_api.h index b3546ba..8a592a0 100644 --- a/include/lidar_api.h +++ b/include/lidar_api.h @@ -128,9 +128,10 @@ 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 calculation * @return int 0 on success, negative error code on failure */ -int lidar_start_stream(device_handle device, int type); +int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr); /** * @brief Stop data streaming from the device @@ -165,45 +166,17 @@ int lidar_activate_stream_type(device_handle device, int type); */ int lidar_deactivate_stream_type(device_handle device, int type); -// /** -// * @brief Perform over-the-air firmware update -// * -// * Updates the device firmware using the specified file. -// * -// * @param device Handle to the target device -// * @param type Type of OTA update to perform -// * @param filepath Path to the firmware file -// * @param process_cb Callback function to report update progress -// * @return int 0 on success, negative error code on failure -// */ -// int lidar_ota_update(device_handle device, char* filepath, void(*process_cb)(float process)); - -int ONLY_FOR_DEV_DONT_PUB_2adb(device_handle device); - +/** + * @brief Get calibration file from the device + * + * Retrieves the calibration file from the device. + * + * @param device Handle to the target device + * @param path Path to save the calibration file + * @return int 0 on success, negative error code on failure + */ int lidar_get_calib_file(device_handle device, const char* path); -/** - * @brief Get device calibration parameters - * - * Retrieves the current calibration parameters from the device. - * - * @param device Handle to the target device - * @param param Pointer to receive the calibration parameters - * @return int 0 on success, negative error code on failure - */ -int lidar_get_calibration(device_handle device, lidar_calibration_t* param); - -/** - * @brief Set device calibration parameters - * - * Applies new calibration parameters to the device. - * - * @param device Handle to the target device - * @param param Pointer to the calibration parameters to set - * @return int 0 on success, negative error code on failure - */ -int lidar_set_calibration(device_handle device, const lidar_calibration_t *param); - /** * @brief Set log verbosity level * diff --git a/include/lidar_api_type.h b/include/lidar_api_type.h index 3d79a3f..a37006b 100644 --- a/include/lidar_api_type.h +++ b/include/lidar_api_type.h @@ -81,6 +81,15 @@ typedef struct { int64_t orient[4]; } ros2_odom_convert_t; +typedef struct { + uint64_t timestamp_ns; + int64_t pos[3]; + int64_t orient[4]; + int64_t linear_velocity[3]; + int64_t angular_velocity[3]; + int64_t cov[3 * 3 * 2]; +} ros_odom_convert_complete_t; + typedef struct icm_6aixs_data_t { int16_t aacx; int16_t aacy; diff --git a/lib/liblydHostApi_amd.a b/lib/liblydHostApi_amd.a index c983024..d707ee2 100644 Binary files a/lib/liblydHostApi_amd.a and b/lib/liblydHostApi_amd.a differ diff --git a/lib/liblydHostApi_arm.a b/lib/liblydHostApi_arm.a index 6f785bf..aa9afa8 100644 Binary files a/lib/liblydHostApi_arm.a and b/lib/liblydHostApi_arm.a differ diff --git a/src/host_sdk_sample.cpp b/src/host_sdk_sample.cpp index 7875fa7..1aa120f 100644 --- a/src/host_sdk_sample.cpp +++ b/src/host_sdk_sample.cpp @@ -28,7 +28,7 @@ #include #include #endif -#define ros_driver_version "0.3.1" +#define ros_driver_version "0.4.0" // Global variable declarations static device_handle odinDevice = nullptr; static std::atomic deviceConnected(false); @@ -65,6 +65,26 @@ int g_sendcloudslam = 0; int g_sendcloudrender = 0; int g_sendrgb_compressed = 0; +class RosNodeControlImpl : public RosNodeControlInterface { + public: + void setDtofSubframeODR(int interval) override { + dtof_subframe_interval_time = interval; + } + + int getDtofSubframeODR() const override { + return dtof_subframe_interval_time; + } + + private: + int dtof_subframe_interval_time = 0; + }; + +static RosNodeControlImpl g_rosNodeControlImpl; + +RosNodeControlInterface* getRosNodeControl() { + return &g_rosNodeControlImpl; +} + void clear_all_queues(); // detect USB3.0 @@ -406,7 +426,8 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach return; } - if (lidar_start_stream(odinDevice, type)) { + uint32_t dtof_subframe_odr = 0; + if (lidar_start_stream(odinDevice, type, dtof_subframe_odr)) { #ifdef ROS2 RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Start stream failed"); #else @@ -418,6 +439,10 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach return; } + if (dtof_subframe_odr > 0) { + g_rosNodeControlImpl.setDtofSubframeODR(dtof_subframe_odr); + } + if (g_sendrgb) { lidar_activate_stream_type(odinDevice, LIDAR_DT_RAW_RGB); }