#include "livox_mid70_sdk.h" #include "livox_mid70_def.h" #include <iostream> #include <thread> #include <chrono> #include <cstring>// 设备状态枚举 typedef enum {kDeviceStateDisconnect = 0,kDeviceStateConnect = 1,kDeviceStateSampling = 2, } Livox_mid70_DeviceState;// 设备信息结构 typedef struct {uint8_t handle;Livox_mid70_DeviceState device_state;DeviceInfo info; } Livox_mid70_DeviceItem;// 全局变量 Livox_mid70_DeviceItem mid70_devices[kMaxLidarCount]; const char* livox_mid70_broadcast_code = "6565N8V00200631"; // 请替换为实际雷达广播码// 设备广播信息回调 void OnMid70DeviceBroadcast(const BroadcastDeviceInfo* info) {if (info == nullptr) return;printf("发现设备广播: %s\n", info->broadcast_code); }// 设备状态变化回调 void OnMid70DeviceInfoChange(const DeviceInfo* info, DeviceEvent type) {if (info == nullptr) return;uint8_t handle = info->handle;if (handle >= kMaxLidarCount) return;if (type == kEventConnect) {mid70_devices[handle].device_state = kDeviceStateConnect;mid70_devices[handle].info = *info;printf("设备连接成功，句柄: %d\n", handle);}else if (type == kEventDisconnect) {mid70_devices[handle].device_state = kDeviceStateDisconnect;printf("设备断开连接，句柄: %d\n", handle);} }// 点云数据回调函数 void GetMid70LidarData(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data) {if (data == nullptr) return;switch (data->data_type) {case kCartesian: {LivoxRawPoint* points = (LivoxRawPoint*)data->data;for (uint32_t i = 0; i < data_num; i++) {float x = points[i].x / 1000.0f;float y = points[i].y / 1000.0f;float z = points[i].z / 1000.0f;float reflectivity = points[i].reflectivity;printf("点云坐标: (%.3f, %.3f, %.3f), 反射率: %d\n", x, y, z, (int)reflectivity);}break;}case kSpherical: {LivoxSpherPoint* spherical_points = (LivoxSpherPoint*)data->data;for (uint32_t i = 0; i < data_num; i++) {float depth = spherical_points[i].depth / 1000.0f;float theta = spherical_points[i].theta / 100.0f;float phi = spherical_points[i].phi / 100.0f;float reflectivity = spherical_points[i].reflectivity;printf("球坐标: 距离=%.3fm, 方位角=%.2f°, 俯仰角=%.2f°, 反射率: %d\n",depth, theta, phi, (int)reflectivity);}break;}case kExtendCartesian: {// 扩展笛卡尔坐标系数据LivoxExtendRawPoint* extend_points = (LivoxExtendRawPoint*)data->data;for (uint32_t i = 0; i < data_num; i++) {float x = extend_points[i].x / 1000.0f;float y = extend_points[i].y / 1000.0f;float z = extend_points[i].z / 1000.0f;float reflectivity = extend_points[i].reflectivity;if (reflectivity > 0){std::cout << "扩展笛卡尔坐标: (" << x << ", " << y << ", " << z << "), 反射: " << reflectivity << " , 标签: " << (int)extend_points[i].tag << std::endl;}// }break;}default:printf("未知数据类型: %d\n", (int)data->data_type);break;} }int main() {printf("=== Livox Mid-70雷达点云采集程序 ===\n");printf("开始初始化Livox Mid-70雷达...\n");// 1. 初始化SDKif (!Init()) {printf("SDK初始化失败!\n");return -1;}printf("Livox SDK初始化完成\n");// 显示SDK版本信息 LivoxSdkVersion livox_mid70_sdk_version;GetLivoxSdkVersion(&livox_mid70_sdk_version);printf("Livox SDK版本 %d.%d.%d\n", livox_mid70_sdk_version.major, livox_mid70_sdk_version.minor, livox_mid70_sdk_version.patch);// 初始化设备数组memset(mid70_devices, 0, sizeof(mid70_devices));// 2. 设置回调函数 SetBroadcastCallback(OnMid70DeviceBroadcast);SetDeviceStateUpdateCallback(OnMid70DeviceInfoChange);// 3. 开始设备发现if (!Start()) {printf("启动设备发现失败!\n");Uninit();return -1;}printf("开始设备发现...\n");// 4. 等待设备准备就绪printf("等待设备初始化...\n");std::this_thread::sleep_for(std::chrono::milliseconds(2000));// 5. 添加设备连接uint8_t livox_mid70_handle = 0;livox_status result = AddLidarToConnect(livox_mid70_broadcast_code, &livox_mid70_handle);if (result == kStatusSuccess) {mid70_devices[livox_mid70_handle].handle = livox_mid70_handle;mid70_devices[livox_mid70_handle].device_state = kDeviceStateDisconnect;printf("添加设备成功，句柄: %d\n", livox_mid70_handle);}else {printf("添加设备失败! 请检查:\n");printf("1. 雷达是否上电\n");printf("2. 网络连接是否正常\n");printf("3. 广播码是否正确\n");Uninit();return -1;}// 6. 等待设备连接printf("等待设备连接...\n");int wait_count = 0;while (mid70_devices[livox_mid70_handle].device_state != kDeviceStateConnect && wait_count < 10) {std::this_thread::sleep_for(std::chrono::milliseconds(500));wait_count++;}if (mid70_devices[livox_mid70_handle].device_state != kDeviceStateConnect) {printf("设备连接超时!\n");Uninit();return -1;}// 7. 设置点云数据回调 SetDataCallback(livox_mid70_handle, GetMid70LidarData, nullptr);printf("设置数据回调完成\n");// 8. 启动采样livox_status status = LidarStartSampling(livox_mid70_handle, nullptr, nullptr);if (status == kStatusSuccess) {printf("开始点云数据采集...\n");}else {printf("启动采样失败! 错误码: %d\n", status);Uninit();return -1;}// 9. 保持程序运行printf("程序运行中，采集10秒后自动停止...\n");printf("======================================\n");for (int i = 0; i < 10; i++) {printf("运行时间: %d秒\n", i + 1);std::this_thread::sleep_for(std::chrono::seconds(1));}// 10. 停止采样并清理资源printf("停止采样...\n");LidarStopSampling(livox_mid70_handle, nullptr, nullptr);Uninit();printf("程序正常结束\n");return 0; }
###########################