From 45ad5c83eed83f0b0cda33db5dc1fd9137d89706 Mon Sep 17 00:00:00 2001 From: Hayodea Hekol Date: Sat, 1 Nov 2025 22:41:58 -0400 Subject: [PATCH] livoxProto1: Open pcloudDataSocket in UdpCmdDemux The pcloudData socket is now opened by UdpCommandDemuxer, when libLivoxProto1 is initialized. We can now just pick up the socket and be certain it'll be there if the lib is being executed. --- commonLibs/livoxProto1/device.cpp | 93 +------------------- commonLibs/livoxProto1/device.h | 1 - commonLibs/livoxProto1/livoxProto1.cpp | 15 ++++ commonLibs/livoxProto1/livoxProto1.h | 4 + commonLibs/livoxProto1/udpCommandDemuxer.cpp | 77 +++++++++++++++- commonLibs/livoxProto1/udpCommandDemuxer.h | 10 ++- 6 files changed, 101 insertions(+), 99 deletions(-) diff --git a/commonLibs/livoxProto1/device.cpp b/commonLibs/livoxProto1/device.cpp index 05c5a9a..b30b3a8 100644 --- a/commonLibs/livoxProto1/device.cpp +++ b/commonLibs/livoxProto1/device.cpp @@ -113,13 +113,9 @@ Device::~Device() if (pcloudDataActive.load()) { pcloudDataActive.store(false); - if (pcloudDataSocketDesc) { - pcloudDataSocketDesc->cancel(); - } } heartbeatTimer.reset(); - pcloudDataSocketDesc.reset(); } /** @@ -1347,9 +1343,6 @@ public: // The timeout timer. boost::asio::deadline_timer timeoutTimer; - // Temporary point cloud data socket descriptor (for enable operations) - boost::asio::posix::stream_descriptor tmpPcloudEnableFdDesc; - // Received data storage uint8_t responseBuffer[1024]{}; ssize_t bytesReceived = -1; @@ -1362,8 +1355,7 @@ protected: smo::Callback cb) : smo::NonPostedAsynchronousContinuation(std::move(cb)), device(dev), - timeoutTimer(device.componentThread->getIoService()), - tmpPcloudEnableFdDesc(device.componentThread->getIoService()) + timeoutTimer(device.componentThread->getIoService()) {} public: @@ -1540,10 +1532,6 @@ protected: void cleanup() { timeoutTimer.cancel(); - if (tmpPcloudEnableFdDesc.is_open()) { - tmpPcloudEnableFdDesc.cancel(); - tmpPcloudEnableFdDesc.close(); - } } // Pure virtual methods that derived classes must implement @@ -1551,61 +1539,6 @@ protected: virtual const char* getCommandName() const = 0; virtual void setPcloudDataActiveState() = 0; - // Method to set up temporary point cloud data socket - bool setupTmpPcloudDataSocket() - { - // RAII class to manage socket file descriptor - struct SocketRAII - { - int fd; - SocketRAII(int socketFd) : fd(socketFd) {} - ~SocketRAII() { if (fd >= 0) close(fd); } - void release() { fd = -1; } // Release ownership, prevent close - int getFd() const { return fd; } - bool isValid() const { return fd >= 0; } - }; - - // Create UDP socket for point cloud data reception - SocketRAII socketGuard(socket(AF_INET, SOCK_DGRAM, 0)); - if (!socketGuard.isValid()) - { - std::cerr << __func__ << ": Failed to create socket: " - << strerror(errno) << std::endl; - return false; - } - - // Set socket to non-blocking mode - int flags = fcntl(socketGuard.getFd(), F_GETFL, 0); - if (flags < 0 || - fcntl(socketGuard.getFd(), F_SETFL, flags | O_NONBLOCK) < 0) - { - std::cerr << __func__ << ": Failed to set non-blocking mode: " - << strerror(errno) << std::endl; - return false; - } - - // Bind to the data port (65001) - struct sockaddr_in localAddr; - memset(&localAddr, 0, sizeof(localAddr)); - localAddr.sin_family = AF_INET; - localAddr.sin_addr.s_addr = INADDR_ANY; - localAddr.sin_port = htons(65001); // Data port - - if (bind( - socketGuard.getFd(), (struct sockaddr *)&localAddr, - sizeof(localAddr)) < 0) - { - std::cerr << __func__ << ": Failed to bind to data port: " - << strerror(errno) << std::endl; - return false; - } - - // Assign the file descriptor to the stream descriptor - tmpPcloudEnableFdDesc.assign(socketGuard.getFd()); - socketGuard.release(); - return true; - } - // Common sendCommand implementation bool sendCommand() { @@ -1691,17 +1624,6 @@ private: void setPcloudDataActiveState() override { - // Transfer ownership of the socket from temporary to main descriptor - if (tmpPcloudEnableFdDesc.is_open()) - { - // Close the temporary descriptor (but don't close the fd) - int fd = tmpPcloudEnableFdDesc.native_handle(); - tmpPcloudEnableFdDesc.release(); - // Give the transient FD to the Device object. - device.pcloudDataSocketDesc = - std::make_unique( - device.componentThread->getIoService(), fd); - } device.pcloudDataActive.store(true); } }; @@ -1758,15 +1680,6 @@ void Device::enablePcloudDataReq( return; } - // Set up the temporary point cloud data socket for actual data reception - if (!request->setupTmpPcloudDataSocket()) - { - std::cerr << __func__ << ": Failed to set up transient pcloud data FD." - << std::endl; - request->callOriginalCallbackWithFailure(); - return; - } - // Send the start sampling command if (!request->sendCommand()) { @@ -1813,10 +1726,6 @@ void Device::disablePcloudDataReq( void Device::cleanupPcloudDataSocket() { - if (pcloudDataSocketDesc) { - pcloudDataSocketDesc->cancel(); - pcloudDataSocketDesc.reset(); - } pcloudDataActive.store(false); } diff --git a/commonLibs/livoxProto1/device.h b/commonLibs/livoxProto1/device.h index ea56c24..f44617b 100644 --- a/commonLibs/livoxProto1/device.h +++ b/commonLibs/livoxProto1/device.h @@ -163,7 +163,6 @@ public: std::atomic heartbeatActive; // Point cloud data state - std::unique_ptr pcloudDataSocketDesc; std::atomic pcloudDataActive; // Cached last-known return mode for this device diff --git a/commonLibs/livoxProto1/livoxProto1.cpp b/commonLibs/livoxProto1/livoxProto1.cpp index 4656441..e108cfc 100644 --- a/commonLibs/livoxProto1/livoxProto1.cpp +++ b/commonLibs/livoxProto1/livoxProto1.cpp @@ -1,8 +1,10 @@ #include #include +#include #include "livoxProto1.h" #include "device.h" #include "core.h" +#include "udpCommandDemuxer.h" extern "C" { @@ -104,4 +106,17 @@ void livoxProto1_device_getReturnModeReq( device->getReturnModeReq(callback); } +std::shared_ptr +livoxProto1_getPcloudDataFdDesc(void) +{ + auto& protoState = livoxProto1::getProtoState(); + if (!protoState.deviceManager) + { + throw std::runtime_error(std::string(__func__) + + ": DeviceManager not initialized"); + } + + return protoState.deviceManager->udpCommandDemuxer.pcloudDataSocketDesc; +} + } // extern "C" diff --git a/commonLibs/livoxProto1/livoxProto1.h b/commonLibs/livoxProto1/livoxProto1.h index 4110868..4b865f8 100644 --- a/commonLibs/livoxProto1/livoxProto1.h +++ b/commonLibs/livoxProto1/livoxProto1.h @@ -85,6 +85,9 @@ typedef void livoxProto1_device_getReturnModeReqFn( std::shared_ptr device, smo::Callback callback); +typedef std::shared_ptr + livoxProto1_getPcloudDataFdDescFn(void); + livoxProto1_mainFn livoxProto1_main; livoxProto1_exitFn livoxProto1_exit; livoxProto1_getOrCreateDeviceReqFn livoxProto1_getOrCreateDeviceReq; @@ -93,6 +96,7 @@ livoxProto1_device_enablePcloudDataReqFn livoxProto1_device_enablePcloudDataReq; livoxProto1_device_disablePcloudDataReqFn livoxProto1_device_disablePcloudDataReq; livoxProto1_device_getReturnModeReqFn livoxProto1_device_getReturnModeReq; +livoxProto1_getPcloudDataFdDescFn livoxProto1_getPcloudDataFdDesc; #ifdef __cplusplus } diff --git a/commonLibs/livoxProto1/udpCommandDemuxer.cpp b/commonLibs/livoxProto1/udpCommandDemuxer.cpp index 001237f..3e8c6b9 100644 --- a/commonLibs/livoxProto1/udpCommandDemuxer.cpp +++ b/commonLibs/livoxProto1/udpCommandDemuxer.cpp @@ -18,10 +18,11 @@ namespace comms { UdpCommandDemuxer::UdpCommandDemuxer( const std::shared_ptr &componentThread, DeviceManager &deviceManager, - uint16_t commandPort + uint16_t commandPort, + uint16_t dataPort ) : componentThread(componentThread), deviceManager(deviceManager), -commandPort(commandPort), +commandPort(commandPort), dataPort(dataPort), senderAddrLen(sizeof(senderAddr)) { } @@ -42,7 +43,7 @@ void UdpCommandDemuxer::start() try { - setupSocket(); + setupSockets(); isActive.store(true); shouldStop.store(false); @@ -77,13 +78,25 @@ void UdpCommandDemuxer::stop() cmdEndpointFdDesc.reset(); } + if (pcloudDataSocketDesc) + { + pcloudDataSocketDesc->cancel(); + pcloudDataSocketDesc.reset(); + } + isActive.store(false); std::cout << __func__ << ": UDP Command Demuxer stopped" << std::endl; } -void UdpCommandDemuxer::setupSocket() +void UdpCommandDemuxer::setupSockets() +{ + setupCommandSocket(); + setupPcloudDataSocket(); +} + +void UdpCommandDemuxer::setupCommandSocket() { // RAII class to manage socket file descriptor struct SocketRAII @@ -139,6 +152,62 @@ void UdpCommandDemuxer::setupSocket() socketGuard.commit(); } +void UdpCommandDemuxer::setupPcloudDataSocket() +{ + // RAII class to manage socket file descriptor + struct SocketRAII + { + int fd; + SocketRAII(int socketFd) : fd(socketFd) {} + ~SocketRAII() { if (fd >= 0) close(fd); } + void commit() { fd = -1; } // Transfer ownership, prevent close + int getFd() const { return fd; } + bool isValid() const { return fd >= 0; } + }; + + // Create UDP socket for point cloud data reception + SocketRAII socketGuard(socket(AF_INET, SOCK_DGRAM, 0)); + if (!socketGuard.isValid()) + { + throw std::runtime_error( + std::string(__func__) + + ": Failed to create socket: " + strerror(errno)); + } + + // Set socket to non-blocking mode + int flags = fcntl(socketGuard.getFd(), F_GETFL, 0); + if (flags < 0 || + fcntl(socketGuard.getFd(), F_SETFL, flags | O_NONBLOCK) < 0) + { + throw std::runtime_error( + std::string(__func__) + + ": Failed to set non-blocking mode: " + strerror(errno)); + } + + // Bind to the data port + struct sockaddr_in localAddr; + memset(&localAddr, 0, sizeof(localAddr)); + localAddr.sin_family = AF_INET; + localAddr.sin_addr.s_addr = INADDR_ANY; + localAddr.sin_port = htons(dataPort); + + if (bind( + socketGuard.getFd(), (struct sockaddr *)&localAddr, + sizeof(localAddr)) < 0) + { + throw std::runtime_error( + std::string(__func__) + ": Failed to bind to data port: " + + std::to_string(dataPort) + ": " + strerror(errno)); + } + + // Create boost wrapper for async operations + pcloudDataSocketDesc = std::make_shared( + componentThread->getIoService(), socketGuard.getFd()); + + // Transfer ownership, prevent auto-close + socketGuard.commit(); +} + void UdpCommandDemuxer::startAsyncReceive() { if (!isActive.load() || shouldStop.load()) diff --git a/commonLibs/livoxProto1/udpCommandDemuxer.h b/commonLibs/livoxProto1/udpCommandDemuxer.h index 7f44b1c..03e8b4d 100644 --- a/commonLibs/livoxProto1/udpCommandDemuxer.h +++ b/commonLibs/livoxProto1/udpCommandDemuxer.h @@ -37,7 +37,8 @@ public: UdpCommandDemuxer( const std::shared_ptr& componentThread, DeviceManager& deviceManager, - uint16_t commandPort = 56001); + uint16_t commandPort = 56001, + uint16_t dataPort = 56000); ~UdpCommandDemuxer(); @@ -53,7 +54,9 @@ public: } private: - void setupSocket(); + void setupSockets(); + void setupCommandSocket(); + void setupPcloudDataSocket(); void startAsyncReceive(); void onDataReady(const boost::system::error_code& error); void processIncomingData(); @@ -61,7 +64,10 @@ private: std::shared_ptr componentThread; DeviceManager& deviceManager; uint16_t commandPort; + uint16_t dataPort; + // Socket and async objects + std::shared_ptr pcloudDataSocketDesc; // Socket and async objects std::shared_ptr cmdEndpointFdDesc;