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

livoxGen1: Implement attachDevice/detachDevice

Browse Source 
Preliminary draft implementation. Unsurprisingly it doesn't
work. The handshake doesn't get accepted by the device it seems?
And we can't receive bcast dgrams since we're blocking
synchronously -- therefore we're not processing the incoming UDP
dgrams.
This commit is contained in:
Hayodea Hekol
2025-09-06 21:38:27 -04:00
parent 7f367fd6e3
commit 5d55157ffd
1 changed files with 126 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
senseApis/livoxGen1/livoxGen1.cpp
+126 -9
View File
@@ -4,10 +4,12 @@
#include <string>
#include <map>
#include <functional>
#include <algorithm>
#include <dlfcn.h>
#include <user/senseApiDesc.h>
#include <user/deviceAttachmentSpec.h>
#include <livoxProto1/livoxProto1.h>
#include <livoxProto1/device.h>
namespace smo {
namespace sense_api {
@@ -22,7 +24,8 @@ struct LivoxProto1DllState
LivoxProto1DllState()
: dlopenHandle(nullptr, DlCloser),
livoxProto1_main(nullptr),
livoxProto1_exit(nullptr)
livoxProto1_exit(nullptr),
livoxProto1_getOrCreateDevice(nullptr)
{}
static void DlCloser(void* handle)
@@ -35,10 +38,14 @@ struct LivoxProto1DllState
std::unique_ptr<void, void(*)(void*)> dlopenHandle;
livoxProto1_mainFn *livoxProto1_main;
livoxProto1_exitFn *livoxProto1_exit;
livoxProto1_getOrCreateDeviceFn *livoxProto1_getOrCreateDevice;
};
static LivoxProto1DllState livoxProto1;
// Attached Livox devices
static std::vector<std::shared_ptr<livoxProto1::Device>> g_attachedDevices;
// Callback function declarations
extern "C" int livoxGen1_initializeInd(void);
extern "C" int livoxGen1_finalizeInd(void);
@@ -93,24 +100,34 @@ extern "C" int livoxGen1_initializeInd(void)
dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_main"));
livoxProto1.livoxProto1_exit = reinterpret_cast<livoxProto1_exitFn *>(
dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_exit"));
livoxProto1.livoxProto1_getOrCreateDevice = reinterpret_cast<
livoxProto1_getOrCreateDeviceFn *>(
dlsym(
livoxProto1.dlopenHandle.get(),
"livoxProto1_getOrCreateDevice"));
if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit) {
if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit
|| !livoxProto1.livoxProto1_getOrCreateDevice)
{
throw std::runtime_error(
std::string(__func__) +
": Failed to get LivoxProto1 library functions");
}
// Call LivoxProto1 library main function
livoxProto1.livoxProto1_main(smoThreadingModelDesc.componentThread);
(*livoxProto1.livoxProto1_main)(smoThreadingModelDesc.componentThread);
return 0; // Success
}
extern "C" int livoxGen1_finalizeInd(void)
{
// TODO: Implement finalization logic
// Clear all attached devices
g_attachedDevices.clear();
// Call LivoxProto1 library exit function
if (livoxProto1.livoxProto1_exit) {
livoxProto1.livoxProto1_exit();
(*livoxProto1.livoxProto1_exit)();
}
if (livoxProto1.dlopenHandle)
@@ -127,8 +144,84 @@ extern "C" int livoxGen1_attachDeviceReq(
const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
)
{
// TODO: Implement device attachment logic
(void)desc; // Suppress unused parameter warning
if (!livoxProto1.livoxProto1_getOrCreateDevice)
{
throw std::runtime_error(
std::string(__func__) + ": LivoxProto1 getOrCreateDevice function "
"not available");
}
// Parse integer parameters from provider params with defaults
int handshakeTimeoutMs = 250; // Default: 50ms
int retryDelayMs = 500; // Default: 500ms
uint8_t smoSubnetNbits = 24; // Default: /24 subnet
uint16_t dataPort = 56000; // Default data port
uint16_t cmdPort = 56001; // Default command port
uint16_t imuPort = 56002; // Default IMU port
// Default: 10.42.0.1
std::string smoIp = "10.42.0.1";
// Parse optional integer parameters from provider params
for (const auto& param : desc->providerParams)
{
if (param.first == "handshake-timeout-ms")
{
handshakeTimeoutMs = smo::device::DeviceAttachmentSpec
::parseRequiredParamAsInt(*desc, "handshake-timeout-ms");
} else if (param.first == "retry-delay-ms")
{
retryDelayMs = smo::device::DeviceAttachmentSpec
::parseRequiredParamAsInt(*desc, "retry-delay-ms");
} else if (param.first == "smo-subnet-nbits")
{
smoSubnetNbits = static_cast<uint8_t>(
smo::device::DeviceAttachmentSpec
::parseRequiredParamAsInt(*desc, "smo-subnet-nbits"));
} else if (param.first == "data-port")
{
dataPort = static_cast<uint16_t>(
smo::device::DeviceAttachmentSpec
::parseRequiredParamAsInt(*desc, "data-port"));
} else if (param.first == "cmd-port")
{
cmdPort = static_cast<uint16_t>(
smo::device::DeviceAttachmentSpec
::parseRequiredParamAsInt(*desc, "cmd-port"));
} else if (param.first == "imu-port")
{
imuPort = static_cast<uint16_t>(
smo::device::DeviceAttachmentSpec
::parseRequiredParamAsInt(*desc, "imu-port"));
} else if (param.first == "smo-ip") {
smoIp = param.second;
}
else
{
throw std::runtime_error(
std::string(__func__) + ": Unknown provider parameter: "
+ param.first);
}
}
// Call getOrCreateDevice with parsed parameters
auto device = (*livoxProto1.livoxProto1_getOrCreateDevice)(
desc->deviceSelector, // deviceIdentifier (broadcast code)
smoThreadingModelDesc.componentThread,
handshakeTimeoutMs, retryDelayMs,
smoIp, smoSubnetNbits,
dataPort, cmdPort, imuPort);
if (!device)
{
throw std::runtime_error(
std::string(__func__) + ": Failed to create Livox device: "
+ desc->deviceSelector);
}
g_attachedDevices.push_back(device);
std::cout << __func__ << ": Successfully attached Livox device: "
<< desc->deviceSelector << " (ID: " << desc->deviceIdentifier << ")\n";
return 0; // Success
}
@@ -136,8 +229,32 @@ extern "C" int livoxGen1_detachDeviceReq(
const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
)
{
// TODO: Implement device detachment logic
(void)desc; // Suppress unused parameter warning
// 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())
{
std::cerr << __func__ << ": Device not found for detachment: "
<< desc->deviceIdentifier << "\n";
return -1; // Device not found
}
g_attachedDevices.erase(it);
std::cout << __func__ << ": Successfully detached Livox device: "
<< desc->deviceIdentifier << "\n";
return 0; // Success
}