diff --git a/stimBuffApis/livoxGen1/livoxGen1.cpp b/stimBuffApis/livoxGen1/livoxGen1.cpp index 9a4c1e8..b8d5e98 100644 --- a/stimBuffApis/livoxGen1/livoxGen1.cpp +++ b/stimBuffApis/livoxGen1/livoxGen1.cpp @@ -15,6 +15,7 @@ #include #include #include +#include "pcloudStimulusBuffer.h" namespace smo { @@ -24,6 +25,26 @@ namespace stim_buff { static const SmoCallbacks* smoHooksPtr = nullptr; static SmoThreadingModelDesc smoThreadingModelDesc; +// Local collection of stimulus buffers +static std::vector> attachedStimBuffs; + +// Get stimulus buffer by device attachment spec +static std::shared_ptr +getStimBuff(const std::shared_ptr& spec) +{ + for (const auto& stimBuff : attachedStimBuffs) + { + // Compare device selectors to find matching buffer + if (stimBuff->deviceAttachmentSpec.deviceSelector + == spec->deviceSelector) + { + return stimBuff; + } + } + + return nullptr; +} + // LivoxProto1 library state struct LivoxProto1DllState { @@ -56,22 +77,6 @@ struct LivoxProto1DllState }; 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 @@ -88,7 +93,8 @@ public: public: const std::shared_ptr spec; - std::shared_ptr device; + std::shared_ptr stimBuff; + private: std::unique_ptr delayTimer; @@ -99,22 +105,32 @@ public: { if (!success || !dev) { - std::cerr << __func__ << ": Failed to create Livox device: " + std::cerr << __func__ << ": Failed to create/find Livox device: " << context->spec->deviceSelector << std::endl; context->callOriginalCb(false, context->spec); return; } - g_attachedDevices.push_back(dev); + // Create and add PcloudStimulusBuffer to collection + StimulusBuffer::PcloudFormatDesc formatDesc; + formatDesc.format = StimulusBuffer::PcloudFormatDesc::Format::XYZI; + auto pcloudStimBuff = std::make_shared( + *context->spec, dev, formatDesc); + + context->stimBuff = pcloudStimBuff; + dev->nAttachedStimBuffs++; + attachedStimBuffs.push_back(pcloudStimBuff); + if (1 || OptionParser::getOptions().verbose) { - std::cout << __func__ << ": Successfully attached Livox " + std::cout << __func__ << ": Successfully attached/found Livox " "device: " << context->spec->deviceSelector << " (ID: " << context->spec->deviceIdentifier << ")\n"; } - // Set the device in the context and enable point cloud data after 5ms delay - context->device = dev; + /* Delay here because getOrCreate just sent HandshakeReq, so device + * may not yet be ready for another command. + */ context->delayedEnablePcloudData(context); } @@ -126,8 +142,8 @@ public: { std::cerr << __func__ << ": Failed to enable pcloud data for dev " << context->spec->deviceSelector << std::endl; - context->callOriginalCb(false, context->spec); - return; + + // Fallthrough. } if (1 || OptionParser::getOptions().verbose) @@ -145,7 +161,7 @@ public: { // Initialize timer with device's component thread delayTimer = std::make_unique( - device->componentThread->getIoService()); + context->stimBuff->device->componentThread->getIoService()); delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); delayTimer->async_wait( @@ -169,7 +185,7 @@ public: } (*livoxProto1.livoxProto1_device_enablePcloudDataReq)( - device, + context->stimBuff->device, {context, std::bind( &AttachDeviceReq::attachDeviceReq2, context.get(), context, @@ -183,16 +199,16 @@ class DetachDeviceReq public: DetachDeviceReq( const std::shared_ptr& spec, - const std::shared_ptr& device, + const std::shared_ptr& stimBuff, smo::Callback cb) : smo::NonPostedAsynchronousContinuation( std::move(cb)), - spec(spec), device(device) + spec(spec), stimBuff(stimBuff) {} public: const std::shared_ptr spec; - std::shared_ptr device; + std::shared_ptr stimBuff; private: std::unique_ptr delayTimer; @@ -203,8 +219,8 @@ public: { if (!success) { - std::cerr << __func__ << ": Failed to disable pcloud data for dev " - << context->spec->deviceSelector << std::endl; + std::cerr << __func__ << ": Failed to disable pcloud data for " + "stimbuff " << context->spec->deviceSelector << std::endl; // Fallthrough. } @@ -212,68 +228,24 @@ public: 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 deviceToRemove = getDevice(context->spec->deviceSelector); - if (!deviceToRemove) - { - std::cerr << __func__ << ": Race condition: device not found " - "in g_attachedDevices for detachment: " - << context->spec->deviceIdentifier << "\n"; - context->callOriginalCb(false, context->spec); - return; - } - - // 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()); + context->stimBuff->device->componentThread->getIoService()); delayTimer->expires_from_now(boost::posix_time::milliseconds(5)); delayTimer->async_wait( std::bind( - &DetachDeviceReq::delayedDestroyDeviceCallback, + &DetachDeviceReq::detachDeviceReq1_delayed, context.get(), context, std::placeholders::_1)); } // Callback for the delayed destroyDeviceReq - void delayedDestroyDeviceCallback( + void detachDeviceReq1_delayed( std::shared_ptr context, const boost::system::error_code& error) { @@ -284,13 +256,49 @@ public: // Fallthrough. } + // Remove stimulus buffer from collection before destroying device + context->stimBuff->device->nAttachedStimBuffs--; + auto it = std::find( + attachedStimBuffs.begin(), attachedStimBuffs.end(), + context->stimBuff); + if (it != attachedStimBuffs.end()) + { attachedStimBuffs.erase(it); } + (*livoxProto1.livoxProto1_destroyDeviceReq)( - context->device, + context->stimBuff->device, {context, std::bind( &DetachDeviceReq::detachDeviceReq2, context.get(), context, std::placeholders::_1)}); } + + void detachDeviceReq2( + std::shared_ptr context, + bool success) + { + if (!success) + { + std::cerr << __func__ << ": Failed to destroy dev " + "device " << context->spec->deviceSelector << " for stimbuff." + "\n"; + + /** NOTE: + * There's a decent argument for falling through here and still + * removing the stimulus buffer from attachedStimBuffs. + */ + context->callOriginalCb(false, context->spec); + return; + } + + if (1 || OptionParser::getOptions().verbose) + { + std::cout << __func__ << ": Successfully detached pcloud stimbuff " + "for device " << context->spec->deviceSelector + << " and possibly also destroyed device.\n"; + } + + context->callOriginalCb(success, context->spec); + } }; // Callback function declarations @@ -386,8 +394,8 @@ extern "C" int livoxGen1_initializeInd(void) extern "C" int livoxGen1_finalizeInd(void) { - // Clear all attached devices - g_attachedDevices.clear(); + + attachedStimBuffs.clear(); // Call LivoxProto1 library exit function if (livoxProto1.livoxProto1_exit) { @@ -419,17 +427,32 @@ extern "C" void livoxGen1_attachDeviceReq( auto request = std::make_shared(desc, cb); - // Check if device already exists - auto existingDevice = getDevice(desc->deviceSelector); - if (existingDevice) + // Check if stimulus buffer already exists in the collection + auto pcloudStimBuff = std::static_pointer_cast( + getStimBuff(desc)); + + if (pcloudStimBuff) { - // 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); + request->stimBuff = pcloudStimBuff; + + // Check if device's point cloud data is already active + if (pcloudStimBuff->device && pcloudStimBuff->device->pcloudDataActive) + { + // Point cloud data is already active, call success callback + request->callOriginalCb(true, request->spec); + return; + } + + /* Enable pcloud data. Don't need delay since this no commands were + * sent to device prior to us reaching here. + */ + (*livoxProto1.livoxProto1_device_enablePcloudDataReq)( + pcloudStimBuff->device, + {request, std::bind( + &AttachDeviceReq::attachDeviceReq2, + request.get(), request, + std::placeholders::_1)}); + return; } @@ -522,22 +545,22 @@ extern "C" void livoxGen1_detachDeviceReq( Callback cb ) { - // Find the device in our collection - auto device = getDevice(desc->deviceSelector); - if (!device) + // Check if stimulus buffer exists in the collection + auto stimBuff = std::static_pointer_cast( + getStimBuff(desc)); + + if (!stimBuff) { - std::cerr << std::string(__func__) - << ": Device not found for detachment: " - << desc->deviceIdentifier << std::endl; cb.callbackFn(false, desc); return; } - auto request = std::make_shared(desc, device, cb); + auto request = std::make_shared( + desc, stimBuff, cb); // Disable point cloud data first (*livoxProto1.livoxProto1_device_disablePcloudDataReq)( - device, + stimBuff->device, {request, std::bind( &DetachDeviceReq::detachDeviceReq1, request.get(), request, diff --git a/stimBuffApis/livoxGen1/pcloudStimulusBuffer.h b/stimBuffApis/livoxGen1/pcloudStimulusBuffer.h index 26f5f23..a3fd664 100644 --- a/stimBuffApis/livoxGen1/pcloudStimulusBuffer.h +++ b/stimBuffApis/livoxGen1/pcloudStimulusBuffer.h @@ -3,6 +3,7 @@ #include #include +#include namespace smo { namespace stim_buff { @@ -21,12 +22,14 @@ class PcloudStimulusBuffer public: explicit PcloudStimulusBuffer( const device::DeviceAttachmentSpec& deviceAttachmentSpec, + std::shared_ptr &device, const PcloudFormatDesc& formatDesc) : StimulusBuffer(deviceAttachmentSpec), + deviceAttachmentSpec(deviceAttachmentSpec), device(device), formatDesc(formatDesc) {} - ~PcloudStimulusBuffer(); + ~PcloudStimulusBuffer() = default; // Non-copyable, movable PcloudStimulusBuffer(const PcloudStimulusBuffer&) = delete; @@ -34,7 +37,9 @@ public: PcloudStimulusBuffer(PcloudStimulusBuffer&&) = default; PcloudStimulusBuffer& operator=(PcloudStimulusBuffer&&) = default; -private: +public: + device::DeviceAttachmentSpec deviceAttachmentSpec; + std::shared_ptr device; PcloudFormatDesc formatDesc; };