5f3d5c7818
Replace local param parsing with DeviceAttachmentSpec primitives for consistency with lcameraBuff and intrin threshold modules. Co-authored-by: Cursor <cursoragent@cursor.com>
767 lines
24 KiB
C++
767 lines
24 KiB
C++
#include <algorithm>
|
|
#include <dlfcn.h>
|
|
#include <iostream>
|
|
#include <opts.h>
|
|
#include <stdexcept>
|
|
|
|
#include <adapters/boostAsio/deadlineTimerAReq.h>
|
|
#include <livoxProto1/protocol.h>
|
|
#include <user/intrinThresholdParams.h>
|
|
#include <user/senseApiDesc.h>
|
|
|
|
#include "livoxGen1Internal.h"
|
|
|
|
namespace smo::stim_buff {
|
|
|
|
constexpr int LIVOX_GEN1_DEVICE_COMMAND_DELAY_MS = 5;
|
|
|
|
// Salmanoff hooks, obtained from SMO_GET_STIM_BUFF_API_DESC_FN_NAME().
|
|
const SmoCallbacks *smoHooksPtr = nullptr;
|
|
SmoThreadingModelDesc smoThreadingModelDesc;
|
|
|
|
// Local collection of stimulus producers
|
|
std::vector<std::shared_ptr<StimulusProducer>> attachedStimulusProducers;
|
|
|
|
LivoxProto1DllState::LivoxProto1DllState()
|
|
: dlopenHandle(nullptr, DlCloser),
|
|
livoxProto1_main(nullptr),
|
|
livoxProto1_exit(nullptr),
|
|
livoxProto1_getOrCreateDeviceCReq(nullptr),
|
|
livoxProto1_destroyDeviceCReq(nullptr),
|
|
livoxProto1_device_enablePcloudDataCReq(nullptr),
|
|
livoxProto1_device_disablePcloudDataCReq(nullptr),
|
|
livoxProto1_device_getReturnModeCReq(nullptr),
|
|
livoxProto1_getPcloudDataFdDesc(nullptr)
|
|
{}
|
|
|
|
void LivoxProto1DllState::DlCloser(void *handle)
|
|
{
|
|
if (handle) {
|
|
dlclose(handle);
|
|
}
|
|
}
|
|
|
|
LivoxProto1DllState livoxProto1;
|
|
|
|
std::shared_ptr<StimulusProducer> getStimulusProducer(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &spec)
|
|
{
|
|
for (const auto &stimProducer : attachedStimulusProducers)
|
|
{
|
|
if (livoxProto1::comms::deviceIdentifiersEqual(
|
|
stimProducer->deviceAttachmentSpec->deviceSelector,
|
|
spec->deviceSelector)
|
|
&& stimProducer->exportsQualeIfaceApi(spec->qualeIfaceApi))
|
|
{
|
|
return stimProducer;
|
|
}
|
|
}
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
size_t parseNDgramsPerFrame(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &spec)
|
|
{
|
|
const std::vector<std::string> nDgramsPerFrameParamNames = {
|
|
"n-dgrams-per-frame",
|
|
"num-dgrams-per-frame"
|
|
};
|
|
|
|
return static_cast<size_t>(
|
|
device::DeviceAttachmentSpec::parseOptionalParamAsIntWithSynonyms(
|
|
spec->stimBuffApiParams,
|
|
nDgramsPerFrameParamNames,
|
|
84));
|
|
}
|
|
|
|
LivoxProviderParams parseLivoxProviderParams(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &desc)
|
|
{
|
|
using device::DeviceAttachmentSpec;
|
|
|
|
const auto& providerParams = desc->providerParams;
|
|
|
|
static const std::vector<std::string> cmdTimeoutSynonyms = {
|
|
"cmd-timeout-ms",
|
|
"command-timeout-ms",
|
|
};
|
|
static const std::vector<std::string> retryDelaySynonyms = {
|
|
"retry-delay-ms",
|
|
};
|
|
static const std::vector<std::string> smoSubnetSynonyms = {
|
|
"smo-subnet-nbits",
|
|
};
|
|
static const std::vector<std::string> dataPortSynonyms = {
|
|
"data-port",
|
|
};
|
|
static const std::vector<std::string> cmdPortSynonyms = {
|
|
"cmd-port",
|
|
};
|
|
static const std::vector<std::string> imuPortSynonyms = {
|
|
"imu-port",
|
|
};
|
|
static const std::vector<std::string> smoIpSynonyms = {
|
|
"smo-ip",
|
|
};
|
|
|
|
DeviceAttachmentSpec::rejectUnknownParams(
|
|
providerParams,
|
|
std::string(__func__) + ": Unknown provider parameter: ",
|
|
cmdTimeoutSynonyms,
|
|
retryDelaySynonyms,
|
|
smoSubnetSynonyms,
|
|
dataPortSynonyms,
|
|
cmdPortSynonyms,
|
|
imuPortSynonyms,
|
|
smoIpSynonyms);
|
|
|
|
LivoxProviderParams params;
|
|
|
|
if (const auto matchedParam = DeviceAttachmentSpec::findOptionalParamWithSynonyms(
|
|
providerParams, cmdTimeoutSynonyms))
|
|
{
|
|
params.commandTimeoutMs = DeviceAttachmentSpec::parseParamValueAsInt(
|
|
matchedParam->first, matchedParam->second);
|
|
}
|
|
|
|
if (const auto matchedParam = DeviceAttachmentSpec::findOptionalParamWithSynonyms(
|
|
providerParams, retryDelaySynonyms))
|
|
{
|
|
params.retryDelayMs = DeviceAttachmentSpec::parseParamValueAsInt(
|
|
matchedParam->first, matchedParam->second);
|
|
}
|
|
|
|
if (const auto matchedParam = DeviceAttachmentSpec::findOptionalParamWithSynonyms(
|
|
providerParams, smoSubnetSynonyms))
|
|
{
|
|
params.smoSubnetNbits = static_cast<uint8_t>(
|
|
DeviceAttachmentSpec::parseParamValueAsInt(
|
|
matchedParam->first, matchedParam->second));
|
|
}
|
|
|
|
if (const auto matchedParam = DeviceAttachmentSpec::findOptionalParamWithSynonyms(
|
|
providerParams, dataPortSynonyms))
|
|
{
|
|
params.dataPort = static_cast<uint16_t>(
|
|
DeviceAttachmentSpec::parseParamValueAsInt(
|
|
matchedParam->first, matchedParam->second));
|
|
}
|
|
|
|
if (const auto matchedParam = DeviceAttachmentSpec::findOptionalParamWithSynonyms(
|
|
providerParams, cmdPortSynonyms))
|
|
{
|
|
params.cmdPort = static_cast<uint16_t>(
|
|
DeviceAttachmentSpec::parseParamValueAsInt(
|
|
matchedParam->first, matchedParam->second));
|
|
}
|
|
|
|
if (const auto matchedParam = DeviceAttachmentSpec::findOptionalParamWithSynonyms(
|
|
providerParams, imuPortSynonyms))
|
|
{
|
|
params.imuPort = static_cast<uint16_t>(
|
|
DeviceAttachmentSpec::parseParamValueAsInt(
|
|
matchedParam->first, matchedParam->second));
|
|
}
|
|
|
|
if (const auto matchedParam = DeviceAttachmentSpec::findOptionalParamWithSynonyms(
|
|
providerParams, smoIpSynonyms))
|
|
{
|
|
if (matchedParam->second.empty()) {
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": smo-ip parameter is empty");
|
|
}
|
|
|
|
if (matchedParam->second.find('.') == std::string::npos
|
|
|| std::count(
|
|
matchedParam->second.begin(),
|
|
matchedParam->second.end(),
|
|
'.') != 3)
|
|
{
|
|
throw std::runtime_error(
|
|
std::string(__func__)
|
|
+ ": smo-ip parameter is not an IPv4 address");
|
|
}
|
|
|
|
params.smoIp = matchedParam->second;
|
|
}
|
|
|
|
return params;
|
|
}
|
|
|
|
// Helper method to ensure StimBuffer is attached
|
|
// Returns true if successful, false on error
|
|
bool ensureStimBufferAttachedWithoutDuplicates(
|
|
const std::shared_ptr<PcloudStimulusProducer> &stimProducer,
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &spec)
|
|
{
|
|
if (!stimProducer) {
|
|
std::cerr << __func__ << ": stimProducer is null" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
// Check for duplicate qualeIfaceApi
|
|
const std::string &qualeIfaceApi = spec->qualeIfaceApi;
|
|
if (stimProducer->hasBufferWithQualeIfaceApi(qualeIfaceApi)) {
|
|
std::cerr << __func__ << ": Buffer with qualeIfaceApi '"
|
|
<< qualeIfaceApi << "' already exists for this producer. "
|
|
"Each producer can only have one buffer per qualeIfaceApi."
|
|
<< std::endl;
|
|
return false;
|
|
}
|
|
|
|
// Call getOrCreateAttachedStimulusBuffer (may throw, catch and return failure)
|
|
try {
|
|
stimProducer->getOrCreateAttachedStimulusBuffer(spec);
|
|
} 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 caller can handle error callback
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
namespace {
|
|
|
|
constexpr size_t MAX_STIM_PRODUCERS_PER_DEVICE = 2;
|
|
|
|
bool validateAttachRequest(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &desc)
|
|
{
|
|
// Validate qualeIfaceApi
|
|
const std::string &qualeIfaceApi = desc->qualeIfaceApi;
|
|
if (qualeIfaceApi == "gyro" || qualeIfaceApi == "accel")
|
|
{
|
|
// These are for ImuStimulusProducer (not yet implemented)
|
|
std::cerr << __func__ << ": qualeIfaceApi '" << qualeIfaceApi
|
|
<< "' requires ImuStimulusProducer which is not yet implemented"
|
|
<< std::endl;
|
|
return false;
|
|
}
|
|
|
|
try {
|
|
smo::intrin::validateNoIntrinParamsOnQualeIface(
|
|
desc->qualeIfaceApi,
|
|
desc->qualeIfaceApiParams);
|
|
} catch (const std::runtime_error &e) {
|
|
std::cerr << __func__ << ": " << e.what() << std::endl;
|
|
return false;
|
|
}
|
|
|
|
if (!PcloudStimulusProducer::supportsQualeIfaceApi(qualeIfaceApi))
|
|
{
|
|
// Unknown qualeIfaceApi
|
|
std::cerr << __func__ << ": Unsupported qualeIfaceApi '"
|
|
<< qualeIfaceApi << "' for LivoxGen1. "
|
|
"Supported values: mesh, pcloudIntensity, "
|
|
"pcloudLightAmbience, pcloudDarkAmbience"
|
|
<< std::endl;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
sscl::co::ViralNonPostingInvoker<StimBuffDeviceOpResult>
|
|
enablePcloudDataForAttach(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &desc,
|
|
const std::shared_ptr<sscl::ComponentThread> &/*componentThread*/,
|
|
const std::shared_ptr<livoxProto1::Device> &device)
|
|
{
|
|
/* Enable pcloud data. Don't need delay since no commands were
|
|
* sent to device prior to us reaching here (or delay already handled).
|
|
*/
|
|
const bool enabled = co_await (*livoxProto1.livoxProto1_device_enablePcloudDataCReq)(
|
|
device);
|
|
|
|
if (!enabled)
|
|
{
|
|
std::cerr << __func__ << ": Failed to enable pcloud data for dev "
|
|
<< desc->deviceSelector << std::endl;
|
|
|
|
co_return StimBuffDeviceOpResult{false, desc};
|
|
}
|
|
|
|
if (smoHooksPtr->OptionParser_getOptions().verbose)
|
|
{
|
|
std::cout << __func__ << ": Enabled pcloud data for device: "
|
|
<< desc->deviceSelector << std::endl;
|
|
}
|
|
|
|
co_return StimBuffDeviceOpResult{true, desc};
|
|
}
|
|
|
|
sscl::co::ViralNonPostingInvoker<StimBuffDeviceOpResult>
|
|
attachBufferAndEnablePcloud(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &desc,
|
|
const std::shared_ptr<sscl::ComponentThread> &componentThread,
|
|
const std::shared_ptr<PcloudStimulusProducer> &stimProducer)
|
|
{
|
|
// Ensure StimBuffer is attached
|
|
if (!ensureStimBufferAttachedWithoutDuplicates(stimProducer, desc)) {
|
|
co_return StimBuffDeviceOpResult{false, desc};
|
|
}
|
|
|
|
// Continue to enable pcloud data if needed
|
|
co_return co_await enablePcloudDataForAttach(
|
|
desc, componentThread, stimProducer->device);
|
|
}
|
|
|
|
sscl::co::ViralNonPostingInvoker<StimBuffDeviceOpResult>
|
|
attachToExistingProducer(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &desc,
|
|
const std::shared_ptr<sscl::ComponentThread> &componentThread,
|
|
const std::shared_ptr<PcloudStimulusProducer> &stimProducer)
|
|
{
|
|
// Case 1: Check if StimBuffer already exists
|
|
auto existingBuffer = stimProducer->getAttachedStimulusBuffer(desc);
|
|
if (existingBuffer)
|
|
{
|
|
// 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
|
|
co_return StimBuffDeviceOpResult{true, desc};
|
|
}
|
|
|
|
// StimBuffer exists but pcloud data is not active, enable it
|
|
co_return co_await enablePcloudDataForAttach(
|
|
desc, componentThread,
|
|
stimProducer->device);
|
|
}
|
|
|
|
/** EXPLANATION:
|
|
* StimProducer exists, StimBuffer doesn't (DASpec doesn't match)
|
|
* Check if producer already has a buffer with the requested
|
|
* qualeIfaceApi but different DASpec - this is not allowed.
|
|
*/
|
|
if (stimProducer->hasBufferWithQualeIfaceApi(desc->qualeIfaceApi))
|
|
{
|
|
std::cerr << __func__ << ": Producer already has a buffer with "
|
|
"qualeIfaceApi '" << desc->qualeIfaceApi
|
|
<< "' but with a different DeviceAttachmentSpec. "
|
|
"A single LivoxGen1 device cannot support multiple DASpecs "
|
|
"with the same qualeIfaceApi."
|
|
<< std::endl;
|
|
|
|
co_return StimBuffDeviceOpResult{false, desc};
|
|
}
|
|
|
|
// Ensure StimBuffer is attached and enable pcloud data if needed
|
|
co_return co_await attachBufferAndEnablePcloud(
|
|
desc, componentThread, stimProducer);
|
|
}
|
|
|
|
sscl::co::ViralNonPostingInvoker<StimBuffDeviceOpResult>
|
|
attachByCreatingProducer(
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &desc,
|
|
const std::shared_ptr<sscl::ComponentThread> &componentThread)
|
|
{
|
|
// 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
|
|
* the delay we insert between commands sent to the device. 5ms has been
|
|
* shown to be sufficient for the Livox Avia.
|
|
*/
|
|
|
|
/* The Livox Avia will generally respond to a handshake request within
|
|
* 5ms.
|
|
*/
|
|
/* Based on testing on a Livox Avia, the device will generally resume
|
|
* sending broadcast advertisement dgrams after about 5 seconds at most.
|
|
* Generally, it will resume sending them within 1-2 seconds.
|
|
*/
|
|
const LivoxProviderParams params = parseLivoxProviderParams(desc);
|
|
LivoxProto1GetOrCreateDeviceResult deviceResult =
|
|
co_await (*livoxProto1.livoxProto1_getOrCreateDeviceCReq)(
|
|
desc->deviceSelector,
|
|
componentThread,
|
|
params.commandTimeoutMs, params.retryDelayMs,
|
|
params.smoIp, params.smoSubnetNbits,
|
|
params.dataPort, params.cmdPort, params.imuPort);
|
|
|
|
if (!deviceResult.success || !deviceResult.device)
|
|
{
|
|
std::cerr << __func__ << ": Failed to create/find Livox device: "
|
|
<< desc->deviceSelector << std::endl;
|
|
|
|
co_return StimBuffDeviceOpResult{false, desc};
|
|
}
|
|
|
|
// Stash device pointer until after getReturnMode succeeds
|
|
if (smoHooksPtr->OptionParser_getOptions().verbose)
|
|
{
|
|
std::cout << __func__ << ": Successfully attached/found Livox "
|
|
"device: " << desc->deviceSelector << " (ID: "
|
|
<< desc->deviceIdentifier << ")\n";
|
|
}
|
|
|
|
/* Delay here because getOrCreate just sent HandshakeReq, so device
|
|
* may not yet be ready for another command.
|
|
*/
|
|
// Initialize timer with LivoxGen1 metadata io_context
|
|
boost::asio::deadline_timer commandDelayTimer(
|
|
componentThread->getIoContext());
|
|
const bool delayOk = co_await adapters::boostAsio::getDeadlineTimerAReqAwaiter(
|
|
componentThread->getIoContext(),
|
|
commandDelayTimer,
|
|
boost::posix_time::milliseconds(LIVOX_GEN1_DEVICE_COMMAND_DELAY_MS));
|
|
|
|
if (!delayOk)
|
|
{
|
|
std::cerr << __func__ << ": Delay timer failed before getReturnMode\n";
|
|
co_return StimBuffDeviceOpResult{false, desc};
|
|
}
|
|
|
|
LivoxProto1GetReturnModeResult returnModeResult =
|
|
co_await (*livoxProto1.livoxProto1_device_getReturnModeCReq)(
|
|
deviceResult.device);
|
|
|
|
if (!returnModeResult.success)
|
|
{
|
|
std::cerr << __func__ << ": Failed to get return mode for dev "
|
|
<< desc->deviceSelector << std::endl;
|
|
co_return StimBuffDeviceOpResult{false, desc};
|
|
}
|
|
|
|
if (smoHooksPtr->OptionParser_getOptions().verbose)
|
|
{
|
|
std::cout << __func__ << ": Got return mode ("
|
|
<< static_cast<int>(returnModeResult.returnMode)
|
|
<< ") for device: " << desc->deviceSelector << std::endl;
|
|
}
|
|
|
|
/* Check if PcloudStimulusProducer already exists
|
|
* (race condition or double-add)
|
|
*/
|
|
auto existingProducer = getStimulusProducer(desc);
|
|
if (existingProducer)
|
|
{
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": PcloudStimulusProducer already "
|
|
"exists for device " + desc->deviceSelector + " "
|
|
"(race condition or double-add)");
|
|
}
|
|
|
|
// Create & add PcloudStimulusProducer to collection since dev now ready
|
|
PcloudStimulusProducer::PcloudFormatDesc formatDesc;
|
|
formatDesc.format = PcloudStimulusProducer::PcloudFormatDesc::Format::XYZI;
|
|
|
|
// Parse n-dgrams-per-frame from stim-buff-api params (default: 84)
|
|
const size_t nDgramsPerFrame = parseNDgramsPerFrame(desc);
|
|
|
|
auto pcloudDataProducer = std::make_shared<PcloudStimulusProducer>(
|
|
desc, deviceResult.device,
|
|
formatDesc, nDgramsPerFrame);
|
|
|
|
deviceResult.device->nAttachedStimulusProducers++;
|
|
if (deviceResult.device->nAttachedStimulusProducers
|
|
> MAX_STIM_PRODUCERS_PER_DEVICE)
|
|
{
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": Each LivoxGen1 device can only have "
|
|
"at most two StimulusProducers attached to it. Found "
|
|
+ std::to_string(deviceResult.device->nAttachedStimulusProducers)
|
|
+ ".");
|
|
}
|
|
|
|
attachedStimulusProducers.push_back(pcloudDataProducer);
|
|
if (false
|
|
/*attachedStimulusProducers.size() >= 2*nDevicesKnownToGen1Lib */)
|
|
{
|
|
/** TODO:
|
|
* It would be nice to add an nDevicesKnownToGen1Lib counter, and
|
|
* then add a check here to ensure that
|
|
* attachedStimulusProducers.size() is always less than or equal to
|
|
* 2*nDevicesKnownToGen1Lib.
|
|
*
|
|
* (2 stim producers per device).
|
|
*/
|
|
#if 0
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": Number of StimulusProducers attached "
|
|
"to LivoxGen1 devices known to the library ("
|
|
+ std::to_string(attachedStimulusProducers.size())
|
|
+ ") is greater than "
|
|
"expected. Lib knows about "
|
|
+ std::to_string(nDevicesKnownToGen1Lib) + " devices, "
|
|
"so there should be at most "
|
|
+ std::to_string(2*nDevicesKnownToGen1Lib)
|
|
+ " StimulusProducers attached to devices.");
|
|
#endif
|
|
}
|
|
|
|
pcloudDataProducer->start();
|
|
|
|
// Ensure StimBuffer is attached
|
|
co_return co_await attachBufferAndEnablePcloud(
|
|
desc, componentThread, pcloudDataProducer);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
sscl::co::DynamicViralPostingInvoker<StimBuffDeviceOpResult>
|
|
livoxGen1_attachDeviceCReq(
|
|
[[maybe_unused]] sscl::co::ExplicitPostTarget postTarget,
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &desc,
|
|
const std::shared_ptr<sscl::ComponentThread> &componentThread)
|
|
{
|
|
(void)postTarget;
|
|
if (!livoxProto1.livoxProto1_getOrCreateDeviceCReq)
|
|
{
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": LivoxProto1 getOrCreateDevice function "
|
|
"not available");
|
|
}
|
|
|
|
if (!validateAttachRequest(desc)) {
|
|
co_return StimBuffDeviceOpResult{false, desc};
|
|
}
|
|
|
|
auto stimProducer = std::dynamic_pointer_cast<PcloudStimulusProducer>(
|
|
getStimulusProducer(desc));
|
|
|
|
if (stimProducer)
|
|
{
|
|
co_return co_await attachToExistingProducer(
|
|
desc,
|
|
componentThread,
|
|
stimProducer);
|
|
}
|
|
|
|
co_return co_await attachByCreatingProducer(desc, componentThread);
|
|
}
|
|
|
|
sscl::co::DynamicViralPostingInvoker<StimBuffDeviceOpResult>
|
|
livoxGen1_detachDeviceCReq(
|
|
[[maybe_unused]] sscl::co::ExplicitPostTarget postTarget,
|
|
const std::shared_ptr<device::DeviceAttachmentSpec> &desc)
|
|
{
|
|
(void)postTarget;
|
|
// Case 1: Check if StimBuffer doesn't exist (early return)
|
|
auto stimProducerBase = getStimulusProducer(desc);
|
|
if (!stimProducerBase) {
|
|
// StimProducer doesn't exist, nothing to detach - success
|
|
co_return StimBuffDeviceOpResult{true, desc};
|
|
}
|
|
|
|
auto stimProducer = std::dynamic_pointer_cast<PcloudStimulusProducer>(
|
|
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
|
|
co_return StimBuffDeviceOpResult{true, desc};
|
|
}
|
|
|
|
// Case 2: StimBuffer exists - proceed with detach
|
|
auto requestComponentThread = stimProducer->device->componentThread;
|
|
|
|
// Remove StimBuffer from collection if it exists
|
|
stimProducer->destroyAttachedStimulusBuffer(stimBuffer);
|
|
|
|
// Check if StimProducer has other buffers
|
|
if (!stimProducer->attachedStimulusBuffers.empty()) {
|
|
// Other buffers exist - just remove this buffer, done
|
|
co_return StimBuffDeviceOpResult{true, desc};
|
|
}
|
|
|
|
// Last buffer on producer: disable pcloud before tearing down device
|
|
// Disable point cloud data first
|
|
const bool disabled = co_await (*livoxProto1.livoxProto1_device_disablePcloudDataCReq)(
|
|
stimProducer->device);
|
|
|
|
if (!disabled)
|
|
{
|
|
std::cerr << __func__ << ": Failed to disable pcloud data for "
|
|
"stim producer " << desc->deviceSelector << std::endl;
|
|
// Fallthrough.
|
|
}
|
|
|
|
// Add 5ms delay before destroying device
|
|
|
|
// Helper method to delay and then call destroyDeviceReq
|
|
// Initialize timer with LivoxGen1 metadata io_context
|
|
boost::asio::deadline_timer commandDelayTimer(
|
|
requestComponentThread->getIoContext());
|
|
co_await adapters::boostAsio::getDeadlineTimerAReqAwaiter(
|
|
requestComponentThread->getIoContext(),
|
|
commandDelayTimer,
|
|
boost::posix_time::milliseconds(LIVOX_GEN1_DEVICE_COMMAND_DELAY_MS));
|
|
|
|
// 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 it = std::find_if(
|
|
attachedStimulusProducers.begin(),
|
|
attachedStimulusProducers.end(),
|
|
[&stimProducer](const std::shared_ptr<StimulusProducer> &producer)
|
|
{
|
|
/** FIXME:
|
|
* When we implement the ImuStimulusProducer, we need to make
|
|
* sure we handle that properly here.
|
|
*/
|
|
auto pcloudProducer =
|
|
std::dynamic_pointer_cast<PcloudStimulusProducer>(producer);
|
|
return pcloudProducer && pcloudProducer->device == stimProducer->device;
|
|
});
|
|
if (it != attachedStimulusProducers.end()) {
|
|
attachedStimulusProducers.erase(it);
|
|
}
|
|
|
|
const bool destroyed = co_await (*livoxProto1.livoxProto1_destroyDeviceCReq)(
|
|
stimProducer->device);
|
|
if (!destroyed) {
|
|
std::cerr << __func__ << ": Failed to destroy dev "
|
|
"device " << desc->deviceSelector << " for stim "
|
|
"producer.\n";
|
|
|
|
/** NOTE:
|
|
* There's a decent argument for falling through here and still
|
|
* removing the stimulus producer from attachedStimulusProducers.
|
|
*/
|
|
co_return StimBuffDeviceOpResult{false, desc};
|
|
}
|
|
|
|
if (smoHooksPtr->OptionParser_getOptions().verbose) {
|
|
std::cout << __func__ << ": Successfully detached pcloud stim "
|
|
"producer for device " << desc->deviceSelector
|
|
<< " and possibly also destroyed device.\n";
|
|
}
|
|
|
|
co_return StimBuffDeviceOpResult{true, desc};
|
|
}
|
|
|
|
sscl::co::ViralNonPostingInvoker<int> livoxGen1_initializeCInd()
|
|
{
|
|
if (!smoHooksPtr) {
|
|
throw std::runtime_error(std::string(__func__) + ": SMO hooks "
|
|
"pointers not filled in.");
|
|
}
|
|
|
|
// Load LivoxProto1 library
|
|
auto libPath = smoHooksPtr->searchForLibInSmoSearchPaths("liblivoxProto1.so");
|
|
livoxProto1.dlopenHandle.reset(dlopen(
|
|
libPath.value_or("liblivoxProto1.so").c_str(),
|
|
RTLD_LAZY));
|
|
if (!livoxProto1.dlopenHandle) {
|
|
throw std::runtime_error(
|
|
std::string(__func__) +
|
|
": Failed to load LivoxProto1 library: "
|
|
+ (dlerror() ? dlerror() : "unknown error"));
|
|
}
|
|
|
|
// Get LivoxProto1 library functions
|
|
livoxProto1.livoxProto1_main = reinterpret_cast<livoxProto1_mainFn *>(
|
|
dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_main"));
|
|
livoxProto1.livoxProto1_exit = reinterpret_cast<livoxProto1_exitFn *>(
|
|
dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_exit"));
|
|
livoxProto1.livoxProto1_getOrCreateDeviceCReq = reinterpret_cast<
|
|
livoxProto1_getOrCreateDeviceCReqFn *>(
|
|
dlsym(
|
|
livoxProto1.dlopenHandle.get(),
|
|
"livoxProto1_getOrCreateDeviceCReq"));
|
|
livoxProto1.livoxProto1_destroyDeviceCReq = reinterpret_cast<
|
|
livoxProto1_destroyDeviceCReqFn *>(
|
|
dlsym(
|
|
livoxProto1.dlopenHandle.get(),
|
|
"livoxProto1_destroyDeviceCReq"));
|
|
livoxProto1.livoxProto1_device_enablePcloudDataCReq = reinterpret_cast<
|
|
livoxProto1_device_enablePcloudDataCReqFn *>(
|
|
dlsym(
|
|
livoxProto1.dlopenHandle.get(),
|
|
"livoxProto1_device_enablePcloudDataCReq"));
|
|
livoxProto1.livoxProto1_device_disablePcloudDataCReq = reinterpret_cast<
|
|
livoxProto1_device_disablePcloudDataCReqFn *>(
|
|
dlsym(
|
|
livoxProto1.dlopenHandle.get(),
|
|
"livoxProto1_device_disablePcloudDataCReq"));
|
|
livoxProto1.livoxProto1_device_getReturnModeCReq = reinterpret_cast<
|
|
livoxProto1_device_getReturnModeCReqFn *>(
|
|
dlsym(
|
|
livoxProto1.dlopenHandle.get(),
|
|
"livoxProto1_device_getReturnModeCReq"));
|
|
livoxProto1.livoxProto1_getPcloudDataFdDesc = reinterpret_cast<
|
|
livoxProto1_getPcloudDataFdDescFn *>(
|
|
dlsym(
|
|
livoxProto1.dlopenHandle.get(),
|
|
"livoxProto1_getPcloudDataFdDesc"));
|
|
|
|
if (!livoxProto1.livoxProto1_main
|
|
|| !livoxProto1.livoxProto1_exit
|
|
|| !livoxProto1.livoxProto1_getOrCreateDeviceCReq
|
|
|| !livoxProto1.livoxProto1_destroyDeviceCReq
|
|
|| !livoxProto1.livoxProto1_device_enablePcloudDataCReq
|
|
|| !livoxProto1.livoxProto1_device_disablePcloudDataCReq
|
|
|| !livoxProto1.livoxProto1_device_getReturnModeCReq
|
|
|| !livoxProto1.livoxProto1_getPcloudDataFdDesc)
|
|
{
|
|
throw std::runtime_error(
|
|
std::string(__func__) + ": Failed to get LivoxProto1 library functions");
|
|
}
|
|
|
|
(*livoxProto1.livoxProto1_main)(
|
|
smoThreadingModelDesc.componentThread,
|
|
*smoHooksPtr);
|
|
co_return 0;
|
|
}
|
|
|
|
sscl::co::ViralNonPostingInvoker<int> livoxGen1_finalizeCInd()
|
|
{
|
|
attachedStimulusProducers.clear();
|
|
if (livoxProto1.livoxProto1_exit) {
|
|
(*livoxProto1.livoxProto1_exit)();
|
|
}
|
|
|
|
livoxProto1.dlopenHandle.reset(nullptr);
|
|
livoxProto1 = LivoxProto1DllState();
|
|
co_return 0;
|
|
}
|
|
|
|
static const StimBuffApiDesc livoxGen1ApiDesc = {
|
|
.name = "livoxGen1",
|
|
.exportedQualeIfaceApis = {
|
|
{.name = "mesh"},
|
|
{.name = "pcloudIntensity"},
|
|
{.name = "pcloudLightAmbience"},
|
|
{.name = "pcloudDarkAmbience"},
|
|
{.name = "gyro"},
|
|
{.name = "accel"}
|
|
},
|
|
.sal_mgmt_libOps = {
|
|
.initializeCInd = livoxGen1_initializeCInd,
|
|
.finalizeCInd = livoxGen1_finalizeCInd,
|
|
.attachDeviceCReq = livoxGen1_attachDeviceCReq,
|
|
.detachDeviceCReq = livoxGen1_detachDeviceCReq
|
|
}
|
|
};
|
|
|
|
extern "C" SMO_GET_STIM_BUFF_API_DESC_FN_TYPEDEF
|
|
SMO_GET_STIM_BUFF_API_DESC_FN_NAME;
|
|
|
|
const StimBuffApiDesc& SMO_GET_STIM_BUFF_API_DESC_FN_NAME(
|
|
const SmoCallbacks& callbacks,
|
|
const SmoThreadingModelDesc& threadingModel)
|
|
{
|
|
smoHooksPtr = &callbacks;
|
|
smoThreadingModelDesc = threadingModel;
|
|
return livoxGen1ApiDesc;
|
|
}
|
|
|
|
} // namespace smo::stim_buff
|