diff --git a/senseApis/livoxGen1/livoxGen1.cpp b/senseApis/livoxGen1/livoxGen1.cpp index 80d3541..6ce2447 100644 --- a/senseApis/livoxGen1/livoxGen1.cpp +++ b/senseApis/livoxGen1/livoxGen1.cpp @@ -4,10 +4,12 @@ #include #include #include +#include #include #include #include #include +#include namespace smo { namespace sense_api { @@ -22,7 +24,8 @@ struct LivoxProto1DllState LivoxProto1DllState() : dlopenHandle(nullptr, DlCloser), livoxProto1_main(nullptr), - livoxProto1_exit(nullptr) + livoxProto1_exit(nullptr), + livoxProto1_getOrCreateDevice(nullptr) {} static void DlCloser(void* handle) @@ -35,10 +38,14 @@ struct LivoxProto1DllState std::unique_ptr dlopenHandle; livoxProto1_mainFn *livoxProto1_main; livoxProto1_exitFn *livoxProto1_exit; + livoxProto1_getOrCreateDeviceFn *livoxProto1_getOrCreateDevice; }; static LivoxProto1DllState livoxProto1; +// Attached Livox devices +static std::vector> g_attachedDevices; + // Callback function declarations extern "C" int livoxGen1_initializeInd(void); extern "C" int livoxGen1_finalizeInd(void); @@ -93,24 +100,34 @@ extern "C" int livoxGen1_initializeInd(void) dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_main")); livoxProto1.livoxProto1_exit = reinterpret_cast( dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_exit")); + livoxProto1.livoxProto1_getOrCreateDevice = reinterpret_cast< + livoxProto1_getOrCreateDeviceFn *>( + dlsym( + livoxProto1.dlopenHandle.get(), + "livoxProto1_getOrCreateDevice")); - if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit) { + if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit + || !livoxProto1.livoxProto1_getOrCreateDevice) + { throw std::runtime_error( std::string(__func__) + ": Failed to get LivoxProto1 library functions"); } // Call LivoxProto1 library main function - livoxProto1.livoxProto1_main(smoThreadingModelDesc.componentThread); + (*livoxProto1.livoxProto1_main)(smoThreadingModelDesc.componentThread); return 0; // Success } extern "C" int livoxGen1_finalizeInd(void) { - // TODO: Implement finalization logic + // Clear all attached devices + g_attachedDevices.clear(); + + // Call LivoxProto1 library exit function if (livoxProto1.livoxProto1_exit) { - livoxProto1.livoxProto1_exit(); + (*livoxProto1.livoxProto1_exit)(); } if (livoxProto1.dlopenHandle) @@ -127,8 +144,84 @@ extern "C" int livoxGen1_attachDeviceReq( const std::shared_ptr& desc ) { - // TODO: Implement device attachment logic - (void)desc; // Suppress unused parameter warning + if (!livoxProto1.livoxProto1_getOrCreateDevice) + { + throw std::runtime_error( + std::string(__func__) + ": LivoxProto1 getOrCreateDevice function " + "not available"); + } + + // Parse integer parameters from provider params with defaults + int handshakeTimeoutMs = 250; // Default: 50ms + int retryDelayMs = 500; // Default: 500ms + uint8_t smoSubnetNbits = 24; // Default: /24 subnet + uint16_t dataPort = 56000; // Default data port + uint16_t cmdPort = 56001; // Default command port + uint16_t imuPort = 56002; // Default IMU port + // Default: 10.42.0.1 + std::string smoIp = "10.42.0.1"; + + // Parse optional integer parameters from provider params + for (const auto& param : desc->providerParams) + { + if (param.first == "handshake-timeout-ms") + { + handshakeTimeoutMs = smo::device::DeviceAttachmentSpec + ::parseRequiredParamAsInt(*desc, "handshake-timeout-ms"); + } else if (param.first == "retry-delay-ms") + { + retryDelayMs = smo::device::DeviceAttachmentSpec + ::parseRequiredParamAsInt(*desc, "retry-delay-ms"); + } else if (param.first == "smo-subnet-nbits") + { + smoSubnetNbits = static_cast( + smo::device::DeviceAttachmentSpec + ::parseRequiredParamAsInt(*desc, "smo-subnet-nbits")); + } else if (param.first == "data-port") + { + dataPort = static_cast( + smo::device::DeviceAttachmentSpec + ::parseRequiredParamAsInt(*desc, "data-port")); + } else if (param.first == "cmd-port") + { + cmdPort = static_cast( + smo::device::DeviceAttachmentSpec + ::parseRequiredParamAsInt(*desc, "cmd-port")); + } else if (param.first == "imu-port") + { + imuPort = static_cast( + smo::device::DeviceAttachmentSpec + ::parseRequiredParamAsInt(*desc, "imu-port")); + } else if (param.first == "smo-ip") { + smoIp = param.second; + } + else + { + throw std::runtime_error( + std::string(__func__) + ": Unknown provider parameter: " + + param.first); + } + } + + // Call getOrCreateDevice with parsed parameters + auto device = (*livoxProto1.livoxProto1_getOrCreateDevice)( + desc->deviceSelector, // deviceIdentifier (broadcast code) + smoThreadingModelDesc.componentThread, + handshakeTimeoutMs, retryDelayMs, + smoIp, smoSubnetNbits, + dataPort, cmdPort, imuPort); + + if (!device) + { + throw std::runtime_error( + std::string(__func__) + ": Failed to create Livox device: " + + desc->deviceSelector); + } + + g_attachedDevices.push_back(device); + std::cout << __func__ << ": Successfully attached Livox device: " + << desc->deviceSelector << " (ID: " << desc->deviceIdentifier << ")\n"; + return 0; // Success } @@ -136,8 +229,32 @@ extern "C" int livoxGen1_detachDeviceReq( const std::shared_ptr& desc ) { - // TODO: Implement device detachment logic - (void)desc; // Suppress unused parameter warning + // Find and remove the device from our collection + auto it = std::find_if(g_attachedDevices.begin(), g_attachedDevices.end(), + [&desc](const std::shared_ptr& dev) { + /** EXPLANATION: + * Compare the first 14 characters of the deviceIdentifier with + * the first 14 characters of the deviceSelector + */ + const std::string& devId = dev->discoveredDevice.deviceIdentifier; + std::string devIdPrefix = devId.substr( + 0, std::min(14, devId.size())); + + return devIdPrefix == desc->deviceSelector.substr( + 0, std::min(14, desc->deviceSelector.size())); + } + ); + + if (it == g_attachedDevices.end()) + { + std::cerr << __func__ << ": Device not found for detachment: " + << desc->deviceIdentifier << "\n"; + return -1; // Device not found + } + + g_attachedDevices.erase(it); + std::cout << __func__ << ": Successfully detached Livox device: " + << desc->deviceIdentifier << "\n"; return 0; // Success }