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(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(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