From 6f4a2dd6498332a1b70b824b6a6810185741b950 Mon Sep 17 00:00:00 2001 From: Hayodea Hekol Date: Sat, 25 Oct 2025 12:53:07 -0400 Subject: [PATCH] LivoxGen/Proto1: Move en/disablePcloudData call to Gen1 We no longer try to enable pcloud data as part of the connectReq() sequence. Instead we separate them so that a device can be connected but not be issuing pcloud data. --- commonLibs/livoxProto1/device.cpp | 60 ++---- commonLibs/livoxProto1/livoxProto1.cpp | 28 +++ commonLibs/livoxProto1/livoxProto1.h | 17 +- stimBuffApis/livoxGen1/livoxGen1.cpp | 244 ++++++++++++++++++++----- 4 files changed, 254 insertions(+), 95 deletions(-) diff --git a/commonLibs/livoxProto1/device.cpp b/commonLibs/livoxProto1/device.cpp index 37a219c..a4d079e 100644 --- a/commonLibs/livoxProto1/device.cpp +++ b/commonLibs/livoxProto1/device.cpp @@ -142,17 +142,6 @@ public: * WE need to assign the ipAddr to the Device being connected up. */ - // Helper method to delay and then call enablePcloudDataReq - void delayedEnablePcloudData(std::shared_ptr context) - { - delayTimer.expires_from_now(boost::posix_time::milliseconds(5)); - delayTimer.async_wait( - std::bind( - &ConnectReq::connectReq3, - context.get(), context, - std::placeholders::_1)); - } - // Callback methods for the connection sequence void connectReq1( std::shared_ptr context, @@ -181,8 +170,7 @@ public: context->device.discoveredDevice.ipAddr = ipAddr; context->device.startHeartbeat(); - // Use async delay before enabling point cloud data - delayedEnablePcloudData(context); + context->connectReq3(context, success); } void connectReq2( @@ -201,39 +189,18 @@ public: context->device.discoveredDevice.ipAddr = ipAddr; context->device.startHeartbeat(); - // Use async delay before enabling point cloud data - delayedEnablePcloudData(context); + context->connectReq3(context, success); } void connectReq3( - std::shared_ptr context, - const boost::system::error_code& error - ) - { - if (error) - { - // Timer was cancelled or error occurred - context->callOriginalCallbackWithFailure(); - return; - } - - context->device.enablePcloudDataReq( - {context, std::bind(&ConnectReq::connectReq4, - context.get(), context, - std::placeholders::_1)}); - } - - void connectReq4( - std::shared_ptr context, - bool success + std::shared_ptr context, bool success ) { if (!success) { - std::cerr << __func__ << ": Failed to enable point cloud data for " - "device (" << context->device.discoveredDevice.deviceIdentifier + std::cerr << __func__ << ": Failed to connect to device " + "(" << context->device.discoveredDevice.deviceIdentifier << ") @(" << context->device.discoveredDevice.ipAddr << ").\n"; - context->callOriginalCallbackWithFailure(); return; } @@ -1550,13 +1517,16 @@ protected: response->command.cmd_id == 0x04 && response->ret_code == 0x00)) { - std::cerr << __func__ << ": Failed to enable pcloud data for dev " - "(" << context->device.discoveredDevice.deviceIdentifier - << ") @(" << context->device.discoveredDevice.ipAddr << "). " - << "cmd_set: " << (int)response->command.cmd_set - << ", cmd_id: " << (int)response->command.cmd_id - << ", ret_code: " << (int)response->ret_code << "\n"; - + if (OptionParser::getOptions().verbose) + { + std::cout << __func__ << ": Failed to en/disable pcloud data " + "for device " + "(" << context->device.discoveredDevice.deviceIdentifier + << ") @(" << context->device.discoveredDevice.ipAddr << "). " + << "cmd_set: " << (int)response->command.cmd_set + << ", cmd_id: " << (int)response->command.cmd_id + << ", ret_code: " << (int)response->ret_code << "\n"; + } context->callOriginalCallbackWithFailure(); return; } diff --git a/commonLibs/livoxProto1/livoxProto1.cpp b/commonLibs/livoxProto1/livoxProto1.cpp index fd785c7..7c185e9 100644 --- a/commonLibs/livoxProto1/livoxProto1.cpp +++ b/commonLibs/livoxProto1/livoxProto1.cpp @@ -62,4 +62,32 @@ void livoxProto1_exit(void) livoxProto1::exit(); } +void livoxProto1_device_enablePcloudDataReq( + std::shared_ptr device, + smo::Callback callback +) +{ + if (!device) + { + throw std::runtime_error(std::string(__func__) + + ": Device pointer is null"); + } + + device->enablePcloudDataReq(callback); +} + +void livoxProto1_device_disablePcloudDataReq( + std::shared_ptr device, + smo::Callback callback +) +{ + if (!device) + { + throw std::runtime_error(std::string(__func__) + + ": Device pointer is null"); + } + + device->disablePcloudDataReq(callback); +} + } // extern "C" diff --git a/commonLibs/livoxProto1/livoxProto1.h b/commonLibs/livoxProto1/livoxProto1.h index 25553c5..b054411 100644 --- a/commonLibs/livoxProto1/livoxProto1.h +++ b/commonLibs/livoxProto1/livoxProto1.h @@ -67,14 +67,29 @@ typedef void livoxProto1_destroyDeviceReqFn( std::shared_ptr device, smo::Callback callback); +typedef std::function + livoxProto1_device_enablePcloudDataReqCbFn; +typedef void livoxProto1_device_enablePcloudDataReqFn( + std::shared_ptr device, + smo::Callback callback); + +typedef std::function + livoxProto1_device_disablePcloudDataReqCbFn; +typedef void livoxProto1_device_disablePcloudDataReqFn( + std::shared_ptr device, + smo::Callback callback); + livoxProto1_mainFn livoxProto1_main; livoxProto1_exitFn livoxProto1_exit; livoxProto1_getOrCreateDeviceReqFn livoxProto1_getOrCreateDeviceReq; livoxProto1_destroyDeviceReqFn livoxProto1_destroyDeviceReq; +livoxProto1_device_enablePcloudDataReqFn livoxProto1_device_enablePcloudDataReq; +livoxProto1_device_disablePcloudDataReqFn + livoxProto1_device_disablePcloudDataReq; #ifdef __cplusplus } #endif -#endif // LIVOXPROTO1_H \ No newline at end of file +#endif // LIVOXPROTO1_H diff --git a/stimBuffApis/livoxGen1/livoxGen1.cpp b/stimBuffApis/livoxGen1/livoxGen1.cpp index 481c778..9a4c1e8 100644 --- a/stimBuffApis/livoxGen1/livoxGen1.cpp +++ b/stimBuffApis/livoxGen1/livoxGen1.cpp @@ -12,7 +12,9 @@ #include #include #include +#include #include +#include namespace smo { @@ -30,7 +32,9 @@ struct LivoxProto1DllState livoxProto1_main(nullptr), livoxProto1_exit(nullptr), livoxProto1_getOrCreateDeviceReq(nullptr), - livoxProto1_destroyDeviceReq(nullptr) + livoxProto1_destroyDeviceReq(nullptr), + livoxProto1_device_enablePcloudDataReq(nullptr), + livoxProto1_device_disablePcloudDataReq(nullptr) {} static void DlCloser(void* handle) @@ -45,13 +49,30 @@ struct LivoxProto1DllState livoxProto1_exitFn *livoxProto1_exit; livoxProto1_getOrCreateDeviceReqFn *livoxProto1_getOrCreateDeviceReq; livoxProto1_destroyDeviceReqFn *livoxProto1_destroyDeviceReq; + livoxProto1_device_enablePcloudDataReqFn + *livoxProto1_device_enablePcloudDataReq; + livoxProto1_device_disablePcloudDataReqFn + *livoxProto1_device_disablePcloudDataReq; }; static LivoxProto1DllState livoxProto1; - // Attached Livox devices static std::vector> g_attachedDevices; +// Utility function to find a device in g_attachedDevices by identifier +static std::shared_ptr getDevice( + const std::string& deviceIdentifier + ) +{ + auto it = std::find_if(g_attachedDevices.begin(), g_attachedDevices.end(), + [&deviceIdentifier](const std::shared_ptr& dev) { + return livoxProto1::comms::deviceIdentifiersEqual( + dev->discoveredDevice.deviceIdentifier, deviceIdentifier); + }); + + return (it != g_attachedDevices.end()) ? *it : nullptr; +} + // Continuation classes for async operations class AttachDeviceReq : public smo::NonPostedAsynchronousContinuation @@ -67,13 +88,16 @@ public: public: const std::shared_ptr spec; + std::shared_ptr device; +private: + std::unique_ptr delayTimer; public: void attachDeviceReq1( std::shared_ptr context, bool success, std::shared_ptr dev) { - if (!dev) + if (!success || !dev) { std::cerr << __func__ << ": Failed to create Livox device: " << context->spec->deviceSelector << std::endl; @@ -89,8 +113,68 @@ public: << context->spec->deviceIdentifier << ")\n"; } + // Set the device in the context and enable point cloud data after 5ms delay + context->device = dev; + context->delayedEnablePcloudData(context); + } + + void attachDeviceReq2( + std::shared_ptr context, + bool success) + { + if (!success) + { + std::cerr << __func__ << ": Failed to enable pcloud data for dev " + << context->spec->deviceSelector << std::endl; + context->callOriginalCb(false, context->spec); + return; + } + + if (1 || OptionParser::getOptions().verbose) + { + std::cout << __func__ << ": Enabled pcloud data for device: " + << context->spec->deviceSelector << std::endl; + } + context->callOriginalCb(success, context->spec); } + + // Helper method to delay and then call enablePcloudDataReq + void delayedEnablePcloudData( + std::shared_ptr context) + { + // Initialize timer with device's component thread + delayTimer = std::make_unique( + device->componentThread->getIoService()); + + delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); + delayTimer->async_wait( + std::bind( + &AttachDeviceReq::delayedEnablePcloudDataCallback, + context.get(), context, + std::placeholders::_1)); + } + + // Callback for the delayed enablePcloudDataReq + void delayedEnablePcloudDataCallback( + std::shared_ptr context, + const boost::system::error_code& error) + { + if (error) + { + std::cerr << __func__ << ": Timer error: " << error.message() + << std::endl; + context->callOriginalCb(false, context->spec); + return; + } + + (*livoxProto1.livoxProto1_device_enablePcloudDataReq)( + device, + {context, std::bind( + &AttachDeviceReq::attachDeviceReq2, + context.get(), context, + std::placeholders::_1)}); + } }; class DetachDeviceReq @@ -99,42 +183,55 @@ class DetachDeviceReq public: DetachDeviceReq( const std::shared_ptr& spec, + const std::shared_ptr& device, smo::Callback cb) : smo::NonPostedAsynchronousContinuation( std::move(cb)), - spec(spec) + spec(spec), device(device) {} public: const std::shared_ptr spec; + std::shared_ptr device; +private: + std::unique_ptr delayTimer; public: void detachDeviceReq1( std::shared_ptr context, bool success) + { + if (!success) + { + std::cerr << __func__ << ": Failed to disable pcloud data for dev " + << context->spec->deviceSelector << std::endl; + // Fallthrough. + } + + // Add 5ms delay before destroying device + context->delayedDestroyDevice(context); + } + + void detachDeviceReq2( + std::shared_ptr context, + bool success) { if (!success) { std::cerr << __func__ << ": Failed to destroy Livox device: " << context->spec->deviceIdentifier << "\n"; + + /** NOTE: + * There's a decent argument for falling through here and still + * removing the device from g_attachedDevices. + */ context->callOriginalCb(false, context->spec); return; } // Find the device in g_attachedDevices and remove it. - auto eraseIt = std::find_if( - g_attachedDevices.begin(), g_attachedDevices.end(), - [context](const std::shared_ptr& dev) - { - const std::string& devId = dev->discoveredDevice.deviceIdentifier; - std::string devIdPrefix = devId.substr( - 0, std::min(14, devId.size())); - return devIdPrefix == context->spec->deviceSelector.substr( - 0, std::min(14, context->spec->deviceSelector.size())); - } - ); - - if (eraseIt == g_attachedDevices.end()) + auto deviceToRemove = getDevice(context->spec->deviceSelector); + if (!deviceToRemove) { std::cerr << __func__ << ": Race condition: device not found " "in g_attachedDevices for detachment: " @@ -143,12 +240,57 @@ public: return; } - g_attachedDevices.erase(eraseIt); - std::cout << __func__ << ": Successfully detached Livox device: " - << context->spec->deviceIdentifier << "\n"; + // Remove the device from the collection + g_attachedDevices.erase( + std::remove( + g_attachedDevices.begin(), g_attachedDevices.end(), + deviceToRemove), + g_attachedDevices.end()); + + if (1 || OptionParser::getOptions().verbose) + { + std::cout << __func__ << ": Successfully detached Livox device: " + << context->spec->deviceIdentifier << "\n"; + } context->callOriginalCb(success, context->spec); } + + // Helper method to delay and then call destroyDeviceReq + void delayedDestroyDevice( + std::shared_ptr context) + { + // Initialize timer with device's component thread + delayTimer = std::make_unique( + device->componentThread->getIoService()); + + delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); + delayTimer->async_wait( + std::bind( + &DetachDeviceReq::delayedDestroyDeviceCallback, + context.get(), context, + std::placeholders::_1)); + } + + // Callback for the delayed destroyDeviceReq + void delayedDestroyDeviceCallback( + std::shared_ptr context, + const boost::system::error_code& error) + { + if (error) + { + std::cerr << __func__ << ": Timer error: " << error.message() + << std::endl; + // Fallthrough. + } + + (*livoxProto1.livoxProto1_destroyDeviceReq)( + context->device, + {context, std::bind( + &DetachDeviceReq::detachDeviceReq2, + context.get(), context, + std::placeholders::_1)}); + } }; // Callback function declarations @@ -213,10 +355,22 @@ extern "C" int livoxGen1_initializeInd(void) dlsym( livoxProto1.dlopenHandle.get(), "livoxProto1_destroyDeviceReq")); + livoxProto1.livoxProto1_device_enablePcloudDataReq = reinterpret_cast< + livoxProto1_device_enablePcloudDataReqFn *>( + dlsym( + livoxProto1.dlopenHandle.get(), + "livoxProto1_device_enablePcloudDataReq")); + livoxProto1.livoxProto1_device_disablePcloudDataReq = reinterpret_cast< + livoxProto1_device_disablePcloudDataReqFn *>( + dlsym( + livoxProto1.dlopenHandle.get(), + "livoxProto1_device_disablePcloudDataReq")); if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit || !livoxProto1.livoxProto1_getOrCreateDeviceReq - || !livoxProto1.livoxProto1_destroyDeviceReq) + || !livoxProto1.livoxProto1_destroyDeviceReq + || !livoxProto1.livoxProto1_device_enablePcloudDataReq + || !livoxProto1.livoxProto1_device_disablePcloudDataReq) { throw std::runtime_error( std::string(__func__) + @@ -263,13 +417,20 @@ extern "C" void livoxGen1_attachDeviceReq( "not available"); } - for (const auto& dev : g_attachedDevices) + auto request = std::make_shared(desc, cb); + + // Check if device already exists + auto existingDevice = getDevice(desc->deviceSelector); + if (existingDevice) { - if (dev->discoveredDevice.deviceIdentifier == desc->deviceIdentifier) - { - cb.callbackFn(true, desc); - return; - } + // Device already exists, set device and enable point cloud data + std::cout << __func__ << ": Dev " + << existingDevice->discoveredDevice.deviceIdentifier + << " already attached for DASpec: " << desc->deviceSelector + << ".\n"; + request->device = existingDevice; + request->delayedEnablePcloudData(request); + return; } // Parse integer parameters from provider params with defaults @@ -344,8 +505,6 @@ extern "C" void livoxGen1_attachDeviceReq( } } - auto request = std::make_shared(desc, cb); - (*livoxProto1.livoxProto1_getOrCreateDeviceReq)( desc->deviceSelector, // deviceIdentifier (broadcast code) componentThread, @@ -363,23 +522,9 @@ extern "C" void livoxGen1_detachDeviceReq( Callback cb ) { - // 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()) + // Find the device in our collection + auto device = getDevice(desc->deviceSelector); + if (!device) { std::cerr << std::string(__func__) << ": Device not found for detachment: " @@ -388,10 +533,11 @@ extern "C" void livoxGen1_detachDeviceReq( return; } - auto request = std::make_shared(desc, cb); + auto request = std::make_shared(desc, device, cb); - (*livoxProto1.livoxProto1_destroyDeviceReq)( - *it, + // Disable point cloud data first + (*livoxProto1.livoxProto1_device_disablePcloudDataReq)( + device, {request, std::bind( &DetachDeviceReq::detachDeviceReq1, request.get(), request,