From 266cabcddb117ceb5768c1565d1da109da805420 Mon Sep 17 00:00:00 2001 From: Hayodea Hekol Date: Sat, 25 Oct 2025 00:19:06 -0400 Subject: [PATCH] LivoxGen1: Add get/setReturnModeReq() --- commonLibs/livoxProto1/device.cpp | 564 +++++++++++++++++++++++++++++- commonLibs/livoxProto1/device.h | 8 + 2 files changed, 567 insertions(+), 5 deletions(-) diff --git a/commonLibs/livoxProto1/device.cpp b/commonLibs/livoxProto1/device.cpp index 54b627e..37a219c 100644 --- a/commonLibs/livoxProto1/device.cpp +++ b/commonLibs/livoxProto1/device.cpp @@ -193,7 +193,7 @@ public: // Fail early - if this also failed, all connection attempts failed if (!success) { - context->callOriginalCb(false); + context->callOriginalCallbackWithFailure(); return; } @@ -213,7 +213,7 @@ public: if (error) { // Timer was cancelled or error occurred - context->callOriginalCb(false); + context->callOriginalCallbackWithFailure(); return; } @@ -221,7 +221,7 @@ public: {context, std::bind(&ConnectReq::connectReq4, context.get(), context, std::placeholders::_1)}); - } + } void connectReq4( std::shared_ptr context, @@ -234,12 +234,20 @@ public: "device (" << context->device.discoveredDevice.deviceIdentifier << ") @(" << context->device.discoveredDevice.ipAddr << ").\n"; - context->callOriginalCb(false); + context->callOriginalCallbackWithFailure(); return; } - context->callOriginalCb(success); + context->callOriginalCallback(success); } + + // Public accessor for the original callback + void callOriginalCallback(bool success) + { callOriginalCb(success); } + + // Wrapper for failure cases + void callOriginalCallbackWithFailure() + { callOriginalCallback(false); } }; void Device::connectReq(smo::Callback callback) @@ -1957,4 +1965,550 @@ void Device::unregisterUdpCommandHandler( } } +// SetReturnModeReq continuation class +class Device::SetReturnModeReq +: public smo::NonPostedAsynchronousContinuation +{ +public: + enum class SocketState + { + SOCKET_STILL_WAITING = 0, + SOCKET_ERROR, + SOCKET_RECV_SUCCESS, + SOCKET_RECV_ERROR + }; + +public: + Device& device; + uint8_t returnMode; + + // Atomic state flags for async coordination + std::atomic timerFired{false}; + std::atomic socketState{SocketState::SOCKET_STILL_WAITING}; + std::atomic handlerExecuted{false}; + + // The timeout timer. + boost::asio::deadline_timer timeoutTimer; + + // Received data storage + uint8_t responseBuffer[1024]{}; + ssize_t bytesReceived = -1; + struct sockaddr_in senderAddr; + socklen_t senderAddrLen = sizeof(senderAddr); + +public: + friend void Device::setReturnModeReq( + uint8_t returnMode, + smo::Callback callback); + + SetReturnModeReq( + Device& dev, uint8_t mode, + smo::Callback cb) + : smo::NonPostedAsynchronousContinuation( + std::move(cb)), + device(dev), returnMode(mode), + timeoutTimer(device.componentThread->getIoService()) + {} + + virtual ~SetReturnModeReq() + { + cleanup(); + } + + // Public accessor for the original callback + void callOriginalCallback(bool success) + { this->callOriginalCb(success); } + + void callOriginalCallbackWithFailure() + { this->callOriginalCb(false); } + + void setupAsyncCallbacks(std::shared_ptr request) + { + // Set up timeout timer + timeoutTimer.expires_from_now(boost::posix_time::milliseconds( + device.handshakeTimeoutMs)); + timeoutTimer.async_wait( + std::bind(&SetReturnModeReq::setReturnModeReq1_1, + this, request, + std::placeholders::_1)); + + // Register UDP command handler for set return mode response (cmd_set=0x01, cmd_id=0x06) + device.registerUdpCommandHandler( + 0x01, 0x06, + std::bind(&SetReturnModeReq::setReturnModeReq1_2, + this, request, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + device.discoveredDevice.ipAddr); + } + + void setReturnModeReq1_1( + std::shared_ptr context, + const boost::system::error_code& error + ) + { + if (error == boost::asio::error::operation_aborted) { + // Timer was cancelled, ignore + return; + } + + context->timerFired.store(true); + context->setReturnModeReq2(context); + } + + void setReturnModeReq1_2( + std::shared_ptr context, + const uint8_t* data, ssize_t bytesReceived, + const struct sockaddr_in& senderAddr + ) + { + // Store the received data + context->bytesReceived = bytesReceived; + context->senderAddr = senderAddr; + context->senderAddrLen = sizeof(senderAddr); + + // Copy the data to our buffer + if (bytesReceived > 0 + && bytesReceived <= (ssize_t)sizeof(context->responseBuffer)) + { + memcpy(context->responseBuffer, data, bytesReceived); + context->socketState = SocketState::SOCKET_RECV_SUCCESS; + } else + { + context->socketState = SocketState::SOCKET_RECV_ERROR; + std::cerr << __func__ << ": Invalid data size: " << bytesReceived + << std::endl; + } + + context->setReturnModeReq2(context); + } + + void setReturnModeReq2( + std::shared_ptr context + ) + { + // Only execute once + if (context->handlerExecuted.exchange(true)) { return; } + + context->timeoutTimer.cancel(); + device.unregisterUdpCommandHandler( + 0x01, 0x06, device.discoveredDevice.ipAddr); + + SocketState finalSocketState = context->socketState.load(); + bool finalTimerFired = context->timerFired.load(); + + // Check for timeout only if there was no socket activity + if (finalTimerFired + && finalSocketState == SocketState::SOCKET_STILL_WAITING) + { + std::cerr << __func__ << ": Set return mode timeout with " + << device.discoveredDevice.ipAddr << "(" + << device.discoveredDevice.deviceIdentifier << ")" << "\n"; + context->callOriginalCallbackWithFailure(); + return; + } + + // Socket error from boost::asio + if (finalSocketState == SocketState::SOCKET_ERROR) + { + std::cerr << __func__ << ": Socket error during set return mode with " + << device.discoveredDevice.ipAddr << std::endl; + context->callOriginalCallbackWithFailure(); + return; + } + + if (finalSocketState == SocketState::SOCKET_RECV_ERROR) + { + std::cerr << __func__ << ": Receive error during set return mode with " + << device.discoveredDevice.ipAddr << "\n"; + context->callOriginalCallbackWithFailure(); + return; + } + + /* Result must have been RECV_SUCCESS state if we reach here. + * Data was already read in the async callback, just validate it + */ + if (context->bytesReceived + < static_cast(sizeof(comms::SetLiDARReturnModeResponse))) + { + std::cerr << __func__ << ": Response of size " + << context->bytesReceived << " too small from " + << device.discoveredDevice.ipAddr << "\n"; + context->callOriginalCallbackWithFailure(); + return; + } + + comms::SetLiDARReturnModeResponse* response = + reinterpret_cast( + context->responseBuffer); + response->swapContentsToHostEndianness(); + + // Early callback return on error; success path only if all checks pass + if (response->command.cmd_set != 0x01 || + response->command.cmd_id != 0x06 || + response->ret_code != 0x00) + { + context->callOriginalCallbackWithFailure(); + return; + } + + context->callOriginalCallback(true); + } + + void cleanup() + { + timeoutTimer.cancel(); + } + + bool sendCommand() + { + // Get the command endpoint from the UdpCommandDemuxer + auto& protoState = livoxProto1::getProtoState(); + if (!protoState.deviceManager) + { + std::cerr << __func__ << ": No device manager available.\n"; + return false; + } + + auto cmdEndpointFdDesc = protoState.deviceManager->udpCommandDemuxer + .getCmdEndpointFdDesc(); + if (!cmdEndpointFdDesc) + { + std::cerr << __func__ << ": No command endpoint available.\n"; + return false; + } + + // Create set return mode message + comms::SetLiDARReturnMode setReturnModeMsg; + setReturnModeMsg.mode = returnMode; + setReturnModeMsg.swapContentsToProtocolEndianness(); + setReturnModeMsg.header.setCrc16FromRawBytes(); + setReturnModeMsg.header.swapCrc16ToProtocolEndianness(); + setReturnModeMsg.footer.crc_32 = setReturnModeMsg.calculateCrc32(); + setReturnModeMsg.footer.swapCrc32ToProtocolEndianness(); + + // Set up destination address + struct sockaddr_in deviceAddr; + deviceAddr.sin_family = AF_INET; + deviceAddr.sin_addr.s_addr = inet_addr( + device.discoveredDevice.ipAddr.c_str()); + deviceAddr.sin_port = htons(65000); // Commands go to port 65000 + + // Send set return mode message + ssize_t bytesSent = sendto( + cmdEndpointFdDesc->native_handle(), + &setReturnModeMsg, sizeof(setReturnModeMsg), 0, + (struct sockaddr*)&deviceAddr, sizeof(deviceAddr)); + + if (bytesSent < 0) + { + std::cerr << __func__ << ": Failed to send set return mode message: " + << strerror(errno) << std::endl; + return false; + } + + std::cout << __func__ << ": Sent set return mode message to " + << device.discoveredDevice.ipAddr << ":" << 65000 << std::endl; + return true; + } +}; + +// GetReturnModeReq continuation class +class Device::GetReturnModeReq +: public smo::NonPostedAsynchronousContinuation +{ +public: + enum class SocketState + { + SOCKET_STILL_WAITING = 0, + SOCKET_ERROR, + SOCKET_RECV_SUCCESS, + SOCKET_RECV_ERROR + }; + +public: + Device& device; + + // Atomic state flags for async coordination + std::atomic timerFired{false}; + std::atomic socketState{SocketState::SOCKET_STILL_WAITING}; + std::atomic handlerExecuted{false}; + + // The timeout timer. + boost::asio::deadline_timer timeoutTimer; + + // Received data storage + uint8_t responseBuffer[1024]{}; + ssize_t bytesReceived = -1; + struct sockaddr_in senderAddr; + socklen_t senderAddrLen = sizeof(senderAddr); + +public: + friend void Device::getReturnModeReq( + smo::Callback callback); + + GetReturnModeReq( + Device& dev, + smo::Callback cb) + : smo::NonPostedAsynchronousContinuation( + std::move(cb)), + device(dev), + timeoutTimer(device.componentThread->getIoService()) + {} + + virtual ~GetReturnModeReq() + { + cleanup(); + } + + // Public accessor for the original callback + void callOriginalCallback(bool success, uint8_t returnMode) + { this->callOriginalCb(success, returnMode); } + + void callOriginalCallbackWithFailure() + { this->callOriginalCb(false, 0); } + + void setupAsyncCallbacks(std::shared_ptr request) + { + // Set up timeout timer + timeoutTimer.expires_from_now(boost::posix_time::milliseconds( + device.handshakeTimeoutMs)); + + timeoutTimer.async_wait( + std::bind(&GetReturnModeReq::getReturnModeReq1_1, + this, request, + std::placeholders::_1)); + + // Register UDP command handler for get return mode response (cmd_set=0x01, cmd_id=0x07) + device.registerUdpCommandHandler( + 0x01, 0x07, + std::bind(&GetReturnModeReq::getReturnModeReq1_2, + this, request, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + device.discoveredDevice.ipAddr); + } + + void getReturnModeReq1_1( + std::shared_ptr context, + const boost::system::error_code& error + ) + { + if (error == boost::asio::error::operation_aborted) { + // Timer was cancelled, ignore + return; + } + + context->timerFired.store(true); + context->getReturnModeReq2(context); + } + + void getReturnModeReq1_2( + std::shared_ptr context, + const uint8_t* data, ssize_t bytesReceived, + const struct sockaddr_in& senderAddr + ) + { + // Store the received data + context->bytesReceived = bytesReceived; + context->senderAddr = senderAddr; + context->senderAddrLen = sizeof(senderAddr); + + // Copy the data to our buffer + if (bytesReceived > 0 + && bytesReceived <= (ssize_t)sizeof(context->responseBuffer)) + { + memcpy(context->responseBuffer, data, bytesReceived); + context->socketState = SocketState::SOCKET_RECV_SUCCESS; + } else + { + context->socketState = SocketState::SOCKET_RECV_ERROR; + std::cerr << __func__ << ": Invalid data size: " << bytesReceived + << std::endl; + } + + context->getReturnModeReq2(context); + } + + void getReturnModeReq2( + std::shared_ptr context + ) + { + // Only execute once + if (context->handlerExecuted.exchange(true)) { return; } + + context->timeoutTimer.cancel(); + device.unregisterUdpCommandHandler( + 0x01, 0x07, device.discoveredDevice.ipAddr); + + SocketState finalSocketState = context->socketState.load(); + bool finalTimerFired = context->timerFired.load(); + + // Check for timeout only if there was no socket activity + if (finalTimerFired + && finalSocketState == SocketState::SOCKET_STILL_WAITING) + { + std::cerr << __func__ << ": Get return mode timeout with " + << device.discoveredDevice.ipAddr << "(" + << device.discoveredDevice.deviceIdentifier + << ")" << "\n"; + context->callOriginalCallbackWithFailure(); + return; + } + // Socket error from boost::asio + if (finalSocketState == SocketState::SOCKET_ERROR) + { + std::cerr << __func__ << ": Socket error during get return mode with " + << device.discoveredDevice.ipAddr << std::endl; + context->callOriginalCallbackWithFailure(); + return; + } + if (finalSocketState == SocketState::SOCKET_RECV_ERROR) + { + std::cerr << __func__ << ": Receive error during get return mode with " + << device.discoveredDevice.ipAddr << std::endl; + context->callOriginalCallbackWithFailure(); + return; + } + /* Result must have been RECV_SUCCESS state if we reach here. + * Data was already read in the async callback, just validate it + */ + if (context->bytesReceived + < static_cast(sizeof(comms::GetLiDARReturnModeResponse))) + { + std::cerr << __func__ << ": Response of size " + << context->bytesReceived << " too small from " + << device.discoveredDevice.ipAddr << std::endl; + context->callOriginalCallbackWithFailure(); + return; + } + + comms::GetLiDARReturnModeResponse* response = + reinterpret_cast( + context->responseBuffer); + response->swapContentsToHostEndianness(); + + // Check if response indicates success + if (!(response->command.cmd_set == 0x01 && + response->command.cmd_id == 0x07 && + response->ret_code == 0x00)) + { + context->callOriginalCallbackWithFailure(); + return; + } + + context->callOriginalCallback(true, response->mode); + } + + void cleanup() + { + timeoutTimer.cancel(); + } + + bool sendCommand() + { + // Get the command endpoint from the UdpCommandDemuxer + auto& protoState = livoxProto1::getProtoState(); + if (!protoState.deviceManager) + { + std::cerr << __func__ << ": No device manager available.\n"; + return false; + } + + auto cmdEndpointFdDesc = protoState.deviceManager->udpCommandDemuxer + .getCmdEndpointFdDesc(); + if (!cmdEndpointFdDesc) + { + std::cerr << __func__ << ": No command endpoint available.\n"; + return false; + } + + // Create get return mode message + comms::GetLiDARReturnMode getReturnModeMsg; + getReturnModeMsg.swapContentsToProtocolEndianness(); + getReturnModeMsg.header.setCrc16FromRawBytes(); + getReturnModeMsg.header.swapCrc16ToProtocolEndianness(); + getReturnModeMsg.footer.crc_32 = getReturnModeMsg.calculateCrc32(); + getReturnModeMsg.footer.swapCrc32ToProtocolEndianness(); + + // Set up destination address + struct sockaddr_in deviceAddr; + deviceAddr.sin_family = AF_INET; + deviceAddr.sin_addr.s_addr = inet_addr(device.discoveredDevice.ipAddr.c_str()); + deviceAddr.sin_port = htons(65000); // Commands go to port 65000 + + // Send get return mode message + ssize_t bytesSent = sendto( + cmdEndpointFdDesc->native_handle(), + &getReturnModeMsg, sizeof(getReturnModeMsg), 0, + (struct sockaddr*)&deviceAddr, sizeof(deviceAddr)); + + if (bytesSent < 0) + { + std::cerr << __func__ << ": Failed to send get return mode message: " + << strerror(errno) << std::endl; + return false; + } + + std::cout << __func__ << ": Sent get return mode message to " + << device.discoveredDevice.ipAddr << ":" << 65000 << std::endl; + return true; + } +}; + +void Device::setReturnModeReq( + uint8_t returnMode, + smo::Callback callback + ) +{ + auto request = std::make_shared( + *this, returnMode, std::move(callback)); + + // Check if device IP is available + if (discoveredDevice.ipAddr.empty()) + { + std::cerr << __func__ << ": No device IP available for device " + << discoveredDevice.deviceIdentifier << std::endl; + request->callOriginalCallbackWithFailure(); + return; + } + + // Send the set return mode command + if (!request->sendCommand()) + { + request->callOriginalCallbackWithFailure(); + return; + } + + // Setup async callbacks + request->setupAsyncCallbacks(request); +} + +void Device::getReturnModeReq( + smo::Callback callback + ) +{ + auto request = std::make_shared( + *this, std::move(callback)); + + // Check if device IP is available + if (discoveredDevice.ipAddr.empty()) + { + std::cerr << __func__ << ": No device IP available for device " + << discoveredDevice.deviceIdentifier << std::endl; + request->callOriginalCallbackWithFailure(); + return; + } + + // Send the get return mode command + if (!request->sendCommand()) + { + request->callOriginalCallbackWithFailure(); + return; + } + + // Setup async callbacks + request->setupAsyncCallbacks(request); +} + } // namespace livoxProto1 diff --git a/commonLibs/livoxProto1/device.h b/commonLibs/livoxProto1/device.h index 48c876f..8bba636 100644 --- a/commonLibs/livoxProto1/device.h +++ b/commonLibs/livoxProto1/device.h @@ -110,6 +110,8 @@ private: class DisconnectReq; class EnablePcloudDataReq; class DisablePcloudDataReq; + class SetReturnModeReq; + class GetReturnModeReq; public: // Utility methods @@ -127,6 +129,9 @@ public: typedef std::function disconnectReqCbFn; typedef std::function enablePcloudDataReqCbFn; typedef std::function disablePcloudDataReqCbFn; + typedef std::function setReturnModeReqCbFn; + typedef std::function + getReturnModeReqCbFn; // Async connection methods void connectReq(smo::Callback callback); @@ -140,6 +145,9 @@ public: void disconnectReq(smo::Callback callback); void enablePcloudDataReq(smo::Callback callback); void disablePcloudDataReq(smo::Callback callback); + void setReturnModeReq( + uint8_t returnMode, smo::Callback callback); + void getReturnModeReq(smo::Callback callback); // Heartbeat state std::unique_ptr heartbeatTimer;