support Odin 1. support Ubuntu 18.04,Ubuntu 20.04,Ubuntu22.04. support for ROS1 and ROS2.

2025-07-11 19:48:05 +08:00
parent 39bb89648b
commit 31bff6868b
18 changed files with 2274 additions and 1 deletions
@@ -0,0 +1,330 @@
+#include "host_sdk_sample.h"
+#include "yaml_parser.h"
+#include <filesystem> 
+#include <thread>
+#include <string>
+#include <stdexcept>
+#include <atomic>
+
+#ifdef ROS2
+    #include <ament_index_cpp/get_package_share_directory.hpp>
+#else
+    #include <ros/package.h>
+#endif
+
+static device_handle odinDevice = nullptr;
+static std::shared_ptr<MultiSensorPublisher> g_ros_object;
+static std::atomic<bool> deviceConnected(false);
+
+// 定义获取包路径的函数
+std::string get_package_share_path(const std::string& package_name) {
+#ifdef ROS2
+    try {
+        return ament_index_cpp::get_package_share_directory(package_name);
+    } catch (const std::exception& e) {
+        throw std::runtime_error("Package not found: " + std::string(e.what()));
+    }
+#else
+    try {
+        return ros::package::getPath(package_name);
+    } catch (const ros::InvalidNameException& e) {
+        throw std::runtime_error("Package not found: " + std::string(e.what()));
+    }
+#endif
+}
+
+// 全局配置变量
+static int g_sendrgb = 1;
+static int g_sendimu = 1;
+static int g_senddtof = 1;
+static int g_sendodom = 1;
+static int g_sendcloudslam = 0;
+
+static void lidar_data_callback(const lidar_data_t *data, void *user_data)
+{
+    device_handle *dev_handle = static_cast<device_handle *>(user_data);
+    if(!dev_handle || !data) {
+        printf("Invalid device handle or data.\n");
+        return;
+    }
+    
+    switch(data->type) {
+        case LIDAR_DT_NONE:
+            printf("empty lidar data type: %x\n", data->type);
+            break;
+        case LIDAR_DT_RAW_RGB:
+            if (g_sendrgb) {
+                g_ros_object->publishRgb((capture_Image_List_t *)&data->stream);
+            }
+            break;
+        case LIDAR_DT_RAW_IMU:
+            if (g_sendimu) {
+                g_ros_object->publishImu((icm_6aixs_data_t *)data->stream.imageList[0].pAddr);
+            }
+            break;
+        case LIDAR_DT_RAW_DTOF:
+            if (g_senddtof) {
+                g_ros_object->publishIntensityCloud((capture_Image_List_t *)&data->stream, 1);
+            }
+            break;
+        case LIDAR_DT_SLAM_CLOUD:
+            if (g_sendcloudslam) {
+                g_ros_object->publishPC2XYZRGBA((capture_Image_List_t *)&data->stream, 0);
+            }
+            break;
+        case LIDAR_DT_SLAM_ODOMETRY:
+            if (g_sendodom) {
+                g_ros_object->publishOdometry((capture_Image_List_t *)&data->stream);
+            }
+            break;
+        default:
+            printf("Unknown lidar data type: %x", data->type);
+            return;
+    }
+}
+
+static void lidar_device_callback(const lidar_device_info_t* device, bool attach)
+{
+    int type = LIDAR_MODE_SLAM;
+    if(attach == true) {
+        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device attaching...");
+        #else
+            ROS_INFO("Device attaching...");
+        #endif
+        
+        // 清理任何已存在的设备
+        if (odinDevice) {
+            lidar_stop_stream(odinDevice, type);
+            lidar_unregister_stream_callback(odinDevice);
+            lidar_close_device(odinDevice);
+            lidar_destory_device(odinDevice);
+            odinDevice = nullptr;
+        }
+        
+        // 修复：使用const_cast移除const限定符
+        if (lidar_create_device(const_cast<lidar_device_info_t*>(device), &odinDevice)) {
+            #ifdef ROS2
+                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Create device failed");
+            #else
+                ROS_ERROR("Create device failed");
+            #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;
+        }
+        
+        // 设置模式
+        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;
+        }
+        
+        // 注册回调
+        lidar_data_callback_info_t data_callback_info;
+        data_callback_info.data_callback = lidar_data_callback;
+        data_callback_info.user_data = &odinDevice;
+
+        if (lidar_register_stream_callback(odinDevice, data_callback_info)) {
+            #ifdef ROS2
+                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Register callback failed");
+            #else
+                ROS_ERROR("Register callback failed");
+            #endif
+            lidar_close_device(odinDevice);
+            lidar_destory_device(odinDevice);
+            odinDevice = nullptr;
+            return;
+        }
+        
+        // 启动数据流
+        if (lidar_start_stream(odinDevice, type)) {
+            #ifdef ROS2
+                RCLCPP_ERROR(rclcpp::get_logger("device_cb"), "Start stream failed");
+            #else
+                ROS_ERROR("Start stream failed");
+            #endif
+            lidar_close_device(odinDevice);
+            lidar_destory_device(odinDevice);
+            odinDevice = nullptr;
+            return;
+        }
+        
+        // 根据配置激活数据流类型
+        if (g_sendrgb) {
+            lidar_activate_stream_type(odinDevice, LIDAR_DT_RAW_RGB);
+        }
+        if (g_sendimu) {
+            lidar_activate_stream_type(odinDevice, LIDAR_DT_RAW_IMU);
+        }
+        if (g_sendodom) {
+            lidar_activate_stream_type(odinDevice, LIDAR_DT_SLAM_ODOMETRY);
+        }
+        if (g_senddtof) {
+            lidar_activate_stream_type(odinDevice, LIDAR_DT_RAW_DTOF);
+        }
+        if (g_sendcloudslam) {
+            lidar_activate_stream_type(odinDevice, LIDAR_DT_SLAM_CLOUD);
+        }
+        
+        deviceConnected = true;
+        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device ready and streams activated");
+        #else
+            ROS_INFO("Device ready and streams activated");
+        #endif
+    } else {
+        #ifdef ROS2
+            RCLCPP_INFO(rclcpp::get_logger("device_cb"), "Device detaching...");
+        #else
+            ROS_INFO("Device detaching...");
+        #endif
+        
+        deviceConnected = false;
+        
+        if (odinDevice) {
+            lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM);
+            lidar_unregister_stream_callback(odinDevice);
+            lidar_close_device(odinDevice);
+            lidar_destory_device(odinDevice);
+            odinDevice = nullptr;
+        }
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    // ROS初始化
+    #ifdef ROS2
+        rclcpp::init(argc, argv);
+        auto node = std::make_shared<rclcpp::Node>("lydros_node");
+        g_ros_object = std::make_shared<MultiSensorPublisher>(node);
+    #else
+        ros::init(argc, argv, "lydros_node");
+        ros::NodeHandle nh;
+        g_ros_object = std::make_shared<MultiSensorPublisher>(nh);
+    #endif
+
+    try {
+        std::string package_path = get_package_share_path("odin_ros_driver");
+        std::string config_file = package_path + "/config/control_command.yaml";   
+        
+        // 创建 YAML 解析器
+        odin_ros_driver::YamlParser parser(config_file);
+        
+        // 加载配置
+        if (!parser.loadConfig()) {
+        #ifdef ROS2
+            RCLCPP_ERROR(node->get_logger(), "Failed to load config file: %s", config_file.c_str());
+        #else
+            ROS_ERROR("Failed to load config file: %s", config_file.c_str());
+        #endif
+            return -1;
+        }
+        
+        // 获取键值映射
+        auto keys = parser.getRegisterKeys();
+        
+        // 打印配置
+        parser.printConfig();
+        
+        // 获取键值函数
+        auto get_key_value = [&](const std::string& key_name, int default_value) -> int {  
+            auto it = keys.find(key_name);
+            if (it != keys.end()) {
+                return it->second;
+            }
+            return default_value;
+        };
+        
+        // 读取配置值到全局变量
+        g_sendrgb = get_key_value("sendrgb", 1);
+        g_sendimu = get_key_value("sendimu", 1);
+        g_senddtof = get_key_value("senddtof", 1);
+        g_sendodom = get_key_value("sendodom", 1);
+        g_sendcloudslam = get_key_value("sendcloudslam", 0);
+        
+        // 设置日志级别
+        lidar_log_set_level(LIDAR_LOG_INFO);
+
+        // 初始化系统，启动USB监控
+        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;
+        }
+        
+        // 等待设备连接（最长30秒）
+        #ifdef ROS2
+            RCLCPP_INFO(node->get_logger(), "Waiting for device connection...");
+        #else
+            ROS_INFO("Waiting for device connection...");
+        #endif
+        
+        auto start = std::chrono::steady_clock::now();
+        while (!deviceConnected) {
+            auto now = std::chrono::steady_clock::now();
+            auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - start);
+            
+            if (elapsed.count() >= 30) {
+            #ifdef ROS2
+                RCLCPP_ERROR(node->get_logger(), "No device connected after 30 seconds");
+            #else
+                ROS_ERROR("No device connected after 30 seconds");
+            #endif
+                lidar_system_deinit();
+                return -1;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+    
+    } catch (const std::exception& e) {
+        #ifdef ROS2
+            RCLCPP_ERROR(node->get_logger(), "Exception: %s", e.what());
+        #else
+            ROS_ERROR("Exception: %s", e.what());
+        #endif
+            lidar_system_deinit();
+            return -1;
+    }
+
+    // ROS主循环
+    #ifdef ROS2
+        rclcpp::spin(node);
+        rclcpp::shutdown();
+    #else
+        ros::spin();
+        ros::shutdown();
+    #endif
+
+    // 清理
+    if (odinDevice) {
+        lidar_stop_stream(odinDevice, LIDAR_MODE_SLAM);
+        lidar_unregister_stream_callback(odinDevice);
+        lidar_close_device(odinDevice);
+        lidar_destory_device(odinDevice);
+    }
+    lidar_system_deinit();
+    
+    return 0;
+}