hayodea/salmanoff
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 2 Wiki Activity

LivoxGen1: Add basic stimbuff creation & destruction

Browse Source
This commit is contained in:
Hayodea Hekol
2025-10-25 23:04:59 -04:00
parent 10e19a3237
commit 2a8a6edf22
2 changed files with 129 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
stimBuffApis/livoxGen1/livoxGen1.cpp
+122 -99
View File
@@ -15,6 +15,7 @@
#include <livoxProto1/protocol.h>
#include <asynchronousContinuation.h>
#include <boost/asio/deadline_timer.hpp>
#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<std::shared_ptr<StimulusBuffer>> attachedStimBuffs;
// Get stimulus buffer by device attachment spec
static std::shared_ptr<StimulusBuffer>
getStimBuff(const std::shared_ptr<smo::device::DeviceAttachmentSpec>& 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<std::shared_ptr<livoxProto1::Device>> g_attachedDevices;
// Utility function to find a device in g_attachedDevices by identifier
static std::shared_ptr<livoxProto1::Device> getDevice(
const std::string& deviceIdentifier
)
{
auto it = std::find_if(g_attachedDevices.begin(), g_attachedDevices.end(),
[&deviceIdentifier](const std::shared_ptr<livoxProto1::Device>& 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<smo::device::DeviceAttachmentSpec> spec;
std::shared_ptr<livoxProto1::Device> device;
std::shared_ptr<PcloudStimulusBuffer> stimBuff;
private:
std::unique_ptr<boost::asio::deadline_timer> 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<PcloudStimulusBuffer>(
*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<boost::asio::deadline_timer>(
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<smo::device::DeviceAttachmentSpec>& spec,
const std::shared_ptr<livoxProto1::Device>& device,
const std::shared_ptr<PcloudStimulusBuffer>& stimBuff,
smo::Callback<sal_mlo_detachDeviceReqCbFn> cb)
: smo::NonPostedAsynchronousContinuation<sal_mlo_detachDeviceReqCbFn>(
std::move(cb)),
spec(spec), device(device)
spec(spec), stimBuff(stimBuff)
{}
public:
const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
std::shared_ptr<livoxProto1::Device> device;
std::shared_ptr<PcloudStimulusBuffer> stimBuff;
private:
std::unique_ptr<boost::asio::deadline_timer> 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<DetachDeviceReq> 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<DetachDeviceReq> context)
{
// Initialize timer with device's component thread
delayTimer = std::make_unique<boost::asio::deadline_timer>(
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<DetachDeviceReq> 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<DetachDeviceReq> 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<AttachDeviceReq>(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<PcloudStimulusBuffer>(
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<smo::stim_buff::sal_mlo_detachDeviceReqCbFn> 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<PcloudStimulusBuffer>(
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<DetachDeviceReq>(desc, device, cb);
auto request = std::make_shared<DetachDeviceReq>(
desc, stimBuff, cb);
// Disable point cloud data first
(*livoxProto1.livoxProto1_device_disablePcloudDataReq)(
device,
stimBuff->device,
{request, std::bind(
&DetachDeviceReq::detachDeviceReq1,
request.get(), request,
stimBuffApis/livoxGen1/pcloudStimulusBuffer.h
+7 -2
View File
@@ -3,6 +3,7 @@
#include <user/stimulusBuffer.h>
#include <user/stimFrame.h>
#include <livoxProto1/device.h>
namespace smo {
namespace stim_buff {
@@ -21,12 +22,14 @@ class PcloudStimulusBuffer
public:
explicit PcloudStimulusBuffer(
const device::DeviceAttachmentSpec& deviceAttachmentSpec,
std::shared_ptr<livoxProto1::Device> &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<livoxProto1::Device> device;
PcloudFormatDesc formatDesc;
};