From 3e9eecc279d33afe8a0c29663f476669f37049e1 Mon Sep 17 00:00:00 2001 From: Hayodea Hakol Date: Sat, 6 Sep 2025 08:50:07 -0400 Subject: [PATCH] livoxProto1: Keep protocol headers pure; Split the BroadcastListener, DiscoveredDevice and other concerns out of the protocol header and implementation files. --- commonLibs/livoxProto1/CMakeLists.txt | 1 + commonLibs/livoxProto1/broadcastListener.cpp | 157 +++++++++++++++ commonLibs/livoxProto1/broadcastListener.h | 76 +++++++ commonLibs/livoxProto1/livoxProto1Core.h | 13 +- commonLibs/livoxProto1/livoxProto1Device.cpp | 61 ++++++ commonLibs/livoxProto1/livoxProto1Device.h | 53 +++++ .../livoxProto1/livoxProto1Protocol.cpp | 185 ------------------ commonLibs/livoxProto1/livoxProto1Protocol.h | 88 --------- 8 files changed, 350 insertions(+), 284 deletions(-) create mode 100644 commonLibs/livoxProto1/broadcastListener.cpp create mode 100644 commonLibs/livoxProto1/broadcastListener.h diff --git a/commonLibs/livoxProto1/CMakeLists.txt b/commonLibs/livoxProto1/CMakeLists.txt index ccc94c0..dd72ab4 100644 --- a/commonLibs/livoxProto1/CMakeLists.txt +++ b/commonLibs/livoxProto1/CMakeLists.txt @@ -6,6 +6,7 @@ if(ENABLE_LIB_livoxProto1) livoxProto1Core.cpp livoxProto1Device.cpp livoxProto1Protocol.cpp + broadcastListener.cpp ) # Set config define for header generation diff --git a/commonLibs/livoxProto1/broadcastListener.cpp b/commonLibs/livoxProto1/broadcastListener.cpp new file mode 100644 index 0000000..8c3fcb7 --- /dev/null +++ b/commonLibs/livoxProto1/broadcastListener.cpp @@ -0,0 +1,157 @@ +#include +#include +#include "broadcastListener.h" + +namespace livoxProto1 { +namespace comms { + +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; + + // Validate the message using sanity check methods + 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)) + { + // Device already exists, just log the update + std::cout << "Received broadcast from known device: " << broadcastCode + << " at " << senderIP << std::endl; + return; + } + + // Create new DiscoveredDevice using conversion constructor + auto device = std::make_shared(*msg, senderIP); + discoveredDevices.push_back(device); + + // Output device information using stringify + 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/broadcastListener.h b/commonLibs/livoxProto1/broadcastListener.h new file mode 100644 index 0000000..b796117 --- /dev/null +++ b/commonLibs/livoxProto1/broadcastListener.h @@ -0,0 +1,76 @@ +#ifndef BROADCAST_LISTENER_H +#define BROADCAST_LISTENER_H + +#include +#include +#include +#include +#include +#include "livoxProto1Device.h" + +namespace livoxProto1 { +namespace comms { + +/** 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 // BROADCAST_LISTENER_H diff --git a/commonLibs/livoxProto1/livoxProto1Core.h b/commonLibs/livoxProto1/livoxProto1Core.h index db3cd24..acd648c 100644 --- a/commonLibs/livoxProto1/livoxProto1Core.h +++ b/commonLibs/livoxProto1/livoxProto1Core.h @@ -4,20 +4,11 @@ #include #include #include -#include "livoxProto1Protocol.h" +#include "livoxProto1Device.h" +#include "broadcastListener.h" namespace livoxProto1 { -class Device -{ -public: - Device(const comms::DiscoveredDevice &discoveredDevice); - ~Device() = default; - -public: - comms::DiscoveredDevice discoveredDevice; -}; - class DeviceManager { public: diff --git a/commonLibs/livoxProto1/livoxProto1Device.cpp b/commonLibs/livoxProto1/livoxProto1Device.cpp index e69de29..c670707 100644 --- a/commonLibs/livoxProto1/livoxProto1Device.cpp +++ b/commonLibs/livoxProto1/livoxProto1Device.cpp @@ -0,0 +1,61 @@ +#include +#include "livoxProto1Device.h" + +namespace livoxProto1 { +namespace comms { + +// 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"; + } +} + +} // namespace comms + +// Device implementation +Device::Device(const comms::DiscoveredDevice &discoveredDevice) +: discoveredDevice(discoveredDevice) +{ +} + +} // namespace livoxProto1 diff --git a/commonLibs/livoxProto1/livoxProto1Device.h b/commonLibs/livoxProto1/livoxProto1Device.h index e69de29..3801bd6 100644 --- a/commonLibs/livoxProto1/livoxProto1Device.h +++ b/commonLibs/livoxProto1/livoxProto1Device.h @@ -0,0 +1,53 @@ +#ifndef LIVOX_PROTO1_DEVICE_H +#define LIVOX_PROTO1_DEVICE_H + +#include +#include "livoxProto1Protocol.h" + +namespace livoxProto1 { +namespace comms { + +/** 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; +}; + +} // namespace comms + +class Device +{ +public: + Device(const comms::DiscoveredDevice &discoveredDevice); + ~Device() = default; + +public: + comms::DiscoveredDevice discoveredDevice; +}; + +} // namespace livoxProto1 + +#endif // LIVOX_PROTO1_DEVICE_H diff --git a/commonLibs/livoxProto1/livoxProto1Protocol.cpp b/commonLibs/livoxProto1/livoxProto1Protocol.cpp index d2c64a8..f2c7483 100644 --- a/commonLibs/livoxProto1/livoxProto1Protocol.cpp +++ b/commonLibs/livoxProto1/livoxProto1Protocol.cpp @@ -54,190 +54,5 @@ bool BroadcastMessage::sanityCheck() const 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 index 72c142c..3fe5bce 100644 --- a/commonLibs/livoxProto1/livoxProto1Protocol.h +++ b/commonLibs/livoxProto1/livoxProto1Protocol.h @@ -81,94 +81,6 @@ struct BroadcastMessage 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