diff --git a/README.md b/README.md index 1c7c1de..7b28e72 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.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 \ No newline at end of file +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. diff --git a/config/odin_ros.rviz b/config/odin_ros.rviz index 4435d61..78f2e72 100644 --- a/config/odin_ros.rviz +++ b/config/odin_ros.rviz @@ -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: - Yaw: 2.2254059314727783 + Yaw: 3.3054051399230957 Saved: ~ Window Geometry: Displays: diff --git a/config/odin_ros2.rviz b/config/odin_ros2.rviz index bfb001a..c7f2e27 100644 --- a/config/odin_ros2.rviz +++ b/config/odin_ros2.rviz @@ -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 diff --git a/include/host_sdk_sample.h b/include/host_sdk_sample.h index b3048b8..ec14631 100644 --- a/include/host_sdk_sample.h +++ b/include/host_sdk_sample.h @@ -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 iter_x(*msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); sensor_msgs::PointCloud2Iterator iter_intensity(*msg, "intensity"); + sensor_msgs::PointCloud2Iterator iter_confidence(*msg, "confidence"); - float* xyz_data = static_cast(cloud.pAddr); - uint16_t* intensity_data = static_cast(stream->imageList[2].pAddr); - + float* xyz_data_f = static_cast(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(stream->imageList[2].pAddr); + uint16_t* confidence_data = static_cast(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(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(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(intensity); + } + ++iter_intensity; + + *iter_confidence = 0; + ++iter_confidence; + + valid_points++; } - +} { std::lock_guard lock(pcd_queue_mutex_); @@ -847,7 +885,7 @@ private: rgb_pub_ = node_->create_publisher("odin1/image", 10); cloud_pub_ = node_->create_publisher("odin1/cloud_raw", 10); xyzrgbacloud_pub_ = node_->create_publisher("odin1/cloud_slam", 10); - odom_publisher_ = node_->create_publisher("odin1/odometry_map", 10); + odom_publisher_ = node_->create_publisher("odin1/odometry", 10); rgbcloud_pub_ = node_->create_publisher("odin1/cloud_render", 10); compressed_rgb_pub_ = node_->create_publisher("odin1/image/compressed", 10); #endif @@ -858,7 +896,7 @@ private: rgb_pub_ = nh.advertise("odin1/image", 10); cloud_pub_ = nh.advertise("odin1/cloud_raw", 10); xyzrgbacloud_pub_ = nh.advertise("odin1/cloud_slam", 10); - odom_publisher_ = nh.advertise("odin1/odometry_map", 10); + odom_publisher_ = nh.advertise("odin1/odometry", 10); rgbcloud_pub_ = nh.advertise("odin1/cloud_render", 10); compressed_rgb_pub_ = nh.advertise("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 */ \ No newline at end of file +#define EXIT "q" /* exit sample */ diff --git a/include/lidar_api.h b/include/lidar_api.h index f4fee44..b3546ba 100644 --- a/include/lidar_api.h +++ b/include/lidar_api.h @@ -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 \ No newline at end of file diff --git a/include/lidar_api_type.h b/include/lidar_api_type.h index 0bb5315..3d79a3f 100644 --- a/include/lidar_api_type.h +++ b/include/lidar_api_type.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 } diff --git a/lib/liblydHostApi_amd.a b/lib/liblydHostApi_amd.a index 8425063..c983024 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 90710d8..6f785bf 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 4a52227..98397cc 100644 --- a/src/host_sdk_sample.cpp +++ b/src/host_sdk_sample.cpp @@ -17,7 +17,10 @@ #include #include #include - +#include +#include +#include +#include #ifdef ROS2 #include #include @@ -25,13 +28,14 @@ #include #include #endif - +#define ros_driver_version "0.3.0" // Global variable declarations static device_handle odinDevice = nullptr; static std::atomic deviceConnected(false); static std::atomic deviceDisconnected(false); // Device disconnection flag static std::mutex device_mutex; // Device operation mutex lock - +static std::atomic g_connection_timeout(false); +static std::atomic g_usb_version_error(false); #ifdef ROS2 std::shared_ptr 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 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 buffer; + std::string result; + std::unique_ptr 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(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(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(); @@ -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::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::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; -} \ No newline at end of file +}