<add> 1.Added point cloud confidence and optimized point cloud processing

2.Modified USB handling logic and added a set of new error messages
2025-08-08 21:45:56 +08:00
parent eb750f9fe0
commit 24accb60ec
9 changed files with 300 additions and 104 deletions
@@ -16,7 +16,7 @@ This driver package provides core functionality for point cloud SLAM application

 ## 1. Version

-Current Version: v0.2
+Current Version: v0.3.0

 ## 2. Preparation

@@ -207,9 +207,9 @@ Internal parameters of the Odin ROS driver are defined in config/control_command
 No device connected after 60 seconds 

 **Solution**  
-1.Please power on Odin module again # Disconnect and reconnect odin power
+1. Please power on Odin module again # Disconnect and reconnect odin power

-2.Reinitialize Odin SDK # Execute SDK after device reboot
+2. Reinitialize Odin SDK # Execute SDK after device reboot


 ### 5.2 Library binding failure during compilation
@@ -219,7 +219,7 @@ ld: cannot find -llydHostApi or symbol lookup errors

 **Resolution** 

-1.Clean previous build artifacts
+1. Clean previous build artifacts

 ROS1 
 ```shell
@@ -229,7 +229,7 @@ ROS2
 ```shell
 rm -rf devel/ install/ log/ 
 ``` 
-2.Re-run script installation
+2. Re-run script installation

 ### 5.3 Docker GUI passthrough failure

@@ -266,4 +266,21 @@ ERROR：Missing camera node 'cam_0'

 **Resolution** 

-Please plug and unplug the USB again
+Please plug and unplug the USB again
+
+## 6.  Contact Information
+To help diagnose the issue, please provide the following details to our FAE engineer:
+
+1. Current firmware version 
+```shell
+[device_version_capture]: ros_driver_version: [Version Number]
+``` 
+2. Photos of power adapter and converter cable in use.
+
+3. Does the issue happen occasionally or consistently?
+
+4. Provide images of the problem scenario.
+
+5. Did the troubleshooting methods in Section V resolve the issue?
+
+6. Expected timeline for issue resolution.
@@ -93,7 +93,7 @@ Visualization Manager:
        Shaft Length: 1
        Shaft Radius: 0.05000000074505806
        Value: Axes
-      Topic: /odin1/odometry_map
+      Topic: /odin1/odometry
      Unreliable: false
      Value: true
    - Alpha: 1
@@ -159,10 +159,10 @@ Visualization Manager:
        Min Value: -10
        Value: true
      Axis: Z
-      Channel Name: intensity
+      Channel Name: rgb
      Class: rviz/PointCloud2
      Color: 255; 255; 255
-      Color Transformer: Intensity
+      Color Transformer: RGB8
      Decay Time: 0
      Enabled: false
      Invert Rainbow: false
@@ -175,7 +175,7 @@ Visualization Manager:
      Size (Pixels): 3
      Size (m): 0.009999999776482582
      Style: Flat Squares
-      Topic: /odin1/cloud_raw
+      Topic: /odin1/cloud_slam
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
@@ -208,7 +208,7 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz/Orbit
-      Distance: 5.067190170288086
+      Distance: 4.46931266784668
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
@@ -224,9 +224,9 @@ Visualization Manager:
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: -0.01460082083940506
+      Pitch: 0.3703991174697876
      Target Frame: <Fixed Frame>
-      Yaw: 2.2254059314727783
+      Yaw: 3.3054051399230957
    Saved: ~
 Window Geometry:
  Displays:
@@ -93,7 +93,7 @@ Visualization Manager:
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
-        Value: /odin1/odometry_map
+        Value: /odin1/odometry
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
@@ -498,33 +498,71 @@ void process_pair(const ImageConstPtr &rgb_msg, const PointCloud2ConstPtr &pcd_m
    // Set point cloud fields
    sensor_msgs::PointCloud2Modifier modifier(*msg);
    modifier.setPointCloud2Fields(
-        4,
+        5,
        "x", 1, sensor_msgs::PointField::FLOAT32,
        "y", 1, sensor_msgs::PointField::FLOAT32,
        "z", 1, sensor_msgs::PointField::FLOAT32,
-        "intensity", 1, sensor_msgs::PointField::UINT8
+        "intensity", 1, sensor_msgs::PointField::UINT8,
+        "confidence", 1, sensor_msgs::PointField::UINT16
    );
    modifier.resize(msg->height * msg->width);

-    // Fill point cloud data
+
    sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");
    sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*msg, "intensity");
+    sensor_msgs::PointCloud2Iterator<uint16_t> iter_confidence(*msg, "confidence");

-    float* xyz_data = static_cast<float*>(cloud.pAddr);
-    uint16_t* intensity_data = static_cast<uint16_t*>(stream->imageList[2].pAddr);
-
+    float* xyz_data_f = static_cast<float*>(cloud.pAddr);
    int total_points = cloud.height * cloud.width;
+	//std::cout << stream->imageCount << std::endl;
+	    
+    int valid_points = 0;
+    if (stream->imageCount == 4) {
+
+    uint8_t* intensity_data = static_cast<uint8_t*>(stream->imageList[2].pAddr);
+    uint16_t* confidence_data = static_cast<uint16_t*>(stream->imageList[3].pAddr);
+    
    for (int i = 0; i < total_points; ++i) {
-        float* pf = xyz_data + i * 4;
-
-        *iter_x = pf[2] / 1000.0f; ++iter_x;
-        *iter_y = -pf[0] / 1000.0f; ++iter_y;
-        *iter_z = pf[1] / 1000.0f; ++iter_z;
-        *iter_intensity = static_cast<uint8_t>(intensity_data[i] >> 8); ++iter_intensity;
+           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++;
+    	}
+    } else {
+    uint16_t* intensity_data = static_cast<uint16_t*>(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;
+        
+        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<uint8_t>(intensity);
+        }
+        ++iter_intensity;
+        
+        *iter_confidence = 0;
+        ++iter_confidence;
+        
+        valid_points++;
    }
-
+}
 {
    std::lock_guard<std::mutex> lock(pcd_queue_mutex_);
    
@@ -847,7 +885,7 @@ private:
            rgb_pub_ = node_->create_publisher<ros::Image>("odin1/image", 10);
            cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", 10);
            xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", 10);
-            odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_map", 10);
+            odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", 10);
            rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", 10);
 	    compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", 10);
        #endif
@@ -858,7 +896,7 @@ private:
            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_map", 10);
+            odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry", 10);
            rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 10);
            compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 10);
        }
@@ -945,4 +983,4 @@ private:
 #define SENDODOM   "sendodom"          /* send odometry data */
 #define SENDDTOF   "senddtof"          /* send raw cloud data */
 #define SENDCLOUDSLAM  "sendcloudslam"      /* send rgb cloud data */
-#define EXIT       "q"                 /* exit sample */
+#define EXIT       "q"                 /* exit sample */
@@ -110,17 +110,6 @@ int lidar_open_device(device_handle device);
 * @param device Handle to the device to close
 * @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_close_device(device_handle device);

 /**
@@ -176,18 +165,22 @@ 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, lidar_ota_type_e type, const char* filepath, void(*process_cb)(float process));
+// /**
+//  * @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);
+
+int lidar_get_calib_file(device_handle device, const char* path);

 /**
 * @brief Get device calibration parameters
@@ -220,8 +213,19 @@ int lidar_set_calibration(device_handle device, const lidar_calibration_t *param
 */
 void lidar_log_set_level(lidar_log_level_e level);

+/**
+ * @brief Get the version information of the LiDAR device
+ * 
+ * Retrieves version information including firmware, system, and application versions.
+ * 
+ * @param device Handle to the target device
+ * @param version struct Pointer to receive the version information
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_get_version(device_handle device);
+
 #ifdef __cplusplus
 }
 #endif

-#endif // LIDAR_API_H
+#endif // LIDAR_API_H
@@ -123,6 +123,13 @@ typedef struct {
    void *user_data;
 } lidar_data_callback_info_t;

+typedef struct {
+    char mcu_version[64];
+    char sys_version[64];
+    char slam_version[64];
+    char dev_app_version[64];
+    char host_app_version[64];
+} lidar_version_t;

 #ifdef __cplusplus
 }
@@ -17,7 +17,10 @@
 #include <sys/wait.h>
 #include <signal.h>
 #include <chrono>
-
+#include <fstream>
+#include <vector>
+#include <cstdio>
+#include <array>
 #ifdef ROS2
    #include <ament_index_cpp/get_package_share_directory.hpp>
    #include <rclcpp/rclcpp.hpp>
@@ -25,13 +28,14 @@
    #include <ros/package.h>
    #include <ros/ros.h> 
 #endif
-
+#define ros_driver_version "0.3.0"
 // Global variable declarations
 static device_handle odinDevice = nullptr;
 static std::atomic<bool> deviceConnected(false);
 static std::atomic<bool> deviceDisconnected(false);  // Device disconnection flag
 static std::mutex device_mutex;                      // Device operation mutex lock
-
+static std::atomic<bool> g_connection_timeout(false);
+static std::atomic<bool> g_usb_version_error(false);
 #ifdef ROS2
    std::shared_ptr<MultiSensorPublisher> g_ros_object = nullptr;
 #else
@@ -49,6 +53,9 @@ static capture_Image_List_t g_latest_rgb;
 static bool g_renderer_initialized = false;
 static std::shared_ptr<rawCloudRender> g_renderer = nullptr;

+ // usb device
+static std::string TARGET_VENDOR = "2207";
+static std::string TARGET_PRODUCT = "0019";
 // Global configuration variables
 int g_sendrgb = 1;
 int g_sendimu = 1;
@@ -58,9 +65,89 @@ int g_sendcloudslam = 0;
 int g_sendcloudrender = 0;
 int g_sendrgb_compressed = 0;

-// Function declarations
 void clear_all_queues();

+// detect USB3.0
+bool isUsb3OrHigher(const std::string& vendorId, const std::string& productId) {
+    std::string command = "lsusb -d " + vendorId + ":" + productId + " -v | grep 'bcdUSB'";
+    
+    std::array<char, 128> buffer;
+    std::string result;
+    std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(command.c_str(), "r"), pclose);
+    if (!pipe) {
+        throw std::runtime_error("popen() failed!");
+    }
+    while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) {
+        result += buffer.data();
+    }
+    
+    if (result.empty()) {
+        #ifdef ROS2
+            RCLCPP_ERROR(rclcpp::get_logger("usb_check"), "Failed to get USB version information");
+        #else
+            ROS_ERROR("Failed to get USB version information");
+        #endif
+        return false;
+    }
+    
+    // find bcdUSB
+    size_t pos = result.find("bcdUSB");
+    if (pos == std::string::npos) {
+        #ifdef ROS2
+            RCLCPP_ERROR(rclcpp::get_logger("usb_check"), "bcdUSB field not found in lsusb output");
+        #else
+            ROS_ERROR("bcdUSB field not found in lsusb output");
+        #endif
+        return false;
+    }
+    
+    std::string versionStr = result.substr(pos + 7); // "bcdUSB" + space
+    float version = std::stof(versionStr);
+    
+    #ifdef ROS2
+        RCLCPP_INFO(rclcpp::get_logger("usb_check"), "Detected USB version: %.1f", version);
+    #else
+        ROS_INFO("Detected USB version: %.1f", version);
+    #endif
+    return version >= 3.0;
+}
+
+bool isUsbDevicePresent(const std::string& vendorId, const std::string& productId) {
+    std::ifstream devicesList("/sys/bus/usb/devices");
+    if (devicesList.is_open()) {
+        std::string line;
+        while (std::getline(devicesList, line)) {
+            if (line.find('.') != std::string::npos) continue;
+            if (line.empty()) continue;
+            
+            std::string vendorPath = "/sys/bus/usb/devices/" + line + "/idVendor";
+            std::ifstream vendorFile(vendorPath);
+            if (vendorFile.is_open()) {
+                std::string vendorContent;
+                if (std::getline(vendorFile, vendorContent)) {
+                    vendorContent.erase(vendorContent.find_last_not_of(" \n\r\t") + 1);
+                    
+                    std::string productPath = "/sys/bus/usb/devices/" + line + "/idProduct";
+                    std::ifstream productFile(productPath);
+                    if (productFile.is_open()) {
+                        std::string productContent;
+                        if (std::getline(productFile, productContent)) {
+                            productContent.erase(productContent.find_last_not_of(" \n\r\t") + 1);
+                            
+                            if (vendorContent == vendorId && productContent == productId) {
+                                return true;
+                            }
+                        }
+                        productFile.close();
+                    }
+                }
+                vendorFile.close();
+            }
+        }
+        devicesList.close();
+    }
+    return false;
+}
 // Get package share path
 std::string get_package_share_path(const std::string& package_name) {
 #ifdef ROS2
@@ -144,24 +231,39 @@ static void lidar_data_callback(const lidar_data_t *data, void *user_data)
    }
 }

-// Lidar device callback
 static void lidar_device_callback(const lidar_device_info_t* device, bool attach)
 {
    int type = LIDAR_MODE_SLAM;
+    static std::chrono::steady_clock::time_point software_connect_start; 
+    static bool software_connect_timing = false; 
+    
    if(attach == true) {
        #ifdef ROS2
-            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device attaching...");
+            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Hardware connected, starting software connection...");
        #else
-            ROS_INFO("Device attaching...");
+            ROS_INFO("Hardware connected, starting software connection...");
        #endif
+        if (!isUsb3OrHigher(TARGET_VENDOR, TARGET_PRODUCT)) {
+            #ifdef ROS2
+                RCLCPP_FATAL(rclcpp::get_logger("device_cb"), 
+                            "Device connected to USB 2.0 port. This device requires USB 3.0 or higher. Exiting program.");
+            #else
+                ROS_FATAL("Device connected to USB 2.0 port. This device requires USB 3.0 or higher. Exiting program.");
+            #endif
+
+            g_usb_version_error = true;
+            system("pkill -f rviz");
+            exit(1);
+            return;
+        }
+
+        software_connect_start = std::chrono::steady_clock::now();
+        software_connect_timing = true;
        
-        // Clean up existing device resources
        if (odinDevice) {
-            // Skip stopping data stream, unregistering callbacks, closing device, destroying device
            odinDevice = nullptr;
        }
        
-        // Create new device
        if (lidar_create_device(const_cast<lidar_device_info_t*>(device), &odinDevice)) {
            #ifdef ROS2
                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Create device failed");
@@ -171,7 +273,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            return;
        }
        
-        // Open device
        if (lidar_open_device(odinDevice)) {
            #ifdef ROS2
                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Open device failed");
@@ -183,39 +284,58 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            return;
        }
        
-        // Get package path
 	const std::string package_name = "odin_ros_driver";
 	std::string config_dir = "";
 	#ifdef ROS2
-	    // Get source code directory (not install directory)
 	    char* ros_workspace = std::getenv("COLCON_PREFIX_PATH");
 	    if (ros_workspace) {
-		// Infer source directory from COLCON_PREFIX_PATH
 		std::string workspace_path(ros_workspace);
-		// Remove "/install" part
 		size_t pos = workspace_path.find("/install");
 		if (pos != std::string::npos) {
 		    config_dir = workspace_path.substr(0, pos) + "/src/odin_ros_driver/config";
 		} else {
-		    // Fallback to install directory
 		    config_dir = ament_index_cpp::get_package_share_directory(package_name) + "/config";
 		}
 	    } else {
-		// Fallback to install directory
 		config_dir = ament_index_cpp::get_package_share_directory(package_name) + "/config";
 	    }
 	#else
 	    config_dir = ros::package::getPath(package_name) + "/config";
 	#endif

-        // Print path information
        #ifdef ROS2
            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Calibration files will be saved to: %s", config_dir.c_str());
        #else
            ROS_INFO("Calibration files will be saved to: %s", config_dir.c_str());
        #endif
        
-        // Get calibration files - using modified function
+        auto now = std::chrono::steady_clock::now();
+        auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - software_connect_start);
+        if (elapsed.count() >= 60) {
+            #ifdef ROS2
+                RCLCPP_FATAL(rclcpp::get_logger("device_cb"), 
+                            "Software connection timed out after 60 seconds. Exiting program.");
+            #else
+                ROS_FATAL("Software connection timed out after 60 seconds. Exiting program.");
+            #endif
+            
+            if (odinDevice) {
+                lidar_close_device(odinDevice);
+                lidar_destory_device(odinDevice);
+                odinDevice = nullptr;
+            }
+            
+            g_connection_timeout = true;
+            return;
+        }
+        
+        if(lidar_get_version(odinDevice)) {
+            printf("get version failed.\n");
+        }
+        else {
+            printf("get version success.\n");
+        }
+        
        if (lidar_get_calib_file(odinDevice, config_dir.c_str())) {
            #ifdef ROS2
                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Failed to get calibration file");
@@ -234,7 +354,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            ROS_INFO("Successfully retrieved calibration files");
        #endif
        
-        //  Move point cloud renderer initialization here
        std::string calib_config = config_dir + "/calib.yaml";
        if (std::filesystem::exists(calib_config)) {
            g_renderer = std::make_shared<rawCloudRender>();
@@ -271,7 +390,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            return;
        }
 
-        // Register callback
        lidar_data_callback_info_t data_callback_info;
        data_callback_info.data_callback = lidar_data_callback;
        data_callback_info.user_data = &odinDevice;
@@ -288,7 +406,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            return;
        }
        
-        // Start data stream
        if (lidar_start_stream(odinDevice, type)) {
            #ifdef ROS2
                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Start stream failed");
@@ -301,7 +418,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            return;
        }
        
-        // Activate stream types based on configuration
        if (g_sendrgb) {
            lidar_activate_stream_type(odinDevice, LIDAR_DT_RAW_RGB);
        }
@@ -318,11 +434,17 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            lidar_activate_stream_type(odinDevice, LIDAR_DT_SLAM_CLOUD);
        }
        
+        software_connect_timing = false;
        deviceConnected = true;
-        deviceDisconnected = false;  // Reset disconnection flag
+        deviceDisconnected = false; 
+        
        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Software connection successful in %ld seconds", 
+                       std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now() - software_connect_start).count());
            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device ready and streams activated");
        #else
+            ROS_INFO("Software connection successful in %ld seconds", 
+                    std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now() - software_connect_start).count());
            ROS_INFO("Device ready and streams activated");
        #endif
    } else {
@@ -332,11 +454,9 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            ROS_INFO("Device detaching...");
        #endif

-        // Set device disconnection flag
        deviceConnected = false;
        deviceDisconnected = true;

-        // Clear all message queues
        clear_all_queues();

        #ifdef ROS2
@@ -346,7 +466,6 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
        #endif
    }
 }
-
 int main(int argc, char *argv[])
 {
 #ifdef ROS2
@@ -362,9 +481,7 @@ int main(int argc, char *argv[])
    try {
        std::string package_path = get_package_share_path("odin_ros_driver");
        std::string config_file = package_path + "/config/control_command.yaml";   
-        
-        
-
+      
        odin_ros_driver::YamlParser parser(config_file);
        if (!parser.loadConfig()) {
            #ifdef ROS2
@@ -394,30 +511,43 @@ int main(int argc, char *argv[])
        lidar_log_set_level(LIDAR_LOG_INFO);

        if (lidar_system_init(lidar_device_callback)) {
-        #ifdef ROS2
-                    RCLCPP_ERROR(node->get_logger(), "Lidar system init failed");
-        #else
-                    ROS_ERROR("Lidar system init failed");
-        #endif
-                    return -1;
-        }
-
-        #ifdef ROS2
-                RCLCPP_INFO(node->get_logger(), "Waiting for device connection...");
-        #else
-                ROS_INFO("Waiting for device connection...");
-        #endif
-
-        // Wait indefinitely for device connection
-        while (!deviceConnected) {
            #ifdef ROS2
-                RCLCPP_INFO(node->get_logger(), "Waiting for device connection...");
+                RCLCPP_ERROR(node->get_logger(), "Lidar system init failed");
            #else
-                ROS_INFO("Waiting for device connection...");
+                ROS_ERROR("Lidar system init failed");
            #endif
-            std::this_thread::sleep_for(std::chrono::seconds(1)); // Check every second
+            return -1;
        }
+        

+        bool usbPresent = false;
+        bool usbVersionChecked = false; 
+        while (!deviceConnected) {
+            usbPresent = isUsbDevicePresent(TARGET_VENDOR, TARGET_PRODUCT); 
+            if (usbPresent) { 
+                if (!usbVersionChecked) {
+                    usbVersionChecked = true;
+                    
+                    if (!isUsb3OrHigher(TARGET_VENDOR, TARGET_PRODUCT)) {
+                        #ifdef ROS2
+                            RCLCPP_FATAL(node->get_logger(), 
+                                        "Device connected to USB 2.0 port. This device requires USB 3.0 or higher. Exiting program.Please use USB 3.0 and restart the device.");
+                        #else
+                            ROS_FATAL("Device connected to USB 2.0 port. This device requires USB 3.0 or higher. Exiting program .Please use USB 3.0 and restart the device.");
+                        #endif
+                        
+                        lidar_system_deinit();
+                        return 1;
+                    }
+                }
+            }
+            
+            #ifdef ROS2
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+            #else
+                ros::Duration(1.0).sleep();
+            #endif
+        }
    } catch (const std::exception& e) {
        #ifdef ROS2
                RCLCPP_ERROR(node->get_logger(), "Exception: %s", e.what());
@@ -493,4 +623,4 @@ int main(int argc, char *argv[])
    lidar_system_deinit();

    return 0;
-}
+}