salmanoff/senseApis/livoxGen1/livoxGen1.cpp

#include <iostream>
#include <memory>
#include <vector>
#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 {

// Salmanoff hooks, obtained from SMO_GET_SENSE_API_DESC_FN_NAME().
static const SmoCallbacks* smoHooksPtr = nullptr;
static SmoThreadingModelDesc smoThreadingModelDesc;

// LivoxProto1 library state
struct LivoxProto1DllState
{
	LivoxProto1DllState()
	: dlopenHandle(nullptr, DlCloser),
	  livoxProto1_main(nullptr),
	  livoxProto1_exit(nullptr),
	  livoxProto1_getOrCreateDevice(nullptr)
	{}

	static void DlCloser(void* handle)
	{
		if (handle) {
			dlclose(handle);
		}
	}

	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);
extern "C" int livoxGen1_attachDeviceReq(
	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc);
extern "C" int livoxGen1_detachDeviceReq(
	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc);

// Sense API descriptor
static const SenseApiDesc livoxGen1ApiDesc = {
	.name = "livoxGen1",
	.exportedImplexorApis = {
		{.name = "pointCloudCoords"},
		{.name = "pointCloudIntensity"},
		{.name = "gyro"},
		{.name = "accel"}
	},
	.sal_mgmt_libOps = {
		.initializeInd = livoxGen1_initializeInd,
		.finalizeInd = livoxGen1_finalizeInd,
		.attachDeviceReq = livoxGen1_attachDeviceReq,
		.detachDeviceReq = livoxGen1_detachDeviceReq
	}
};

// Callback function implementations
extern "C" int livoxGen1_initializeInd(void)
{
	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_getOrCreateDevice = reinterpret_cast<
		livoxProto1_getOrCreateDeviceFn *>(
			dlsym(
				livoxProto1.dlopenHandle.get(),
				"livoxProto1_getOrCreateDevice"));

	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);

	return 0; // Success
}

extern "C" int livoxGen1_finalizeInd(void)
{
	// Clear all attached devices
	g_attachedDevices.clear();

	// Call LivoxProto1 library exit function
	if (livoxProto1.livoxProto1_exit) {
		(*livoxProto1.livoxProto1_exit)();
	}

	if (livoxProto1.dlopenHandle)
	{
		dlclose(livoxProto1.dlopenHandle.get());
		livoxProto1.dlopenHandle.reset();
	}

	livoxProto1 = LivoxProto1DllState();
	return 0; // Success
}

extern "C" int livoxGen1_attachDeviceReq(
	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
	)
{
	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")
		{
			if (param.second.empty())
			{
				throw std::runtime_error(
					std::string(__func__) + ": smo-ip parameter is empty");
			}
			if (param.second.find('.') == std::string::npos)
			{
				throw std::runtime_error(
					std::string(__func__) + ": smo-ip parameter is not an IPv4 address");
			}
			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
}

extern "C" int livoxGen1_detachDeviceReq(
	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
	)
{
	// 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
}

// Exported function
extern "C" smo::sense_api::SMO_GET_SENSE_API_DESC_FN_TYPEDEF
	SMO_GET_SENSE_API_DESC_FN_NAME;

const smo::sense_api::SenseApiDesc& SMO_GET_SENSE_API_DESC_FN_NAME(
	const smo::sense_api::SmoCallbacks& callbacks,
	const smo::sense_api::SmoThreadingModelDesc& threadingModel)
{
	smoHooksPtr = &callbacks;
	smoThreadingModelDesc = threadingModel;

	return livoxGen1ApiDesc;
}

} // namespace sense_api
} // namespace smo