diff --git a/commonLibs/CMakeLists.txt b/commonLibs/CMakeLists.txt index c25c92a..a508a5a 100644 --- a/commonLibs/CMakeLists.txt +++ b/commonLibs/CMakeLists.txt @@ -1 +1,2 @@ add_subdirectory(xcbXorg) +add_subdirectory(livoxProto1) diff --git a/commonLibs/livoxProto1/CMakeLists.txt b/commonLibs/livoxProto1/CMakeLists.txt new file mode 100644 index 0000000..ccc94c0 --- /dev/null +++ b/commonLibs/livoxProto1/CMakeLists.txt @@ -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() diff --git a/commonLibs/livoxProto1/livoxProto1.cpp b/commonLibs/livoxProto1/livoxProto1.cpp new file mode 100644 index 0000000..c87ac18 --- /dev/null +++ b/commonLibs/livoxProto1/livoxProto1.cpp @@ -0,0 +1,21 @@ +#include +#include "livoxProto1.h" +#include "livoxProto1Core.h" + +extern "C" { + +livoxProto1_mainFn livoxProto1_main; +livoxProto1_exitFn livoxProto1_exit; + +void livoxProto1_main( + const std::shared_ptr &componentThread) +{ + livoxProto1::main(componentThread); +} + +void livoxProto1_exit(void) +{ + livoxProto1::exit(); +} + +} // extern "C" diff --git a/commonLibs/livoxProto1/livoxProto1.h b/commonLibs/livoxProto1/livoxProto1.h new file mode 100644 index 0000000..a2f85ff --- /dev/null +++ b/commonLibs/livoxProto1/livoxProto1.h @@ -0,0 +1,25 @@ +#ifndef LIVOX_PROTO1_API_H +#define LIVOX_PROTO1_API_H + +#include +#include +#include +#include +#include "livoxProto1Core.h" +#include "livoxProto1Device.h" + +namespace livoxProto1 { + +} // namespace livoxProto1 + +extern "C" { + typedef void (livoxProto1_mainFn)( + const std::shared_ptr &componentThread); + typedef void (livoxProto1_exitFn)(void); + + void livoxProto1_main( + const std::shared_ptr &componentThread); + void livoxProto1_exit(void); +} // extern "C" + +#endif // LIVOX_PROTO1_API_H diff --git a/commonLibs/livoxProto1/livoxProto1Core.cpp b/commonLibs/livoxProto1/livoxProto1Core.cpp new file mode 100644 index 0000000..4cb48df --- /dev/null +++ b/commonLibs/livoxProto1/livoxProto1Core.cpp @@ -0,0 +1,67 @@ +#include +#include "livoxProto1Protocol.h" +#include "livoxProto1Core.h" + +namespace livoxProto1 { + +struct ProtoState +{ + bool isInitialized = false; + std::shared_ptr componentThread; + std::unique_ptr 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 &componentThread) +{ + if (protoState.isInitialized) { + return; + } + + protoState.isInitialized = true; + protoState.componentThread = componentThread; + protoState.deviceManager = std::make_unique(); + 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 diff --git a/commonLibs/livoxProto1/livoxProto1Core.h b/commonLibs/livoxProto1/livoxProto1Core.h new file mode 100644 index 0000000..db3cd24 --- /dev/null +++ b/commonLibs/livoxProto1/livoxProto1Core.h @@ -0,0 +1,39 @@ +#ifndef LIVOXPROTO1_CORE_H +#define LIVOXPROTO1_CORE_H + +#include +#include +#include +#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 devices; + comms::BroadcastListener broadcastListener; +}; + +void main(const std::shared_ptr &componentThread); +void exit(void); + +} // namespace livoxProto1 + +#endif // LIVOXPROTO1_CORE_H diff --git a/commonLibs/livoxProto1/livoxProto1Device.cpp b/commonLibs/livoxProto1/livoxProto1Device.cpp new file mode 100644 index 0000000..e69de29 diff --git a/commonLibs/livoxProto1/livoxProto1Device.h b/commonLibs/livoxProto1/livoxProto1Device.h new file mode 100644 index 0000000..e69de29 diff --git a/commonLibs/livoxProto1/livoxProto1Protocol.cpp b/commonLibs/livoxProto1/livoxProto1Protocol.cpp new file mode 100644 index 0000000..d2c64a8 --- /dev/null +++ b/commonLibs/livoxProto1/livoxProto1Protocol.cpp @@ -0,0 +1,243 @@ +#include +#include +#include +#include +#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(msg.broadcast_code), + static_cast(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& 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 +BroadcastListener::getDevice(const std::string &deviceIdentifier) const +{ + auto it = std::find_if(discoveredDevices.begin(), discoveredDevices.end(), + [&deviceIdentifier](const std::shared_ptr& 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(msg->broadcast_code)); + + // Early return if device already exists + if (deviceExists(broadcastCode)) { return; } + + // Create new DiscoveredDevice using conversion constructor + auto device = std::make_shared(*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 diff --git a/commonLibs/livoxProto1/livoxProto1Protocol.h b/commonLibs/livoxProto1/livoxProto1Protocol.h new file mode 100644 index 0000000..72c142c --- /dev/null +++ b/commonLibs/livoxProto1/livoxProto1Protocol.h @@ -0,0 +1,175 @@ +#ifndef LIVOXPROTO1_PROTOCOL_H +#define LIVOXPROTO1_PROTOCOL_H + +#include +#include +#include +#include +#include +#include + +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& 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 + 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 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> discoveredDevices; + + boost::asio::ip::udp::socket socket; + boost::asio::ip::udp::endpoint listeningEndpoint, senderEndpoint; + std::atomic isListening; + + uint8_t bcastMsgRecvBuffer[UDP_BCAST_MSG_BUFFER_NBYTES]; +}; + +} // namespace comms +} // namespace livoxProto1 + +#endif // LIVOXPROTO1_PROTOCOL_H diff --git a/senseApis/CMakeLists.txt b/senseApis/CMakeLists.txt index 24a54a5..efe2191 100644 --- a/senseApis/CMakeLists.txt +++ b/senseApis/CMakeLists.txt @@ -1 +1,2 @@ add_subdirectory(xcbWindow) +add_subdirectory(livoxGen1) diff --git a/senseApis/livoxGen1/CMakeLists.txt b/senseApis/livoxGen1/CMakeLists.txt new file mode 100644 index 0000000..7b1763c --- /dev/null +++ b/senseApis/livoxGen1/CMakeLists.txt @@ -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() diff --git a/senseApis/livoxGen1/livoxGen1.cpp b/senseApis/livoxGen1/livoxGen1.cpp new file mode 100644 index 0000000..80d3541 --- /dev/null +++ b/senseApis/livoxGen1/livoxGen1.cpp @@ -0,0 +1,159 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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& desc); +extern "C" int livoxGen1_detachDeviceReq( + const std::shared_ptr& 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( + dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_main")); + livoxProto1.livoxProto1_exit = reinterpret_cast( + 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& desc + ) +{ + // TODO: Implement device attachment logic + (void)desc; // Suppress unused parameter warning + return 0; // Success +} + +extern "C" int livoxGen1_detachDeviceReq( + const std::shared_ptr& 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