Livox: Add new loadable libs for Protocolv1 and gen1 lidars
We've added two new libs: * commonLibs/livoxProto1 * senseApis/livoxGen1 They currently get to the point of detecting my Livox Avia on the network over UDP. This was really easy to get done in one night using boost::asio and Cursor, honestly.
This commit is contained in:
@@ -1 +1,2 @@
|
||||
add_subdirectory(xcbXorg)
|
||||
add_subdirectory(livoxProto1)
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
option(ENABLE_LIB_livoxProto1 "Enable Livox Protocol v1 backend lib" OFF)
|
||||
|
||||
if(ENABLE_LIB_livoxProto1)
|
||||
add_library(livoxProto1 SHARED
|
||||
livoxProto1.cpp
|
||||
livoxProto1Core.cpp
|
||||
livoxProto1Device.cpp
|
||||
livoxProto1Protocol.cpp
|
||||
)
|
||||
|
||||
# Set config define for header generation
|
||||
add_compile_definitions(CONFIG_LIB_LIVOXPROTO1_ENABLED)
|
||||
target_include_directories(livoxProto1 PUBLIC ${Boost_INCLUDE_DIRS})
|
||||
target_link_libraries(livoxProto1 ${Boost_LIBRARIES})
|
||||
|
||||
# Install rules
|
||||
install(TARGETS livoxProto1 DESTINATION lib)
|
||||
endif()
|
||||
@@ -0,0 +1,21 @@
|
||||
#include <user/senseApiDesc.h>
|
||||
#include "livoxProto1.h"
|
||||
#include "livoxProto1Core.h"
|
||||
|
||||
extern "C" {
|
||||
|
||||
livoxProto1_mainFn livoxProto1_main;
|
||||
livoxProto1_exitFn livoxProto1_exit;
|
||||
|
||||
void livoxProto1_main(
|
||||
const std::shared_ptr<smo::ComponentThread> &componentThread)
|
||||
{
|
||||
livoxProto1::main(componentThread);
|
||||
}
|
||||
|
||||
void livoxProto1_exit(void)
|
||||
{
|
||||
livoxProto1::exit();
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
@@ -0,0 +1,25 @@
|
||||
#ifndef LIVOX_PROTO1_API_H
|
||||
#define LIVOX_PROTO1_API_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include "livoxProto1Core.h"
|
||||
#include "livoxProto1Device.h"
|
||||
|
||||
namespace livoxProto1 {
|
||||
|
||||
} // namespace livoxProto1
|
||||
|
||||
extern "C" {
|
||||
typedef void (livoxProto1_mainFn)(
|
||||
const std::shared_ptr<smo::ComponentThread> &componentThread);
|
||||
typedef void (livoxProto1_exitFn)(void);
|
||||
|
||||
void livoxProto1_main(
|
||||
const std::shared_ptr<smo::ComponentThread> &componentThread);
|
||||
void livoxProto1_exit(void);
|
||||
} // extern "C"
|
||||
|
||||
#endif // LIVOX_PROTO1_API_H
|
||||
@@ -0,0 +1,67 @@
|
||||
#include <user/senseApiDesc.h>
|
||||
#include "livoxProto1Protocol.h"
|
||||
#include "livoxProto1Core.h"
|
||||
|
||||
namespace livoxProto1 {
|
||||
|
||||
struct ProtoState
|
||||
{
|
||||
bool isInitialized = false;
|
||||
std::shared_ptr<smo::ComponentThread> componentThread;
|
||||
std::unique_ptr<DeviceManager> deviceManager;
|
||||
};
|
||||
|
||||
static ProtoState protoState =
|
||||
{
|
||||
.isInitialized = false,
|
||||
.componentThread = nullptr,
|
||||
.deviceManager = nullptr
|
||||
};
|
||||
|
||||
DeviceManager::DeviceManager()
|
||||
: broadcastListener(protoState.componentThread)
|
||||
{
|
||||
broadcastListener.setDeviceGoneAwayCb(deviceGoneAwayInd);
|
||||
}
|
||||
|
||||
void DeviceManager::deviceGoneAwayInd(const comms::DiscoveredDevice &device)
|
||||
{
|
||||
std::cout << "Device gone away: " << device.stringify() << std::endl;
|
||||
auto it = std::find_if(
|
||||
protoState.deviceManager->devices.begin(),
|
||||
protoState.deviceManager->devices.end(),
|
||||
[&device](const Device &d) {
|
||||
return d.discoveredDevice == device;
|
||||
}
|
||||
);
|
||||
|
||||
if (it != protoState.deviceManager->devices.end()) {
|
||||
protoState.deviceManager->devices.erase(it);
|
||||
}
|
||||
}
|
||||
|
||||
void main(const std::shared_ptr<smo::ComponentThread> &componentThread)
|
||||
{
|
||||
if (protoState.isInitialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
protoState.isInitialized = true;
|
||||
protoState.componentThread = componentThread;
|
||||
protoState.deviceManager = std::make_unique<DeviceManager>();
|
||||
protoState.deviceManager->broadcastListener.start();
|
||||
}
|
||||
|
||||
void exit(void)
|
||||
{
|
||||
if (!protoState.isInitialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
protoState.deviceManager->broadcastListener.stop();
|
||||
protoState.deviceManager.reset();
|
||||
protoState.isInitialized = false;
|
||||
protoState.componentThread = nullptr;
|
||||
}
|
||||
|
||||
} // namespace livoxProto1
|
||||
@@ -0,0 +1,39 @@
|
||||
#ifndef LIVOXPROTO1_CORE_H
|
||||
#define LIVOXPROTO1_CORE_H
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include "livoxProto1Protocol.h"
|
||||
|
||||
namespace livoxProto1 {
|
||||
|
||||
class Device
|
||||
{
|
||||
public:
|
||||
Device(const comms::DiscoveredDevice &discoveredDevice);
|
||||
~Device() = default;
|
||||
|
||||
public:
|
||||
comms::DiscoveredDevice discoveredDevice;
|
||||
};
|
||||
|
||||
class DeviceManager
|
||||
{
|
||||
public:
|
||||
DeviceManager();
|
||||
~DeviceManager() = default;
|
||||
|
||||
static void deviceGoneAwayInd(const comms::DiscoveredDevice &device);
|
||||
|
||||
public:
|
||||
std::vector<Device> devices;
|
||||
comms::BroadcastListener broadcastListener;
|
||||
};
|
||||
|
||||
void main(const std::shared_ptr<smo::ComponentThread> &componentThread);
|
||||
void exit(void);
|
||||
|
||||
} // namespace livoxProto1
|
||||
|
||||
#endif // LIVOXPROTO1_CORE_H
|
||||
@@ -0,0 +1,243 @@
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <cstring>
|
||||
#include "livoxProto1Protocol.h"
|
||||
|
||||
namespace livoxProto1 {
|
||||
namespace comms {
|
||||
|
||||
// Header methods
|
||||
void Header::swapToHostEndianness()
|
||||
{
|
||||
if (endian::isLittleEndian()) { return; }
|
||||
length = __builtin_bswap16(length);
|
||||
seq_num = __builtin_bswap16(seq_num);
|
||||
crc_16 = __builtin_bswap16(crc_16);
|
||||
}
|
||||
|
||||
bool Header::sanityCheck() const
|
||||
{
|
||||
return (sof == 0xAA) && (version == 1);
|
||||
}
|
||||
|
||||
// Footer methods
|
||||
void Footer::swapToHostEndianness()
|
||||
{
|
||||
if (endian::isLittleEndian()) { return; }
|
||||
crc_32 = __builtin_bswap32(crc_32);
|
||||
}
|
||||
|
||||
bool Footer::sanityCheck() const
|
||||
{
|
||||
/** FIXME:
|
||||
* Add CRC validation here.
|
||||
*/
|
||||
return true;
|
||||
}
|
||||
|
||||
// BroadcastMessage methods
|
||||
void BroadcastMessage::swapToHostEndianness()
|
||||
{
|
||||
if (endian::isLittleEndian()) { return; }
|
||||
header.swapToHostEndianness();
|
||||
reserved = __builtin_bswap16(reserved);
|
||||
footer.swapToHostEndianness();
|
||||
}
|
||||
|
||||
bool BroadcastMessage::sanityCheck() const
|
||||
{
|
||||
return header.sanityCheck() &&
|
||||
(cmd_set == 0x00) &&
|
||||
(cmd_id == 0x00) &&
|
||||
(header.cmd_type == 0x02) &&
|
||||
footer.sanityCheck();
|
||||
}
|
||||
|
||||
// DiscoveredDevice constructors
|
||||
DiscoveredDevice::DiscoveredDevice(
|
||||
const std::string &deviceIdentifier,
|
||||
DeviceType deviceType,
|
||||
const std::string &ipAddr)
|
||||
: deviceIdentifier(deviceIdentifier),
|
||||
deviceType(deviceType),
|
||||
ipAddr(ipAddr)
|
||||
{
|
||||
}
|
||||
|
||||
DiscoveredDevice::DiscoveredDevice(
|
||||
const BroadcastMessage &msg, const std::string &ipAddr
|
||||
)
|
||||
: DiscoveredDevice(
|
||||
reinterpret_cast<const char*>(msg.broadcast_code),
|
||||
static_cast<DeviceType>(msg.dev_type),
|
||||
ipAddr)
|
||||
{
|
||||
}
|
||||
|
||||
std::string DiscoveredDevice::stringify(void) const
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << "DiscoveredDevice{"
|
||||
<< "identifier='" << deviceIdentifier << "', "
|
||||
<< "ipAddr='" << ipAddr << "', "
|
||||
<< "deviceType=" << (int)deviceType << " (" << getDeviceTypeName() << ")"
|
||||
<< "}";
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
std::string DiscoveredDevice::getDeviceTypeName(void) const
|
||||
{
|
||||
switch (deviceType)
|
||||
{
|
||||
case DeviceType::Hub: return "Hub";
|
||||
case DeviceType::Mid40: return "Mid-40";
|
||||
case DeviceType::Tele15: return "Tele-15";
|
||||
case DeviceType::Horizon: return "Horizon";
|
||||
case DeviceType::Mid70: return "Mid-70";
|
||||
case DeviceType::Avia: return "Avia";
|
||||
default: return "Unknown";
|
||||
}
|
||||
}
|
||||
|
||||
BroadcastListener::BroadcastListener(
|
||||
const std::shared_ptr<smo::ComponentThread>& componentThread,
|
||||
uint16_t listeningPort, uint16_t connectPort
|
||||
)
|
||||
: componentThread(componentThread),
|
||||
listeningPort(listeningPort),
|
||||
connectPort(connectPort),
|
||||
deviceGoneAwayCb(nullptr),
|
||||
socket(componentThread->getIoService()),
|
||||
listeningEndpoint(boost::asio::ip::udp::v4(), listeningPort),
|
||||
isListening(false)
|
||||
{
|
||||
}
|
||||
|
||||
std::shared_ptr<DiscoveredDevice>
|
||||
BroadcastListener::getDevice(const std::string &deviceIdentifier) const
|
||||
{
|
||||
auto it = std::find_if(discoveredDevices.begin(), discoveredDevices.end(),
|
||||
[&deviceIdentifier](const std::shared_ptr<DiscoveredDevice>& device) {
|
||||
return device->deviceIdentifier == deviceIdentifier;
|
||||
}
|
||||
);
|
||||
|
||||
return it != discoveredDevices.end() ? *it : nullptr;
|
||||
}
|
||||
|
||||
void BroadcastListener::broadcastMsgInd(
|
||||
const boost::system::error_code& ec, std::size_t bytes_received)
|
||||
{
|
||||
if (ec)
|
||||
{
|
||||
std::cerr << __func__ << ": Error receiving broadcast message: "
|
||||
<< ec.message() << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if (bytes_received < sizeof(BroadcastMessage))
|
||||
{
|
||||
std::cerr << "Received packet too small: " << bytes_received
|
||||
<< " bytes (expected at least " << sizeof(BroadcastMessage) << ")"
|
||||
<< std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// Use placement new to construct BroadcastMessage in the buffer
|
||||
BroadcastMessage* msg = new (bcastMsgRecvBuffer) BroadcastMessage;
|
||||
|
||||
if (!msg->sanityCheck())
|
||||
{
|
||||
std::cerr << "Broadcast message failed sanity check" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// Convert from little-endian to host endianness
|
||||
msg->swapToHostEndianness();
|
||||
|
||||
// Extract device information
|
||||
std::string senderIP = senderEndpoint.address().to_string();
|
||||
std::string broadcastCode(reinterpret_cast<const char*>(msg->broadcast_code));
|
||||
|
||||
// Early return if device already exists
|
||||
if (deviceExists(broadcastCode)) { return; }
|
||||
|
||||
// Create new DiscoveredDevice using conversion constructor
|
||||
auto device = std::make_shared<DiscoveredDevice>(*msg, senderIP);
|
||||
discoveredDevices.push_back(device);
|
||||
std::cout << "Discovered new Livox device: " << device->stringify()
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
void BroadcastListener::start(void)
|
||||
{
|
||||
if (isListening.load()) { return; }
|
||||
|
||||
try
|
||||
{
|
||||
/** EXPLANATION:
|
||||
* Set up a boost::asio udp listening socket on the broadcast listening
|
||||
* port.
|
||||
*
|
||||
* FIXME:
|
||||
* We should also set up a timer to check for devices that have gone
|
||||
* away.
|
||||
*/
|
||||
socket.open(boost::asio::ip::udp::v4());
|
||||
socket.bind(listeningEndpoint);
|
||||
|
||||
isListening.store(true);
|
||||
// Start the first async receive operation
|
||||
startReceive();
|
||||
std::cout << "BroadcastListener started on port " << listeningPort
|
||||
<< std::endl;
|
||||
}
|
||||
catch (const boost::system::system_error& e)
|
||||
{
|
||||
isListening.store(false);
|
||||
std::cerr << "Failed to start BroadcastListener: " << e.what()
|
||||
<< std::endl;
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void BroadcastListener::startReceive(void)
|
||||
{
|
||||
if (!isListening.load()) { return; }
|
||||
|
||||
socket.async_receive_from(
|
||||
boost::asio::buffer(bcastMsgRecvBuffer, sizeof(bcastMsgRecvBuffer)),
|
||||
senderEndpoint,
|
||||
[this](const boost::system::error_code& ec, std::size_t bytes_received)
|
||||
{
|
||||
broadcastMsgInd(ec, bytes_received);
|
||||
|
||||
// Continue listening for the next packet
|
||||
if (isListening.load())
|
||||
{ startReceive(); }
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
void BroadcastListener::stop(void)
|
||||
{
|
||||
if (!isListening.load()) { return; }
|
||||
|
||||
isListening.store(false);
|
||||
|
||||
try
|
||||
{
|
||||
socket.close();
|
||||
std::cout << "BroadcastListener stopped" << std::endl;
|
||||
}
|
||||
catch (const boost::system::system_error& e)
|
||||
{
|
||||
std::cerr << "Error stopping BroadcastListener: " << e.what()
|
||||
<< std::endl;
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace comms
|
||||
} // namespace livoxProto1
|
||||
@@ -0,0 +1,175 @@
|
||||
#ifndef LIVOXPROTO1_PROTOCOL_H
|
||||
#define LIVOXPROTO1_PROTOCOL_H
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <sstream>
|
||||
#include <atomic>
|
||||
#include <user/senseApiDesc.h>
|
||||
|
||||
namespace livoxProto1 {
|
||||
namespace comms {
|
||||
|
||||
// Endianness detection
|
||||
namespace endian {
|
||||
inline bool isLittleEndian() {
|
||||
union {
|
||||
uint32_t i;
|
||||
char c[4];
|
||||
} test = {0x01020304};
|
||||
return test.c[0] == 4;
|
||||
}
|
||||
}
|
||||
|
||||
/** EXPLANATION:
|
||||
* Device types as defined in the Livox protocol specification
|
||||
*/
|
||||
enum class DeviceType : uint8_t {
|
||||
Hub = 0,
|
||||
Mid40 = 1,
|
||||
Tele15 = 2,
|
||||
Horizon = 3,
|
||||
Mid70 = 6,
|
||||
Avia = 7
|
||||
};
|
||||
|
||||
/** EXPLANATION:
|
||||
* Protocol frame header structure.
|
||||
* All multi-byte fields are in little-endian format as per protocol spec.
|
||||
*/
|
||||
struct Header
|
||||
{
|
||||
uint8_t sof; // 0: Start of Frame (0xAA)
|
||||
uint8_t version; // 1: Protocol Version (1)
|
||||
uint16_t length; // 2-3: Frame Length (little-endian)
|
||||
uint8_t cmd_type; // 4: Command Type (0x02 = MSG for broadcast)
|
||||
uint16_t seq_num; // 5-6: Sequence Number (little-endian)
|
||||
uint16_t crc_16; // 7-8: Header Checksum (little-endian)
|
||||
|
||||
void swapToHostEndianness();
|
||||
bool sanityCheck() const;
|
||||
} __attribute__((packed));
|
||||
|
||||
/** EXPLANATION:
|
||||
* Protocol frame footer structure.
|
||||
* All multi-byte fields are in little-endian format as per protocol spec.
|
||||
*/
|
||||
struct Footer
|
||||
{
|
||||
uint32_t crc_32; // 0-3: Whole Frame Checksum (little-endian)
|
||||
|
||||
void swapToHostEndianness();
|
||||
bool sanityCheck() const;
|
||||
} __attribute__((packed));
|
||||
|
||||
/** EXPLANATION:
|
||||
* Complete wire format for Livox broadcast messages.
|
||||
* All multi-byte fields are in little-endian format as per protocol spec.
|
||||
*/
|
||||
struct BroadcastMessage
|
||||
{
|
||||
Header header; // 0-8: Protocol frame header
|
||||
uint8_t cmd_set; // 9: Command Set (0x00 = General)
|
||||
uint8_t cmd_id; // 10: Command ID (0x00 = Broadcast)
|
||||
uint8_t broadcast_code[16]; // 11-26: Device Broadcast Code (null-terminated string)
|
||||
uint8_t dev_type; // 27: Device Type
|
||||
uint16_t reserved; // 28-29: Reserved (little-endian)
|
||||
Footer footer; // 30-33: Protocol frame footer
|
||||
|
||||
void swapToHostEndianness();
|
||||
bool sanityCheck() const;
|
||||
} __attribute__((packed));
|
||||
|
||||
/** EXPLANATION:
|
||||
* This class represents a discovered device. It is used to store the
|
||||
* device identifier and IP address of a discovered device.
|
||||
*/
|
||||
class DiscoveredDevice
|
||||
{
|
||||
public:
|
||||
DiscoveredDevice(
|
||||
const std::string &deviceIdentifier,
|
||||
DeviceType deviceType,
|
||||
const std::string &ipAddr);
|
||||
|
||||
// "Conversion" constructor from BroadcastMessage
|
||||
DiscoveredDevice(const BroadcastMessage &msg, const std::string &ipAddr);
|
||||
|
||||
~DiscoveredDevice() = default;
|
||||
|
||||
bool operator==(const DiscoveredDevice &other) const
|
||||
{ return deviceIdentifier == other.deviceIdentifier; }
|
||||
|
||||
std::string stringify(void) const;
|
||||
std::string getDeviceTypeName(void) const;
|
||||
|
||||
public:
|
||||
std::string deviceIdentifier;
|
||||
DeviceType deviceType;
|
||||
std::string ipAddr;
|
||||
};
|
||||
|
||||
/** EXPLANATION:
|
||||
* This class merely listens for UDP bcast dgrams on the designated listening
|
||||
* port. It then builds a list of client device IP addrs that it has heard from.
|
||||
* It doesn't connect to them or signal any events to the rest of the lib,
|
||||
* except in the case that a device which the lib is using has gone away.
|
||||
*
|
||||
* Other than that, its role is to tell the lib which devices are available
|
||||
* on the network.
|
||||
*/
|
||||
#define UDP_BCAST_MSG_BUFFER_NBYTES (1024)
|
||||
|
||||
class BroadcastListener
|
||||
{
|
||||
public:
|
||||
BroadcastListener(
|
||||
const std::shared_ptr<smo::ComponentThread>& componentThread,
|
||||
uint16_t listeningPort=55000, uint16_t connectPort=65000);
|
||||
|
||||
~BroadcastListener() = default;
|
||||
|
||||
typedef void (DeviceGoneAwayCbFn)(const DiscoveredDevice &device);
|
||||
void setDeviceGoneAwayCb(DeviceGoneAwayCbFn *cb)
|
||||
{ deviceGoneAwayCb = cb; }
|
||||
|
||||
bool deviceExists(const std::string &deviceIdentifier) const
|
||||
{ return getDevice(deviceIdentifier) != nullptr; }
|
||||
|
||||
std::shared_ptr<DiscoveredDevice>
|
||||
getDevice(const std::string &deviceIdentifier) const;
|
||||
|
||||
void start(void);
|
||||
void stop(void);
|
||||
|
||||
void broadcastMsgInd(
|
||||
const boost::system::error_code& ec, std::size_t bytes_received);
|
||||
|
||||
private:
|
||||
void startReceive(void);
|
||||
|
||||
private:
|
||||
std::shared_ptr<smo::ComponentThread> componentThread;
|
||||
/** EXPLANATION:
|
||||
* The Livox proto says that client devices will spam broadcast UDP
|
||||
* dgrams to us on the listening port. We can then use the source IP from
|
||||
* the bcast dgram to figure out the client device's IP addr. Then we
|
||||
* should send a connect dgram to the connect port. This will tell the
|
||||
* client device our IP addr.
|
||||
*/
|
||||
uint16_t listeningPort, connectPort;
|
||||
DeviceGoneAwayCbFn *deviceGoneAwayCb;
|
||||
std::vector<std::shared_ptr<DiscoveredDevice>> discoveredDevices;
|
||||
|
||||
boost::asio::ip::udp::socket socket;
|
||||
boost::asio::ip::udp::endpoint listeningEndpoint, senderEndpoint;
|
||||
std::atomic<bool> isListening;
|
||||
|
||||
uint8_t bcastMsgRecvBuffer[UDP_BCAST_MSG_BUFFER_NBYTES];
|
||||
};
|
||||
|
||||
} // namespace comms
|
||||
} // namespace livoxProto1
|
||||
|
||||
#endif // LIVOXPROTO1_PROTOCOL_H
|
||||
@@ -1 +1,2 @@
|
||||
add_subdirectory(xcbWindow)
|
||||
add_subdirectory(livoxGen1)
|
||||
|
||||
@@ -0,0 +1,22 @@
|
||||
cmake_dependent_option(ENABLE_SENSEAPI_livoxGen1
|
||||
"Enable Livox Gen1 LiDAR sense API" OFF
|
||||
"ENABLE_LIB_livoxProto1" ON)
|
||||
|
||||
if(ENABLE_SENSEAPI_livoxGen1)
|
||||
add_library(livoxGen1 SHARED
|
||||
livoxGen1.cpp
|
||||
)
|
||||
|
||||
# Set config define for header generation
|
||||
add_compile_definitions(CONFIG_SENSEAPI_LIVOXGEN1_ENABLED)
|
||||
target_include_directories(livoxGen1 PUBLIC
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${CMAKE_SOURCE_DIR}/commonLibs
|
||||
)
|
||||
target_link_libraries(livoxGen1
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
# Install rules
|
||||
install(TARGETS livoxGen1 DESTINATION lib)
|
||||
endif()
|
||||
@@ -0,0 +1,159 @@
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <functional>
|
||||
#include <dlfcn.h>
|
||||
#include <user/senseApiDesc.h>
|
||||
#include <user/deviceAttachmentSpec.h>
|
||||
#include <livoxProto1/livoxProto1.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)
|
||||
{}
|
||||
|
||||
static void DlCloser(void* handle)
|
||||
{
|
||||
if (handle) {
|
||||
dlclose(handle);
|
||||
}
|
||||
}
|
||||
|
||||
std::unique_ptr<void, void(*)(void*)> dlopenHandle;
|
||||
livoxProto1_mainFn *livoxProto1_main;
|
||||
livoxProto1_exitFn *livoxProto1_exit;
|
||||
};
|
||||
|
||||
static LivoxProto1DllState livoxProto1;
|
||||
|
||||
// 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"));
|
||||
|
||||
if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit) {
|
||||
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)
|
||||
{
|
||||
// TODO: Implement finalization logic
|
||||
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
|
||||
)
|
||||
{
|
||||
// TODO: Implement device attachment logic
|
||||
(void)desc; // Suppress unused parameter warning
|
||||
return 0; // Success
|
||||
}
|
||||
|
||||
extern "C" int livoxGen1_detachDeviceReq(
|
||||
const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
|
||||
)
|
||||
{
|
||||
// TODO: Implement device detachment logic
|
||||
(void)desc; // Suppress unused parameter warning
|
||||
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
|
||||
Reference in New Issue
Block a user