From 7a51f02d970cfd51daa93cb0f020e40fc08a3c84 Mon Sep 17 00:00:00 2001 From: Hayodea Hekol Date: Sat, 15 Nov 2025 04:02:25 -0400 Subject: [PATCH] livoxGen1: Implement StimBuff add/del from StimProducers There seems to be a bug where two or more stimProducers or stimBuffs get initialized at once but we can deal with that tomorrow. --- include/user/stimulusProducer.h | 4 + stimBuffApis/livoxGen1/livoxGen1.cpp | 343 +++++++++++++----- .../livoxGen1/pcloudStimulusProducer.cpp | 22 ++ .../livoxGen1/pcloudStimulusProducer.h | 5 + 4 files changed, 281 insertions(+), 93 deletions(-) diff --git a/include/user/stimulusProducer.h b/include/user/stimulusProducer.h index 3bf7f25..0fff2f0 100644 --- a/include/user/stimulusProducer.h +++ b/include/user/stimulusProducer.h @@ -72,6 +72,10 @@ public: std::shared_ptr getAttachedStimulusBuffer( const std::shared_ptr& spec) const; + virtual std::shared_ptr getOrCreateAttachedStimulusBuffer( + const std::shared_ptr& deviceAttachmentSpec, + int histbuffMs) = 0; + protected: SpinLock frameAssemblyRateLimiter; diff --git a/stimBuffApis/livoxGen1/livoxGen1.cpp b/stimBuffApis/livoxGen1/livoxGen1.cpp index 1bc47fd..6034584 100644 --- a/stimBuffApis/livoxGen1/livoxGen1.cpp +++ b/stimBuffApis/livoxGen1/livoxGen1.cpp @@ -30,6 +30,33 @@ static SmoThreadingModelDesc smoThreadingModelDesc; // Local collection of stimulus producers static std::vector> attachedStimulusProducers; +// Check if a StimulusProducer matches the requested stim feature +static bool isProducerForStimFeature( + const std::shared_ptr& stimProducer, + const std::string& qualeIfaceApi) +{ + // Check if the qualeIfaceApi requires a PcloudStimulusProducer + if (qualeIfaceApi == "mesh" || qualeIfaceApi == "pcloudIntensity" || + qualeIfaceApi == "pcloudAmbience") + { + // Attempt to upcast to PcloudStimulusProducer + auto pcloudProducer = std::dynamic_pointer_cast( + stimProducer); + + return pcloudProducer != nullptr; + } + else if (qualeIfaceApi == "gyro" || qualeIfaceApi == "accel") + { + /** TODO: + * Add upcast mappings for gyro and accel later when we implement + * ImuStimulusProducer. + */ + return false; + } + + return false; +} + // Get stimulus producer by device attachment spec static std::shared_ptr getStimulusProducer( @@ -41,7 +68,8 @@ getStimulusProducer( // Compare device selectors to find matching buffer if (livoxProto1::comms::deviceIdentifiersEqual( stimProducer->deviceAttachmentSpec->deviceSelector, - spec->deviceSelector)) + spec->deviceSelector) + && isProducerForStimFeature(stimProducer, spec->qualeIfaceApi)) { return stimProducer; } @@ -50,6 +78,37 @@ getStimulusProducer( return nullptr; } +// Helper function to parse histbuffMs from device attachment spec +static int parseHistbuffMs( + const std::shared_ptr& spec) +{ + int histbuffMs = 30000; // Default: 30000ms (30 seconds) + const std::vector histbuffParamNames = { + "history-buffer-duration-ms", + "hist-buff-duration-ms", + "histbuff-duration-ms", + "histbuff-ms" + }; + + // Loop through synonyms in reverse order; lattermost synonym wins. + for (auto synIt = histbuffParamNames.rbegin(); + synIt != histbuffParamNames.rend(); ++synIt) + { + const auto& paramName = *synIt; + try { + histbuffMs = smo::device::DeviceAttachmentSpec + ::parseRequiredParamAsInt( + spec->qualeIfaceApiParams, paramName); + break; // Found and parsed successfully + } catch (const std::exception&) { + // Parameter not found or parse error, continue to next synonym + continue; + } + } + + return histbuffMs; +} + // LivoxProto1DllState constructor implementation LivoxProto1DllState::LivoxProto1DllState() : dlopenHandle(nullptr, DlCloser), @@ -94,6 +153,34 @@ public: private: std::unique_ptr delayTimer; + // Helper method to ensure StimBuffer is attached + // Returns true if successful, false on error + bool ensureStimBufferAttached(std::shared_ptr context) + { + if (!context->stimProducer) + { + std::cerr << __func__ << ": stimProducer is null" << std::endl; + return false; + } + + // Parse histbuffMs + int histbuffMs = parseHistbuffMs(context->spec); + + // Call getOrCreateAttachedStimulusBuffer (may throw, catch and return failure) + try { + context->stimProducer->getOrCreateAttachedStimulusBuffer( + context->spec, histbuffMs); + } catch (const std::exception& e) { + std::cerr << __func__ << ": Failed to create StimBuffer: " + << e.what() << ". Producer is committed, DeviceReattacher will retry." + << std::endl; + // Return false so DeviceReattacher can retry later + return false; + } + + return true; + } + public: void attachDeviceReq1( std::shared_ptr context, @@ -153,12 +240,12 @@ public: (*livoxProto1.livoxProto1_device_getReturnModeReq)( context->deviceTmp, {context, std::bind( - &AttachDeviceReq::attachDeviceReq3, + &AttachDeviceReq::attachDeviceReq3_doCreateStimProducer, context.get(), context, std::placeholders::_1, std::placeholders::_2)}); } - void attachDeviceReq3( + void attachDeviceReq3_doCreateStimProducer( std::shared_ptr context, bool success, uint8_t mode) { @@ -171,33 +258,19 @@ public: return; } - // Parse history buffer duration from quale-iface-api-params - int histbuffMs = 30000; // Default: 30000ms (30 seconds) - (void)histbuffMs; - const std::vector histbuffParamNames = { - "history-buffer-duration-ms", - "hist-buff-duration-ms", - "histbuff-duration-ms", - "histbuff-ms" - }; - - // Loop through synonyms in reverse order; lattermost synonym wins. - for (auto synIt = histbuffParamNames.rbegin(); - synIt != histbuffParamNames.rend(); ++synIt) + /* Check if PcloudStimulusProducer already exists + * (race condition or double-add) + */ + auto existingProducer = getStimulusProducer(context->spec); + if (existingProducer) { - const auto& paramName = *synIt; - try { - histbuffMs = smo::device::DeviceAttachmentSpec - ::parseRequiredParamAsInt( - context->spec->qualeIfaceApiParams, paramName); - break; // Found and parsed successfully - } catch (const std::exception&) { - // Parameter not found or parse error, continue to next synonym - continue; - } + throw std::runtime_error( + std::string(__func__) + ": PcloudStimulusProducer already " + "exists for device " + context->spec->deviceSelector + " " + "(race condition or double-add)"); } - // Create and add PcloudStimulusProducer to collection now that device is ready + // Create & add PcloudStimulusProducer to collection since dev now ready PcloudStimulusProducer::PcloudFormatDesc formatDesc; formatDesc.format = PcloudStimulusProducer::PcloudFormatDesc::Format ::XYZI; @@ -218,46 +291,51 @@ public: << std::endl; } - context->delayedEnablePcloudData(context); + // Ensure StimBuffer is attached + attachDeviceReq4_doCreateStimBuff_maybeDirectlyCalled(context); } - // Helper method to delay and then call enablePcloudDataReq - void delayedEnablePcloudData( - std::shared_ptr context) + // Ensure StimBuffer is attached + void attachDeviceReq4_doCreateStimBuff_maybeDirectlyCalled( + std::shared_ptr context + ) { - // Initialize timer with device's component thread - delayTimer = std::make_unique( - context->stimProducer->device->componentThread->getIoService()); - - delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); - delayTimer->async_wait( - std::bind( - &AttachDeviceReq::attachDeviceReq4, - context.get(), context, - std::placeholders::_1)); - } - - void attachDeviceReq4( - std::shared_ptr context, - const boost::system::error_code& error) - { - if (error) + // Ensure StimBuffer is attached + if (!ensureStimBufferAttached(context)) { - std::cerr << __func__ << ": Timer error: " << error.message() + context->callOriginalCb(false, context->spec); + return; + } + + // Continue to enable pcloud data if needed + attachDeviceReq5_doEnablePcloudData_maybeDirectlyCalled(context); + } + + // Enable pcloud data if needed + void attachDeviceReq5_doEnablePcloudData_maybeDirectlyCalled( + std::shared_ptr context + ) + { + if (!context->stimProducer || !context->stimProducer->device) + { + std::cerr << __func__ << ": stimProducer or device is null" << std::endl; context->callOriginalCb(false, context->spec); return; } + /* Enable pcloud data. Don't need delay since no commands were + * sent to device prior to us reaching here (or delay already handled). + */ (*livoxProto1.livoxProto1_device_enablePcloudDataReq)( context->stimProducer->device, {context, std::bind( - &AttachDeviceReq::attachDeviceReq5, - context.get(), context, - std::placeholders::_1)}); + &AttachDeviceReq::attachDeviceReq6, + context.get(), context, + std::placeholders::_1)}); } - void attachDeviceReq5( + void attachDeviceReq6( std::shared_ptr context, bool success) { @@ -286,16 +364,16 @@ class DetachDeviceReq public: DetachDeviceReq( const std::shared_ptr& spec, - const std::shared_ptr& stimProducer, + const std::shared_ptr& stimBuffer, smo::Callback cb) : smo::NonPostedAsynchronousContinuation( std::move(cb)), - spec(spec), stimProducer(stimProducer) + spec(spec), stimBuffer(stimBuffer) {} public: const std::shared_ptr spec; - std::shared_ptr stimProducer; + std::shared_ptr stimBuffer; private: std::unique_ptr delayTimer; @@ -319,9 +397,9 @@ public: void delayedDestroyDevice( std::shared_ptr context) { - // Initialize timer with device's component thread + // Initialize timer with LivoxGen1 metadata io_service delayTimer = std::make_unique( - context->stimProducer->device->componentThread->getIoService()); + smoThreadingModelDesc.componentThread->getIoService()); delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); delayTimer->async_wait( @@ -343,17 +421,65 @@ public: // Fallthrough. } - context->stimProducer->stop(); - // Remove stimulus producer from collection before destroying device - context->stimProducer->device->nAttachedStimulusProducers--; + // Remove StimBuffer from collection if it exists + if (!context->stimBuffer) + { + throw std::runtime_error(std::string(__func__) + + ": stimBuffer (API: " + context->spec->stimBuffApi + ") " + + "is missing in detachDeviceReq1_delayed " + + "for device " + context->spec->deviceSelector); + } + + // Get the producer from the buffer's parent + auto& stimProducer = dynamic_cast( + context->stimBuffer->parent); + auto it = std::find( + stimProducer.attachedStimulusBuffers.begin(), + stimProducer.attachedStimulusBuffers.end(), + context->stimBuffer); + if (it != stimProducer.attachedStimulusBuffers.end()) + { + stimProducer.attachedStimulusBuffers.erase(it); + } + + // Clear specialized buffer members if they match + if (stimProducer.xyzStimulusBuffer == context->stimBuffer) + { stimProducer.xyzStimulusBuffer.reset(); } + if (stimProducer.iStimulusBuffer == context->stimBuffer) + { stimProducer.iStimulusBuffer.reset(); } + if (stimProducer.ambienceStimulusBuffer == context->stimBuffer) + { stimProducer.ambienceStimulusBuffer.reset(); } + + // Check if StimProducer has other buffers + if (!stimProducer.attachedStimulusBuffers.empty()) + { + // Other buffers exist - just remove this buffer, done + context->callOriginalCb(true, context->spec); + return; + } + + // No other buffers - stop and remove StimProducer + stimProducer.stop(); + // Remove stimulus producer from collection before destroying device + stimProducer.device->nAttachedStimulusProducers--; + // Find and remove the producer from the collection by comparing device + auto it2 = std::find_if( attachedStimulusProducers.begin(), attachedStimulusProducers.end(), - context->stimProducer); - if (it != attachedStimulusProducers.end()) - { attachedStimulusProducers.erase(it); } + [&stimProducer](const std::shared_ptr& p) + { + /** FIXME: + * When we implement the ImuStimulusProducer, we need to make + * sure we handle that properly here. + */ + auto pcloudProd = std::dynamic_pointer_cast(p); + return pcloudProd && pcloudProd->device == stimProducer.device; + }); + if (it2 != attachedStimulusProducers.end()) + { attachedStimulusProducers.erase(it2); } (*livoxProto1.livoxProto1_destroyDeviceReq)( - context->stimProducer->device, + stimProducer.device, {context, std::bind( &DetachDeviceReq::detachDeviceReq2, context.get(), context, @@ -399,8 +525,9 @@ extern "C" sal_mlo_detachDeviceReqFn livoxGen1_detachDeviceReq; static const StimBuffApiDesc livoxGen1ApiDesc = { .name = "livoxGen1", .exportedQualeIfaceApis = { - {.name = "pcloud"}, + {.name = "mesh"}, {.name = "pcloudIntensity"}, + {.name = "pcloudAmbience"}, {.name = "gyro"}, {.name = "accel"} }, @@ -523,34 +650,45 @@ extern "C" void livoxGen1_attachDeviceReq( auto request = std::make_shared(desc, cb); - // Check if stimulus producer already exists in the collection - auto pcloudDataProducer = std::static_pointer_cast( - getStimulusProducer(desc)); - - if (pcloudDataProducer) + // Case 1: Check if StimBuffer already exists + auto stimProducerBase = getStimulusProducer(desc); + if (stimProducerBase) { - request->stimProducer = pcloudDataProducer; + auto stimProducer = std::static_pointer_cast( + stimProducerBase); - // Check if device's point cloud data is already active - if (pcloudDataProducer->device && pcloudDataProducer->device->pcloudDataActive) + auto existingBuffer = stimProducer->getAttachedStimulusBuffer(desc); + if (existingBuffer) { - // Point cloud data is already active, call success callback - request->callOriginalCb(true, request->spec); + // StimBuffer exists, check if pcloud data is active + if (stimProducer->device && stimProducer->device->pcloudDataActive) + { + // Both StimBuffer and pcloud data are active, early return with success + request->callOriginalCb(true, request->spec); + return; + } + + // StimBuffer exists but pcloud data is not active, enable it + request->stimProducer = stimProducer; + request->attachDeviceReq5_doEnablePcloudData_maybeDirectlyCalled( + request); + return; } + else + { + // StimProducer exists, StimBuffer doesn't + request->stimProducer = stimProducer; + // Ensure StimBuffer is attached and enable pcloud data if needed + request->attachDeviceReq4_doCreateStimBuff_maybeDirectlyCalled( + request); - /* Enable pcloud data first. Don't need delay since no commands were - * sent to device prior to us reaching here. - */ - (*livoxProto1.livoxProto1_device_enablePcloudDataReq)( - pcloudDataProducer->device, - {request, std::bind( - &AttachDeviceReq::attachDeviceReq5, - request.get(), request, - std::placeholders::_1)}); - return; + return; + } } + // StimProducer doesn't exist - need to create device first + // Parse integer parameters from provider params with defaults /** EXPLANATION: * We may want to add a new param here called "command-delay-ms" to control @@ -654,18 +792,37 @@ extern "C" void livoxGen1_detachDeviceReq( Callback cb ) { - // Check if stimulus producer exists in the collection - auto stimProducer = std::static_pointer_cast( - getStimulusProducer(desc)); - - if (!stimProducer) + // Case 1: Check if StimBuffer doesn't exist (early return) + auto stimProducerBase = getStimulusProducer(desc); + if (!stimProducerBase) { - cb.callbackFn(false, desc); + // StimProducer doesn't exist, nothing to detach - success + cb.callbackFn(true, desc); return; } + auto stimProducer = std::dynamic_pointer_cast( + stimProducerBase); + + if (!stimProducer) + { + throw std::runtime_error(std::string(__func__) + + ": Failed to cast StimulusProducer to PcloudStimulusProducer " + "for device " + desc->deviceSelector); + } + + // Check if StimBuffer exists + auto stimBuffer = stimProducer->getAttachedStimulusBuffer(desc); + if (!stimBuffer) + { + // StimBuffer doesn't exist, nothing to detach - success + cb.callbackFn(true, desc); + return; + } + + // Case 2: StimBuffer exists - proceed with detach auto request = std::make_shared( - desc, stimProducer, cb); + desc, stimBuffer, cb); // Disable point cloud data first (*livoxProto1.livoxProto1_device_disablePcloudDataReq)( diff --git a/stimBuffApis/livoxGen1/pcloudStimulusProducer.cpp b/stimBuffApis/livoxGen1/pcloudStimulusProducer.cpp index 0c831c6..d299379 100644 --- a/stimBuffApis/livoxGen1/pcloudStimulusProducer.cpp +++ b/stimBuffApis/livoxGen1/pcloudStimulusProducer.cpp @@ -100,6 +100,28 @@ void produceStimFrameAck(void) { } +std::shared_ptr +PcloudStimulusProducer::getOrCreateAttachedStimulusBuffer( + const std::shared_ptr& deviceAttachmentSpec, + int histbuffMs + ) +{ + // Check if buffer already exists (idempotent) + auto existingBuffer = getAttachedStimulusBuffer(deviceAttachmentSpec); + if (existingBuffer) + { return existingBuffer; } + + // Create new PcloudXyzStimulusBuffer (for now, always use XYZ type) + auto buffer = std::make_shared( + *this, deviceAttachmentSpec, histbuffMs, openClInputConstraints); + + // Add to collection + attachedStimulusBuffers.push_back(buffer); + // Update specialized member + xyzStimulusBuffer = buffer; + return buffer; +} + void PcloudStimulusProducer::stimFrameProductionTimesliceInd() { produceFrameReq({nullptr, nullptr}); diff --git a/stimBuffApis/livoxGen1/pcloudStimulusProducer.h b/stimBuffApis/livoxGen1/pcloudStimulusProducer.h index a0adad5..6eefb71 100644 --- a/stimBuffApis/livoxGen1/pcloudStimulusProducer.h +++ b/stimBuffApis/livoxGen1/pcloudStimulusProducer.h @@ -62,6 +62,11 @@ public: void start() override; void stop() override; + std::shared_ptr getOrCreateAttachedStimulusBuffer( + const std::shared_ptr + &deviceAttachmentSpec, + int histbuffMs) override; + protected: void stimFrameProductionTimesliceInd() override;