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

LivoxGen/Proto1: Move en/disablePcloudData call to Gen1

Browse Source 
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.
This commit is contained in:
Hayodea Hekol
2025-10-25 12:53:07 -04:00
parent 0e872042ee
commit 6f4a2dd649
4 changed files with 254 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
commonLibs/livoxProto1/device.cpp
+15 -45
View File
@@ -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<ConnectReq> 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<ConnectReq> 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<ConnectReq> 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<ConnectReq> context,
bool success
std::shared_ptr<ConnectReq> 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;
}
commonLibs/livoxProto1/livoxProto1.cpp
+28
View File
@@ -62,4 +62,32 @@ void livoxProto1_exit(void)
livoxProto1::exit();
}
void livoxProto1_device_enablePcloudDataReq(
std::shared_ptr<livoxProto1::Device> device,
smo::Callback<livoxProto1_device_enablePcloudDataReqCbFn> callback
)
{
if (!device)
{
throw std::runtime_error(std::string(__func__)
+ ": Device pointer is null");
}
device->enablePcloudDataReq(callback);
}
void livoxProto1_device_disablePcloudDataReq(
std::shared_ptr<livoxProto1::Device> device,
smo::Callback<livoxProto1_device_disablePcloudDataReqCbFn> callback
)
{
if (!device)
{
throw std::runtime_error(std::string(__func__)
+ ": Device pointer is null");
}
device->disablePcloudDataReq(callback);
}
} // extern "C"
commonLibs/livoxProto1/livoxProto1.h
+16 -1
View File
@@ -67,14 +67,29 @@ typedef void livoxProto1_destroyDeviceReqFn(
std::shared_ptr<livoxProto1::Device> device,
smo::Callback<livoxProto1_destroyDeviceReqCbFn> callback);
typedef std::function<void(bool success)>
livoxProto1_device_enablePcloudDataReqCbFn;
typedef void livoxProto1_device_enablePcloudDataReqFn(
std::shared_ptr<livoxProto1::Device> device,
smo::Callback<livoxProto1_device_enablePcloudDataReqCbFn> callback);
typedef std::function<void(bool success)>
livoxProto1_device_disablePcloudDataReqCbFn;
typedef void livoxProto1_device_disablePcloudDataReqFn(
std::shared_ptr<livoxProto1::Device> device,
smo::Callback<livoxProto1_device_disablePcloudDataReqCbFn> 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
#endif // LIVOXPROTO1_H
stimBuffApis/livoxGen1/livoxGen1.cpp
+195 -49
View File
@@ -12,7 +12,9 @@
#include <callback.h>
#include <livoxProto1/livoxProto1.h>
#include <livoxProto1/device.h>
#include <livoxProto1/protocol.h>
#include <asynchronousContinuation.h>
#include <boost/asio/deadline_timer.hpp>
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<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
: public smo::NonPostedAsynchronousContinuation<sal_mlo_attachDeviceReqCbFn>
@@ -67,13 +88,16 @@ public:
public:
const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
std::shared_ptr<livoxProto1::Device> device;
private:
std::unique_ptr<boost::asio::deadline_timer> delayTimer;
public:
void attachDeviceReq1(
std::shared_ptr<AttachDeviceReq> context,
bool success, std::shared_ptr<livoxProto1::Device> 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<AttachDeviceReq> 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<AttachDeviceReq> context)
{
// Initialize timer with device's component thread
delayTimer = std::make_unique<boost::asio::deadline_timer>(
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<AttachDeviceReq> 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<smo::device::DeviceAttachmentSpec>& spec,
const std::shared_ptr<livoxProto1::Device>& device,
smo::Callback<sal_mlo_detachDeviceReqCbFn> cb)
: smo::NonPostedAsynchronousContinuation<sal_mlo_detachDeviceReqCbFn>(
std::move(cb)),
spec(spec)
spec(spec), device(device)
{}
public:
const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
std::shared_ptr<livoxProto1::Device> device;
private:
std::unique_ptr<boost::asio::deadline_timer> delayTimer;
public:
void detachDeviceReq1(
std::shared_ptr<DetachDeviceReq> 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<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 eraseIt = std::find_if(
g_attachedDevices.begin(), g_attachedDevices.end(),
[context](const std::shared_ptr<livoxProto1::Device>& dev)
{
const std::string& devId = dev->discoveredDevice.deviceIdentifier;
std::string devIdPrefix = devId.substr(
0, std::min<size_t>(14, devId.size()));
return devIdPrefix == context->spec->deviceSelector.substr(
0, std::min<size_t>(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<DetachDeviceReq> context)
{
// Initialize timer with device's component thread
delayTimer = std::make_unique<boost::asio::deadline_timer>(
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<DetachDeviceReq> 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<AttachDeviceReq>(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<AttachDeviceReq>(desc, cb);
(*livoxProto1.livoxProto1_getOrCreateDeviceReq)(
desc->deviceSelector, // deviceIdentifier (broadcast code)
componentThread,
@@ -363,23 +522,9 @@ extern "C" void livoxGen1_detachDeviceReq(
Callback<smo::stim_buff::sal_mlo_detachDeviceReqCbFn> 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<livoxProto1::Device>& 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<size_t>(14, devId.size()));
return devIdPrefix == desc->deviceSelector.substr(
0, std::min<size_t>(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<DetachDeviceReq>(desc, cb);
auto request = std::make_shared<DetachDeviceReq>(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,