<feat> compatible with odin1 firmware version 0.8.0

1. add option to use host ros time as data timestamp, not recommanded unless specifically require this setup 2. only pub odm-map tf in relocal mode 3. add option to also not pub odm-base_link tf, not recommanded unless specifically require this setup 4. dev_status.csv now have device timestamp to better help analyzing 5. add control option to enable encrtypted device debug log, should only enable when instructed by official support
2025-12-02 17:06:17 +08:00
parent b4adaf355d
commit 5efa57da6e
8 changed files with 474 additions and 286 deletions
@@ -2,6 +2,8 @@

 ROS driver suite for Odin sensor modules (Manifold Tech Ltd.) 

+Odin1 wiki: https://manifoldtehltd.github.io/wiki/Odin1/Cover.html
+
 ## Odin_ROS_Driver

 Compatibility:
@@ -16,18 +18,20 @@ This driver package provides core functionality for point cloud SLAM application

 ## 1. Version

-Current Version: v0.6.1
+Current Version: v0.7.0

 ## 2. Preparation

 ### 2.1 OS Requirement

-● Ubuntu 18.04 for ROS Melodic;
-
 ● Ubuntu 20.04 for ROS Noetic and ROS2 Foxy;

 ● Ubuntu 22.04 for ROS2 Humble;

+● Ubuntu 18.04 is currently not supported;
+
+● Ubuntu 24.04 is not officially supported but may work with some modifications.
+
 ### 2.2 Dependencies

 ● Opencv >= 4.5.0(recommand 4.5.5/4.8.0. Make sure only one version of opencv is installed)
@@ -67,8 +71,6 @@ sudo apt-get install libopencv-dev
 ```

 #### 2.3.4 ROS install
-For ROS Melodic installation, please refer to:
-[ROS Melodic installation instructions](https://wiki.ros.org/melodic/Installation)

 For ROS Noetic installation, please refer to:
 [ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation)
@@ -437,6 +439,9 @@ Disable odin1/image	with sendrgb = 0 in control_command.yaml and try again. If t
 Purge the unused version of opencv and maintain a single complete version, then rebuild the driver and try again.

 ## 6.  Contact Information
+
+You can contact our support through support@manifoldtech.cn
+
 To help diagnose the issue, please provide the following details to our FAE engineer:

 1. Current firmware version 
@@ -1,20 +1,67 @@
 register_keys:
-  strict_usb3.0_check: 0  # 0: off: 1: on; if off, allow connection even if usb connection is below usb 3.0 
+  # if off, allow connection even if usb connection is below usb 3.0
+  # 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
+
  streamctrl: 1           # 0: off; 1: on
-  sendrgb: 1              # 0: off; 1: on
-  sendimu: 1              # 0: off; 1: on
-  sendodom: 1             # 0: off; 1: on
-  senddtof: 1             # 0: off; 1: on
-  sendcloudslam: 1        # 0: off; 1: on
-  sendcloudrender: 1      # 0: off; 1: on
+
+  # original rgb data in jpeg format from device
  sendrgbcompressed: 1    # 0: off; 1: on
+
+  # RGB data, decoded from original jpeg data from device, bgr8 format
+  # Processed on host device
+  sendrgb: 1              # 0: off; 1: on
+
+  # undistort rgb image processed from decoded rgb data.
+  # depends on sendrgb. related camera parameters can be found in ws/src/odin_ros_driver/config/calib.yaml
+  # Processed on host device
+  sendrgbundistort: 0     # 0: off; 1: on.
+
+  # IMU data
+  sendimu: 1              # 0: off; 1: on
+
+  # Odometry data
+  sendodom: 1             # 0: off; 1: on
+
+  # TF from odom to base_link. Leave it on unless you specifically need it off.
+  # ATTENTION: critical for rviz to show cloud_raw.
+  send_odom_baselink_tf: 1 # 0: off; 1: on.
+
+  # raw dtof data
+  senddtof: 1             # 0: off; 1: on
+
+  # slam cloud data
+  sendcloudslam: 1        # 0: off; 1: on
+
+  # processed with raw point cloud, rgb image, and calib.yaml from device
+  # Processed on host device
+  sendcloudrender: 1      # 0: off; 1: on
+
+  # depth completion demo, high computing resource usage. 
+  # for more information please refer to the readme file.
+  # Processed on host device
  senddepth: 0            # 0: off; 1: on
-  sendrgbundistort: 0     # 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
-  devstatuslog: 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.
+
+  save_log: 0 # 0: off; 1: on; 
+
+  # raw dtof sensor intensity data in gray format, mostly for debug purpose.
  pubintensitygray: 0     # 0: off; 1: on
+
  showpath: 0             # 0: off; 1: on
  showcamerapose: 0       # 0: off; 1: on
+  
  custom_save_map: 0
  custom_map_mode: 0      # 0: Odometry mode 1: SLAM mode 2: Relocalization mode
  custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
@@ -182,6 +182,10 @@ class RosNodeControlInterface {
        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;
    };
    
 RosNodeControlInterface* getRosNodeControl();
@@ -233,7 +237,15 @@ public:
            ros::Imu imu_msg;
        #endif
        
-        imu_msg.header.stamp = ns_to_ros_time(stream->stamp);
+        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);
+        }
        imu_msg.header.frame_id = "imu_link";

        imu_msg.linear_acceleration.y = -1 * stream->accel_x;
@@ -499,7 +511,7 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
    } 
 }

-   void publishIntensityCloud(capture_Image_List_t* stream, int idx) 
+void publishIntensityCloud(capture_Image_List_t* stream, int idx)
 {
    // Check index validity
    if (idx < 0 || idx >= 10) {
@@ -534,7 +546,15 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m

    // Set message header
    msg->header.frame_id = "odin1_base_link";
-    msg->header.stamp = ns_to_ros_time(cloud.timestamp);
+    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);
+    }

    msg->height = cloud.height;
    msg->width = cloud.width;
@@ -653,173 +673,51 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
    #endif
 }

-    void publishGrayUInt8(capture_Image_List_t *stream, int idx) {
-        ImageMsg msg;
-        msg.header.stamp = ns_to_ros_time(stream->imageList[idx].timestamp);
-        msg.header.frame_id = "map";
-
-        int width = stream->imageList[idx].width;
-        int height = stream->imageList[idx].height;
-
-        msg.height = height;
-        msg.width = width;
-        msg.encoding = "mono8";
-        msg.is_bigendian = false;
-        msg.step = width * sizeof(uint8_t);
-
-        size_t image_size = msg.step * height;
-
-        msg.data.resize(image_size);
-
-        memcpy(msg.data.data(), stream->imageList[idx].pAddr, image_size);
-
+void publishGrayUInt8(capture_Image_List_t *stream, int idx) {
+    ImageMsg msg;
+    if (getRosNodeControl()->useHostRosTime()) {
        #ifdef ROS2
-            intensity_gray_pub_->publish(msg);
+            msg.header.stamp = node_->now();
        #else
-            intensity_gray_pub_.publish(msg);
+            msg.header.stamp = ros::Time::now();
        #endif
+    } else {
+        msg.header.stamp = ns_to_ros_time(stream->imageList[idx].timestamp);
    }
+    msg.header.frame_id = "map";

-    void publishRgb(capture_Image_List_t *stream) {
+    int width = stream->imageList[idx].width;
+    int height = stream->imageList[idx].height;
+
+    msg.height = height;
+    msg.width = width;
+    msg.encoding = "mono8";
+    msg.is_bigendian = false;
+    msg.step = width * sizeof(uint8_t);
+
+    size_t image_size = msg.step * height;
+
+    msg.data.resize(image_size);
+
+    memcpy(msg.data.data(), stream->imageList[idx].pAddr, image_size);
+
+    #ifdef ROS2
+        intensity_gray_pub_->publish(msg);
+    #else
+        intensity_gray_pub_.publish(msg);
+    #endif
+}
+
+void publishRgb(capture_Image_List_t *stream) {
    buffer_List_t &image = stream->imageList[0];

    // 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<std_msgs::msg::Header>();
-                header->stamp = ns_to_ros_time(image.timestamp); // Offset compensation
-
-                //RCLCPP_INFO(rclcpp::get_logger("device_cb"), "image rgb %ld",image.timestamp);
-                header->frame_id = "camera_rgb_frame";
-                
-                auto cv_image = std::make_shared<cv_bridge::CvImage>(*header, "bgr8", bgr);
-                auto msg = cv_image->toImageMsg();
-            
-                // Add to unified queue
-                if (g_sendcloudrender) {
-                    std::lock_guard<std::mutex> 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<sensor_msgs::msg::CompressedImage>();
-                compressed_msg->header = *header;
-                compressed_msg->format = "jpeg";
-                
-                // Set compression parameters
-                std::vector<int> compression_params;
-                compression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
-                compression_params.push_back(80);
-                
-                // Compress image
-                cv::imencode(".JPEG", bgr, compressed_msg->data, compression_params);
-
-                // Enqueue binary logging for image
-                if (data_logger_) {
-                    const uint32_t idx_now = image_index_.fetch_add(1, std::memory_order_relaxed);
-                    const double ts_sec = static_cast<double>(image.timestamp) / 1e9;
-                    const uint32_t jpeg_size = static_cast<uint32_t>(compressed_msg->data.size());
-                    std::vector<uint8_t> blob;
-                    blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t) + jpeg_size);
-                    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(idx_now);
-                    append_pod(ts_sec);
-                    append_pod(jpeg_size);
-                    blob.insert(blob.end(), compressed_msg->data.begin(), compressed_msg->data.end());
-                    data_logger_->enqueueImageFrame(std::move(blob));
-                }
-                
-                compressed_rgb_pub_->publish(*compressed_msg);
-                
-            #else
-                // ROS1 version
-                std_msgs::Header header;
-                header.stamp = ns_to_ros_time(image.timestamp); // Offset compensation
-                header.frame_id = "camera_rgb_frame";
-                
-                auto cv_image = boost::make_shared<cv_bridge::CvImage>(header, "bgr8", bgr);
-                auto msg = cv_image->toImageMsg();
-                
-                // Add to unified queue
-                if (g_sendcloudrender) {
-                    std::lock_guard<std::mutex> 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<int> 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);
-
-                // Enqueue binary logging for image
-                if (data_logger_) {
-                    const uint32_t idx_now = image_index_.fetch_add(1, std::memory_order_relaxed);
-                    // Convert ROS1 header.stamp to seconds
-                    const double ts_sec = static_cast<double>(header.stamp.sec) + static_cast<double>(header.stamp.nsec) / 1e9;
-                    const uint32_t jpeg_size = static_cast<uint32_t>(compressed_msg->data.size());
-                    std::vector<uint8_t> blob;
-                    blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t) + jpeg_size);
-                    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(idx_now);
-                    append_pod(ts_sec);
-                    append_pod(jpeg_size);
-                    blob.insert(blob.end(), compressed_msg->data.begin(), compressed_msg->data.end());
-                    data_logger_->enqueueImageFrame(std::move(blob));
-                }
-                compressed_rgb_pub_.publish(compressed_msg);
-                
-            #endif
-    
-        } 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());
-            #endif
-        }
+        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("publishRgb"), "old format rgb data, please upgrade device firmware");
+        #else
+            ROS_INFO("old format rgb data, please upgrade device firmware");
+        #endif
    } else {// new version jpeg data

        std::vector<uint8_t> jpeg_data(static_cast<uint8_t*>(image.pAddr),
@@ -829,7 +727,15 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
        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);
+        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);
+        }
        cv_image.encoding = "bgr8";
        cv_image.image = decoded_image;

@@ -865,10 +771,19 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m

        if (m_undistort_map_init_success) {
            cv::remap(decoded_image, undistorted_image, m_undistort_map_x, m_undistort_map_y, cv::INTER_LINEAR);
-            cv_undistorted_image.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
+            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);
+            }
            cv_undistorted_image.encoding = "bgr8";
            cv_undistorted_image.image = undistorted_image;
        }
+
        #ifdef ROS2
        {
            rgb_pub_->publish(*cv_image.toImageMsg());
@@ -878,7 +793,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m

            // original jpeg
            sensor_msgs::msg::CompressedImage jpeg_msg;
-            jpeg_msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
+            if (getRosNodeControl()->useHostRosTime()) {
+                jpeg_msg.header.stamp = node_->now();
+            } else {
+                jpeg_msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
+            }
            jpeg_msg.format = "jpeg";
            jpeg_msg.data = jpeg_data;

@@ -893,9 +812,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m

            // 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);
+            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->format = "jpeg";
            jpeg_msg->data = jpeg_data;

@@ -912,7 +833,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
        #ifdef ROS2
                sensor_msgs::msg::PointCloud2 msg;
                msg.header.frame_id = "odom";
-                msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
+                if (getRosNodeControl()->useHostRosTime()) {
+                    msg.header.stamp = node_->now();
+                } else {
+                    msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
+                }

                //RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Point cloudrgba %ld",stream->imageList[0].timestamp);

@@ -940,7 +865,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
        #else
            sensor_msgs::PointCloud2 msg;
            msg.header.frame_id = "odom";
-            msg.header.stamp = ns_to_ros_time(stream->imageList[0].timestamp);
+            if (getRosNodeControl()->useHostRosTime()) {
+                msg.header.stamp = ros::Time::now();
+            } else {
+                msg.header.stamp = ns_to_ros_time(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;
@@ -1054,7 +983,15 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
            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);
+                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);
+                }

                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;
@@ -1109,7 +1046,15 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
            } 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);
+                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);
+                }

                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;
@@ -1125,18 +1070,24 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
            switch(odom_type) {
                case OdometryType::STANDARD:
                    {
-                    geometry_msgs::msg::TransformStamped transformStamped;
-                    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;
-                    transformStamped.transform.translation.y = msg.pose.pose.position.y;
-                    transformStamped.transform.translation.z = msg.pose.pose.position.z;
-                    transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
-                    transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
-                    transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
-                    transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
-                    tf_broadcaster->sendTransform(transformStamped);
+                    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.frame_id = "odom";
+                        transformStamped.child_frame_id = "odin1_base_link";
+                        transformStamped.transform.translation.x = msg.pose.pose.position.x;
+                        transformStamped.transform.translation.y = msg.pose.pose.position.y;
+                        transformStamped.transform.translation.z = msg.pose.pose.position.z;
+                        transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
+                        transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
+                        transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
+                        transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
+                        tf_broadcaster->sendTransform(transformStamped);
+                    }
                    odom_publisher_->publish(msg);

                    // Publish odom trajectory as visualization markers (green lines connecting adjacent points)
@@ -1201,7 +1152,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
                case OdometryType::TRANSFORM:
                    {
                    geometry_msgs::msg::TransformStamped transformStamped;
-                    transformStamped.header.stamp = msg.header.stamp;
+                    if (getRosNodeControl()->useHostRosTime()) {
+                        transformStamped.header.stamp = node_->now();
+                    } else {
+                        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;
@@ -1219,18 +1174,24 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
            switch(odom_type) {
                case OdometryType::STANDARD:
                    {
-                    geometry_msgs::TransformStamped transformStamped;
-                    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;
-                    transformStamped.transform.translation.y = msg.pose.pose.position.y;
-                    transformStamped.transform.translation.z = msg.pose.pose.position.z;
-                    transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
-                    transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
-                    transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
-                    transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
-                    tf_broadcaster->sendTransform(transformStamped);
+                    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.frame_id = "odom";
+                        transformStamped.child_frame_id = "odin1_base_link";
+                        transformStamped.transform.translation.x = msg.pose.pose.position.x;
+                        transformStamped.transform.translation.y = msg.pose.pose.position.y;
+                        transformStamped.transform.translation.z = msg.pose.pose.position.z;
+                        transformStamped.transform.rotation.x = msg.pose.pose.orientation.x;
+                        transformStamped.transform.rotation.y = msg.pose.pose.orientation.y;
+                        transformStamped.transform.rotation.z = msg.pose.pose.orientation.z;
+                        transformStamped.transform.rotation.w = msg.pose.pose.orientation.w;
+                        tf_broadcaster->sendTransform(transformStamped);
+                    }
                    odom_publisher_.publish(msg);

                    if (show_path) {
@@ -1296,7 +1257,11 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
                case OdometryType::TRANSFORM:
                    {
                    geometry_msgs::TransformStamped transformStamped;
-                    transformStamped.header.stamp = msg.header.stamp;
+                    if (getRosNodeControl()->useHostRosTime()) {
+                        transformStamped.header.stamp = ros::Time::now();
+                    } else {
+                        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;
@@ -1533,18 +1498,18 @@ private:
    }
    #ifdef ROS1
        void initialize_publishers(ros::NodeHandle& nh) {
-            imu_pub_ = nh.advertise<ros::Imu>("odin1/imu", 10);
-            rgb_pub_ = nh.advertise<ros::Image>("odin1/image", 10);
-            cloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_raw", 10);
-            xyzrgbacloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_slam", 10);
-            odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry", 10);
-            odom_highfreq_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry_highfreq", 10);
-            path_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/path", 10);
-            pub_camera_pose_visual_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/camera_pose_visual", 10);
-            rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 10);
-            compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 10);
-            undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 10);
-            intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 10);
+            imu_pub_ = nh.advertise<ros::Imu>("odin1/imu", 4000);
+            rgb_pub_ = nh.advertise<ros::Image>("odin1/image", 100);
+            cloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_raw", 100);
+            xyzrgbacloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_slam", 100);
+            odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry", 100);
+            odom_highfreq_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry_highfreq", 4000);
+            path_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/path", 100);
+            pub_camera_pose_visual_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/camera_pose_visual", 100);
+            rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 100);
+            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);
            tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>();
        }
    #endif
@@ -245,6 +245,16 @@ 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 enable device log
+ *
+ *
+ * @param device Handle to the target device
+ * @param dest_dir Destination directory to save the logs
+ * @return int 0 on success, -1 on failure
+ */
+ int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir);
+
 #ifdef __cplusplus
 }
 #endif
@@ -56,12 +56,14 @@ typedef enum {
    LIDAR_DT_DEV_STATUS,
    LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
    LIDAR_DT_SLAM_ODOMETRY_TF,
+    LIDAR_DT_SLAM_WIWC
 } lidar_data_type_e;

 typedef struct {
    int8_t serial[LIDAR_SERIAL_MAX];
    int8_t model[LIDAR_MODEL_MAX];
    bool online;
+    uint32_t initial_state;
 } lidar_device_info_t;

 typedef struct {
@@ -187,6 +189,7 @@ typedef struct{
 } lidar_soc_thermal_t;
 typedef struct
 {
+    double uptime_seconds;
    lidar_soc_thermal_t soc_thermal; 

    int cpu_use_rate[8];              /* cpu usage rate */
@@ -202,6 +205,14 @@ typedef struct

 } lidar_device_status_t;

+typedef enum {
+    LIDAR_DEVICE_NONE = 0,
+    LIDAR_DEVICE_NOT_INITIALIZED,
+    LIDAR_DEVICE_INITIALIZED,
+    LIDAR_DEVICE_STREAMING,
+    LIDAR_DEVICE_STREAM_STOPPED,
+} lidar_device_initial_state_e;
+
 #ifdef __cplusplus
 }
 #endif
@@ -35,6 +35,7 @@ limitations under the License.
 #include <vector>
 #include <cstdio>
 #include <array>
+#include <system_error>
 // #include <yaml-cpp/yaml.h>
 #include <iomanip>
 #include <sstream>
@@ -45,7 +46,9 @@ limitations under the License.
    #include <ros/package.h>
    #include <ros/ros.h> 
 #endif
-#define ros_driver_version "0.6.1"
+#define ros_driver_version "0.7.0"
+#define recommended_firmware_version "0.8.0"
+
 // Global variable declarations
 static device_handle odinDevice = nullptr;
 static std::atomic<bool> deviceConnected(false);
@@ -88,6 +91,7 @@ int g_sendrgb = 1;
 int g_sendimu = 1;
 int g_senddtof = 1;
 int g_sendodom = 1;
+int g_send_odom_baselink_tf = 0;
 int g_sendcloudslam = 0;
 int g_sendcloudrender = 0;
 int g_sendrgb_compressed = 0;
@@ -98,6 +102,8 @@ int g_pub_intensity_gray = 0;
 int g_show_path = 0;
 int g_show_camerapose = 0;
 int g_strict_usb3_0_check = 0;
+int g_use_host_ros_time = 0;
+int g_save_log = 0;

 std::filesystem::path log_root_dir_;
 int g_custom_map_mode = 0;
@@ -170,8 +176,26 @@ class RosNodeControlImpl : public RosNodeControlInterface {
            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;
+        }
+
+        bool sendOdomBaseLinkTF() const override {
+            return pub_odom_baselink_tf;
+        }
+
    private:
        int dtof_subframe_interval_time = 0;
+        bool pub_use_host_ros_time = false;
+        bool pub_odom_baselink_tf = false;
    };
    
 static RosNodeControlImpl g_rosNodeControlImpl;
@@ -790,9 +814,8 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
                if (dev_status_csv_file) {
                    // append the data row
                    int rc = 0;
-                    rc = std::fprintf(dev_status_csv_file, "%d,%d,%d,%d,%d,%d,", // %.0f
-                                            // get_uptime_seconds(),
-                                            0,
+                    rc = std::fprintf(dev_status_csv_file, "%.2f,%d,%d,%d,%d,%d,", 
+                                            dev_info_data->uptime_seconds,
                                            dev_info_data->soc_thermal.package_temp,
                                            dev_info_data->soc_thermal.cpu_temp,
                                            dev_info_data->soc_thermal.center_temp,
@@ -937,7 +960,14 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
            break;
            case LIDAR_DT_SLAM_ODOMETRY_TF:
            {
-                g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, OdometryType::TRANSFORM, false, false);
+                if (g_custom_map_mode == 2) {
+                    g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream, OdometryType::TRANSFORM, false, false);
+                }
+            }
+            break;
+            case LIDAR_DT_SLAM_WIWC:
+            {
+                //...
            }
            break;
        default:
@@ -988,18 +1018,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            #endif
            return;
        }
-        
-        if (lidar_open_device(odinDevice)) {
-            #ifdef ROS2
-                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Open device failed");
-            #else
-                ROS_ERROR("Open device failed");
-            #endif
-            lidar_destory_device(odinDevice);
-            odinDevice = nullptr;
-            return;
-        }
-        
 	const std::string package_name = "odin_ros_driver";
 	std::string config_dir = "";
 	#ifdef ROS2
@@ -1025,6 +1043,40 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            ROS_INFO("Calibration files will be saved to: %s", config_dir.c_str());
        #endif

+        std::filesystem::path per_con_log_root_dir;
+        {
+            auto connection_time = std::chrono::system_clock::now();
+            std::time_t t = std::chrono::system_clock::to_time_t(connection_time);
+            std::tm tm{};
+            #ifdef _WIN32
+                localtime_s(&tm, &t);
+            #else
+                localtime_r(&t, &tm);
+            #endif
+            char buf[32];
+            std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm);
+            std::string folder_name = std::string("Conn_") + std::string(buf);
+            std::filesystem::path base_log_dir = log_root_dir_.empty()
+                ? std::filesystem::path(config_dir)
+                : log_root_dir_;
+            per_con_log_root_dir = base_log_dir / folder_name;
+
+            std::error_code per_con_dir_err;
+            std::filesystem::create_directories(per_con_log_root_dir, per_con_dir_err);
+            if (per_con_dir_err) {
+                #ifdef ROS2
+                    RCLCPP_WARN(rclcpp::get_logger("device_cb"),
+                                "Failed to create per-connection log directory %s: %s",
+                                per_con_log_root_dir.c_str(),
+                                per_con_dir_err.message().c_str());
+                #else
+                    ROS_WARN("Failed to create per-connection log directory %s: %s",
+                             per_con_log_root_dir.c_str(),
+                             per_con_dir_err.message().c_str());
+                #endif
+            }
+        }
+
        auto now = std::chrono::steady_clock::now();
        auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - software_connect_start);
        if (elapsed.count() >= 60) {
@@ -1055,30 +1107,122 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            exit(1);
        }
        else {
-            printf("ros_driver_version:%s\n", ros_driver_version);
+            printf("ros_driver_version:%s, recommended_firmware_version:%s\n", ros_driver_version, recommended_firmware_version);
            printf("get version success.\n");
        }

-        if (lidar_get_calib_file(odinDevice, config_dir.c_str())) {
+        if (g_save_log) {
+            if (lidar_enable_encrypted_device_log(const_cast<lidar_device_info_t*>(device), per_con_log_root_dir.c_str())) {
+                #ifdef ROS2
+                    RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Enable log failed");
+                #else
+                    ROS_ERROR("Enable log failed");
+                #endif
+                lidar_close_device(odinDevice);
+                lidar_destory_device(odinDevice);
+                odinDevice = nullptr;
+                return;
+            }
+
            #ifdef ROS2
-                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get calibration file");
+                RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Encrypted device log enabled at: %s", per_con_log_root_dir.c_str());
            #else
-                ROS_ERROR("Failed to get calibration file");
+                ROS_INFO("Encrypted device log enabled at: %s", per_con_log_root_dir.c_str());
+            #endif
+        } else {
+            #ifdef ROS2
+                RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Encrypted device log disabled via configuration");
+            #else
+                ROS_INFO("Encrypted device log disabled via configuration");
            #endif
-            lidar_close_device(odinDevice);
-            lidar_destory_device(odinDevice);
-            odinDevice = nullptr;
-            return;
        }

-        #ifdef ROS2
-            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Successfully retrieved calibration files");
-        #else
-            ROS_INFO("Successfully retrieved calibration files");
-        #endif
+        bool need_open_device = true;
+        bool need_configure_device = true;
+        switch (device->initial_state) {
+            case LIDAR_DEVICE_NOT_INITIALIZED:
+                #ifdef ROS2
+                    RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device state: not initialized, performing full setup");
+                #else
+                    ROS_INFO("Device state: not initialized, performing full setup");
+                #endif
+                break;
+            case LIDAR_DEVICE_INITIALIZED:
+                need_open_device = false;
+                #ifdef ROS2
+                    RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device state: initialized, skip opening device");
+                #else
+                    ROS_INFO("Device state: initialized, skip opening device");
+                #endif
+                break;
+            case LIDAR_DEVICE_STREAMING:
+                need_open_device = false;
+                need_configure_device = false;
+                #ifdef ROS2
+                    RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device state: streaming, skip opening and configuring");
+                #else
+                    ROS_INFO("Device state: streaming, skip opening and configuring");
+                #endif
+                break;
+            case LIDAR_DEVICE_STREAM_STOPPED:
+                need_open_device = false;
+                need_configure_device = false;
+                #ifdef ROS2
+                    RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device state: stream stopped, resume streaming");
+                #else
+                    ROS_INFO("Device state: stream stopped, resume streaming");
+                #endif
+                break;
+            default:
+                #ifdef ROS2
+                    RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Unknown device initial state: %d", device->initial_state);
+                #else
+                    ROS_WARN("Unknown device initial state: %d", device->initial_state);
+                #endif
+                break;
+        }
+
+        if (need_open_device) {
+            if (lidar_open_device(odinDevice)) {
+                #ifdef ROS2
+                    RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Open device failed");
+                #else
+                    ROS_ERROR("Open device failed");
+                #endif
+                lidar_destory_device(odinDevice);
+                odinDevice = nullptr;
+                return;
+            }
+        }
        
        std::string calib_config = config_dir + "/calib.yaml";
        calib_file_ = calib_config;
+        if (need_configure_device) {
+            if (lidar_get_calib_file(odinDevice, config_dir.c_str())) {
+                #ifdef ROS2
+                    RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get calibration file");
+                #else
+                    ROS_ERROR("Failed to get calibration file");
+                #endif
+                lidar_close_device(odinDevice);
+                lidar_destory_device(odinDevice);
+                odinDevice = nullptr;
+                return;
+            }
+            
+            #ifdef ROS2
+                RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Successfully retrieved calibration files");
+            #else
+                ROS_INFO("Successfully retrieved calibration files");
+            #endif
+        } else {
+            #ifdef ROS2
+                RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Skipping calibration retrieval for current device state");
+            #else
+                ROS_INFO("Skipping calibration retrieval for current device state");
+            #endif
+        }
+        
        if (std::filesystem::exists(calib_config)) {
            g_renderer = std::make_shared<rawCloudRender>();
            if (g_renderer->init(calib_config)) {
@@ -1102,24 +1246,32 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            #endif
        }
        
-        if (lidar_set_mode(odinDevice, type)) {
-            #ifdef ROS2
-                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Set mode failed");
-            #else
-                ROS_ERROR("Set mode failed");
-            #endif
-            lidar_close_device(odinDevice);
-            lidar_destory_device(odinDevice);
-            odinDevice = nullptr;
-            return;
-        }
+        if (need_configure_device) {
+            if (lidar_set_mode(odinDevice, type)) {
+                #ifdef ROS2
+                    RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Set mode failed");
+                #else
+                    ROS_ERROR("Set mode failed");
+                #endif
+                lidar_close_device(odinDevice);
+                lidar_destory_device(odinDevice);
+                odinDevice = nullptr;
+                return;
+            }

-        // Apply custom parameters after setting mode
-        if (g_parser && !g_parser->applyCustomParameters(odinDevice)) {
+            // Apply custom parameters after setting mode
+            if (g_parser && !g_parser->applyCustomParameters(odinDevice)) {
+                #ifdef ROS2
+                    RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Some custom parameters failed to apply");
+                #else
+                    ROS_WARN("Some custom parameters failed to apply");
+                #endif
+            }
+        } else {
            #ifdef ROS2
-                RCLCPP_WARN(rclcpp::get_logger("device_cb"), "Some custom parameters failed to apply");
+                RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Skipping device mode configuration for current state");
            #else
-                ROS_WARN("Some custom parameters failed to apply");
+                ROS_INFO("Skipping device mode configuration for current state");
            #endif
        }

@@ -1166,20 +1318,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            return;
        }
        
-        auto con_time = std::chrono::system_clock::now();
-        std::time_t t = std::chrono::system_clock::to_time_t(con_time);
-        std::tm tm{};
-        #ifdef _WIN32
-            localtime_s(&tm, &t);
-        #else
-            localtime_r(&t, &tm);
-        #endif
-        char buf[32];
-        std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm);
-        std::string folder_name = std::string("Conn_") + std::string(buf);
-        std::filesystem::path per_con_log_root_dir_ = log_root_dir_ / folder_name;
-        std::filesystem::create_directories(per_con_log_root_dir_);
-        std::string dev_status_csv_file_path_ = per_con_log_root_dir_ / "dev_status.csv";
+        std::string dev_status_csv_file_path_ = per_con_log_root_dir / "dev_status.csv";

        if (dev_status_csv_file) {
            std::fflush(dev_status_csv_file);
@@ -1359,6 +1498,7 @@ int main(int argc, char *argv[])
        g_sendimu       = get_key_value("sendimu", 1);
        g_senddtof      = get_key_value("senddtof", 1);
        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);
        g_sendcloudrender = get_key_value("sendcloudrender", 1);
        g_sendrgb_compressed = get_key_value("sendrgbcompressed", 1);
@@ -1371,6 +1511,16 @@ int main(int argc, char *argv[])
        g_show_camerapose = get_key_value("showcamerapose", 0);
        g_log_level = get_key_value("log_devel", LOG_LEVEL_INFO);
        g_strict_usb3_0_check = get_key_value("strict_usb3.0_check", 1);
+        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);
+        }

        auto get_key_str_value = [&](const std::string& key, const std::string& default_value) -> std::string {
            auto it = keys_w_str_val.find(key);