From 7a7ec912aa883f950db9ecbb2dafe81867fb16e9 Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Fri, 25 Oct 2024 14:52:38 +0200 Subject: [PATCH 01/10] Add TCP transport bases on WifiClient API --- lib/APPTurtle/src/CustomRosTransport.h | 188 +-------------- lib/APPTurtle/src/MicroRosClient.cpp | 10 +- .../src/Transports/CustomRosTransportBase.cpp | 128 ++++++++++ .../src/Transports/CustomRosTransportBase.h | 220 +++++++++++++++++ .../src/Transports/CustomRosTransportTcp.cpp | 225 ++++++++++++++++++ .../src/Transports/CustomRosTransportTcp.h | 147 ++++++++++++ .../CustomRosTransportUdp.cpp} | 63 +---- .../src/Transports/CustomRosTransportUdp.h | 127 ++++++++++ lib/WiFiNative/src/WiFiClient.h | 148 ++++++++++++ lib/WiFiNative/src/WifiClient.cpp | 174 ++++++++++++++ platformio.ini | 6 +- 11 files changed, 1194 insertions(+), 242 deletions(-) create mode 100644 lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp create mode 100644 lib/APPTurtle/src/Transports/CustomRosTransportBase.h create mode 100644 lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp create mode 100644 lib/APPTurtle/src/Transports/CustomRosTransportTcp.h rename lib/APPTurtle/src/{CustomRosTransport.cpp => Transports/CustomRosTransportUdp.cpp} (75%) create mode 100644 lib/APPTurtle/src/Transports/CustomRosTransportUdp.h create mode 100644 lib/WiFiNative/src/WiFiClient.h create mode 100644 lib/WiFiNative/src/WifiClient.cpp diff --git a/lib/APPTurtle/src/CustomRosTransport.h b/lib/APPTurtle/src/CustomRosTransport.h index abf4ba09..e0076882 100644 --- a/lib/APPTurtle/src/CustomRosTransport.h +++ b/lib/APPTurtle/src/CustomRosTransport.h @@ -25,8 +25,8 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Custom Micro-ROS transport. - * @author Gabryel Reyes + * @brief Custom Micro-ROS transport class selector + * @author Norbert Schulz * * @addtogroup Application * @@ -38,185 +38,11 @@ /****************************************************************************** * Compile Switches *****************************************************************************/ - -/****************************************************************************** - * Includes - *****************************************************************************/ -#include -#include -#include - -/****************************************************************************** - * Macros - *****************************************************************************/ - -/****************************************************************************** - * Types and Classes - *****************************************************************************/ - -/** - * Micro-ROS custom transport adaption. - * - * The static functions are used as these are called from C-language - */ -class CustomRosTransport -{ -public: - /** - * Constructs a custom Micro-ROS transport. - */ - CustomRosTransport() : m_udpClient(), m_address(), m_port(DEFAULT_PORT) - { - } - - /** - * Destroys custom Micro-ROS transport. - * - */ - ~CustomRosTransport() - { - } - - /** - * Initialize custom ROS transport with agent address and port. - * - * @param[in] addr Micro-ROS agent IP-address - * @param[in] port Micro-ROS agent port - */ - void init(const IPAddress& addr, uint16_t port) - { - m_address = addr; - m_port = port; - } - - /** - * Get IP-address of Micro-ROS agent. - * - * @return IP-address - */ - const IPAddress& getIPAddress() const - { - return m_address; - } - - /** - * Get IP-address of Micro-ROS agent as string - * - * @return IP-address as string - */ - const String getIPAddressAsStr() const - { - return m_address.toString(); - } - - /** - * Get port of Micro-ROS agent. - * - * @return Port - */ - uint16_t getPort() const - { - return m_port; - } - - /** - * Open and initialize the custom transport (C-Entry Point). - * https://micro.ros.org/docs/tutorials/advanced/create_custom_transports/ - * - * @param[in] transport The arguments passed through uxr_init_custom_transport. - * - * @return A boolean indicating if the opening was successful. - */ - static bool open(uxrCustomTransport* transport); - - /** - * Close the custom transport (C-Entry Point). - * https://micro.ros.org/docs/tutorials/advanced/create_custom_transports/ - * - * @param[in] transport The arguments passed through uxr_init_custom_transport. - * - * @return A boolean indicating if the closing was successful. - */ - static bool close(uxrCustomTransport* transport); - - /** - * Write data to the custom transport (C-Entry Point). - * https://micro.ros.org/docs/tutorials/advanced/create_custom_transports/ - * - * @param[in] transport The arguments passed through uxr_init_custom_transport. - * @param[in] buffer The buffer to write. - * @param[in] size The size of the buffer. - * @param[out] errorCode The error code. - * - * @return The number of bytes written. - */ - static size_t write(uxrCustomTransport* transport, const uint8_t* buffer, size_t size, uint8_t* errorCode); - - /** - * Read data from the custom transport (C-Entry Point). - * https://micro.ros.org/docs/tutorials/advanced/create_custom_transports/ - * - * @param[in] transport The arguments passed through uxr_init_custom_transport. - * @param[out] buffer The buffer to read into. - * @param[in] size The size of the buffer. - * @param[in] timeout The timeout in milliseconds. - * @param[out] errorCode The error code. - * - * @return The number of bytes read. - */ - static size_t read(uxrCustomTransport* transport, uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode); - -private: - /** - * Open and initialize the custom transport. - * - * @return A boolean indicating if the opening was successful. - */ - bool open(void); - - /** - * Close the custom transport. - * - * @return A boolean indicating if the closing was successful. - */ - bool close(void); - - /** - * Write data to the custom transport. - * - * @param[in] buffer The buffer to write. - * @param[in] size The size of the buffer. - * @param[out] errorCode The error code. - * - * @return The number of bytes written. - */ - size_t write(const uint8_t* buffer, size_t size, uint8_t* errorCode); - - /** - * Read data from the custom transport. - * - * @param[out] buffer The buffer to read into. - * @param[in] size The size of the buffer. - * @param[in] timeout The timeout in milliseconds. - * @param[out] errorCode The error code. - * - * @return The number of bytes read. - */ - size_t read(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode); - - /** - * Default Micro-ROS agent port. - */ - static const int DEFAULT_PORT = 8888; - - WiFiUDP m_udpClient; /**< UDP client */ - IPAddress m_address; /**< IP address of the Micro-ROS agent */ - uint16_t m_port; /**< Port of the Micro-ROS agent */ -}; - -/****************************************************************************** - * Functions - *****************************************************************************/ +#if defined(TRANSPORT_USE_TCP) && TRANSPORT_USE_TCP != 0 +#include "Transports/CustomRosTransportTcp.h" +#else +#include "Transports/CustomRosTransportUdp.h" +#endif /* else defined(TRANSPORT_USE_TCP) && TRANSPORT_USE_TCP != 0 */ #endif /* CUSTOM_ROS_TRANSPORT_H */ /** @} */ diff --git a/lib/APPTurtle/src/MicroRosClient.cpp b/lib/APPTurtle/src/MicroRosClient.cpp index 1f802aec..b6a3e489 100644 --- a/lib/APPTurtle/src/MicroRosClient.cpp +++ b/lib/APPTurtle/src/MicroRosClient.cpp @@ -171,9 +171,13 @@ bool MicroRosClient::setupCustomTransport(const IPAddress& ipAddress, uint16_t p m_customRosTransport.init(ipAddress, port); - if (RCL_RET_OK != rmw_uros_set_custom_transport(false, (void*)&m_customRosTransport, CustomRosTransport::open, - CustomRosTransport::close, CustomRosTransport::write, - CustomRosTransport::read)) + if (RCL_RET_OK != rmw_uros_set_custom_transport( + false, + &m_customRosTransport, + CustomRosTransportBase::open, + CustomRosTransportBase::close, + CustomRosTransportBase::write, + CustomRosTransportBase::read)) { LOG_ERROR("Failed to set custom transport for Micro-ROS."); } diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp new file mode 100644 index 00000000..1344576c --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp @@ -0,0 +1,128 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Custom Transport class with C-language interface for microros. + * @author Norbert Schulz + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "CustomRosTransportBase.h" + +#include +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/** + * Unwrap pointer to CustomRosTransportBase from uxrCustomTransport structure. + * + * This is used by the static C-Languge entry points to forward the request + * to the matching C++ member function. + * + * @param[in] transport The custom transport data structure pointer. + * + * @return The this pointer to transport owning CustomRosTransportBase class. + */ +static inline CustomRosTransportBase* toThis(const uxrCustomTransport* transport); + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +bool CustomRosTransportBase::open(uxrCustomTransport* transport) +{ + CustomRosTransportBase * tp = toThis(transport); + return (nullptr != tp) ? tp->open() : false; +} + +bool CustomRosTransportBase::close(uxrCustomTransport* transport) +{ + CustomRosTransportBase * tp = toThis(transport); + return (nullptr != tp) ? tp->close() : false; +} + +size_t CustomRosTransportBase::write(uxrCustomTransport* transport, const uint8_t* buffer, size_t size, uint8_t* errorCode) +{ + CustomRosTransportBase * tp = toThis(transport); + return (nullptr != tp) ? tp->write(buffer, size, errorCode) : 0U; +} + +size_t CustomRosTransportBase::read(uxrCustomTransport* transport, uint8_t* buffer, size_t size, int timeout, + uint8_t* errorCode) +{ + CustomRosTransportBase * tp = toThis(transport); + return (nullptr != tp) ? tp->read(buffer, size, timeout, errorCode) : 0U; +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + +static inline CustomRosTransportBase* toThis(const uxrCustomTransport* transport) +{ + CustomRosTransportBase* transportClass = nullptr; + + if (nullptr == transport) + { + LOG_FATAL("Invalid uxrCustomTransport pointer."); + } + else + { + transportClass = reinterpret_cast(transport->args); + } + + return transportClass; +} \ No newline at end of file diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportBase.h b/lib/APPTurtle/src/Transports/CustomRosTransportBase.h new file mode 100644 index 00000000..49562abd --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportBase.h @@ -0,0 +1,220 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Custom Transport class with C-language interface for microros. + * @author Norbert Schulz + * + * @addtogroup Application + * + * @{ + */ +#ifndef CUSTOM_ROS_TRANSPORT_BASE_H +#define CUSTOM_ROS_TRANSPORT_BASE_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * Micro-ROS custom transport adaption. + * + * The static functions are needed as these are called from C-language. + */ +class CustomRosTransportBase +{ +public: + /** + * Constructs a custom Micro-ROS transport. + */ + CustomRosTransportBase() : m_address(), m_port(DEFAULT_PORT) + { + } + + /** + * Destroys custom Micro-ROS transport. + * + */ + ~CustomRosTransportBase() + { + } + + /** + * Initialize custom ROS transport with agent address and port. + * + * @param[in] addr Micro-ROS agent IP-address + * @param[in] port Micro-ROS agent port + */ + void init(const IPAddress& addr, uint16_t port) + { + m_address = addr; + m_port = port; + } + + /** + * Get IP-address of Micro-ROS agent. + * + * @return IP-address + */ + const IPAddress& getIPAddress() const + { + return m_address; + } + + /** + * Get IP-address of Micro-ROS agent as string + * + * @return IP-address as string + */ + const String getIPAddressAsStr() const + { + return m_address.toString(); + } + + /** + * Get port of Micro-ROS agent. + * + * @return Port + */ + uint16_t getPort() const + { + return m_port; + } + + /** + * Open and initialize the custom transport (C-Entry Point). + * https://micro.ros.org/docs/tutorials/advanced/create_custom_transports/ + * + * @param[in] transport The arguments passed through uxr_init_custom_transport. + * + * @return A boolean indicating if the opening was successful. + */ + static bool open(uxrCustomTransport* transport); + + /** + * Close the custom transport (C-Entry Point). + * https://micro.ros.org/docs/tutorials/advanced/create_custom_transports/ + * + * @param[in] transport The arguments passed through uxr_init_custom_transport. + * + * @return A boolean indicating if the closing was successful. + */ + static bool close(uxrCustomTransport* transport); + + /** + * Write data to the custom transport (C-Entry Point). + * https://micro.ros.org/docs/tutorials/advanced/create_custom_transports/ + * + * @param[in] transport The arguments passed through uxr_init_custom_transport. + * @param[in] buffer The buffer to write. + * @param[in] size The size of the buffer. + * @param[out] errorCode The error code. + * + * @return The number of bytes written. + */ + static size_t write(uxrCustomTransport* transport, const uint8_t* buffer, size_t size, uint8_t* errorCode); + + /** + * Read data from the custom transport (C-Entry Point). + * https://micro.ros.org/docs/tutorials/advanced/create_custom_transports/ + * + * @param[in] transport The arguments passed through uxr_init_custom_transport. + * @param[out] buffer The buffer to read into. + * @param[in] size The size of the buffer. + * @param[in] timeout The timeout in milliseconds. + * @param[out] errorCode The error code. + * + * @return The number of bytes read. + */ + static size_t read(uxrCustomTransport* transport, uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode); + +protected: + /** + * Open and initialize the custom transport. + * + * @return A boolean indicating if the opening was successful. + */ + virtual bool open(void) = 0; + + /** + * Close the custom transport. + * + * @return A boolean indicating if the closing was successful. + */ + virtual bool close(void) = 0; + + /** + * Write data to the custom transport. + * + * @param[in] buffer The buffer to write. + * @param[in] size The size of the buffer. + * @param[out] errorCode The error code. + * + * @return The number of bytes written. + */ + virtual size_t write(const uint8_t* buffer, size_t size, uint8_t* errorCode) = 0; + + /** + * Read data from the custom transport. + * + * @param[out] buffer The buffer to read into. + * @param[in] size The size of the buffer. + * @param[in] timeout The timeout in milliseconds. + * @param[out] errorCode The error code. + * + * @return The number of bytes read. + */ + virtual size_t read(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) = 0; + + /** + * Default Micro-ROS agent port. + */ + static const int DEFAULT_PORT = 8888; + + IPAddress m_address; /**< IP address of the Micro-ROS agent. */ + uint16_t m_port; /**< Port of the Micro-ROS agent. */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* CUSTOM_ROS_TRANSPORT_BASE_H */ +/** @} */ diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp new file mode 100644 index 00000000..76978f63 --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp @@ -0,0 +1,225 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Custom Micro-ROS transport using TCP over Arduino WifiClient. + * @author Norbert Schulz + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "CustomRosTransportTcp.h" + +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool CustomRosTransportTcp::open(void) +{ + bool isOpen = false; + + if (!m_tcpClient.connect(m_address, m_port)) + { + LOG_ERROR("TCP connect error"); + } + else + { + isOpen = true; + } + + return isOpen; +} + +bool CustomRosTransportTcp::close() +{ + m_tcpClient.stop(); + return true; +} + +size_t CustomRosTransportTcp::write(const uint8_t* buffer, size_t size, uint8_t* errorCode) +{ + size_t sent = 0U; + + if ((nullptr == buffer) || (0U == size) || (nullptr == errorCode)) + { + LOG_ERROR("One or more parameters are invalid."); + } + else + { + uint8_t lengthPrefix[0]; + lengthPrefix[0] = static_cast(size & 0xFF); + lengthPrefix[1] = static_cast((size>>8) & 0xFF); + + m_tcpClient.write(lengthPrefix, 2); + if ((sent = m_tcpClient.write(buffer, size)) != size) + { + LOG_ERROR("Write error (size=%zu, sent=%zu)", size, sent); + * errorCode = 1; + } + else + { + * errorCode = 0; + } + } + + LOG_INFO("Write %zu bytes", size); +{ + String x; + for (int i=0; i < size; ++i) + { + char buf[20]; + sprintf(buf, "%02x ", buffer[i]); + x += buf; + } + LOG_INFO("%s", x.c_str()); +} + + return sent; +} + +size_t CustomRosTransportTcp::read(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) +{ + size_t readBytes = 0U; + + if ((nullptr == buffer) || (0U == size) || (nullptr == errorCode)) + { + LOG_ERROR("One or more parameters are invalid."); + } + else + { + uint8_t lengthPrefix[2]; /* The first 2 bytes hold the payload length that follows. */ + + * errorCode = 1; + + if (readFixedLength(lengthPrefix, sizeof(lengthPrefix), nullptr, timeout)) /* size prefix */ + { + size_t payloadLen; + payloadLen = static_cast(lengthPrefix[0]) + (static_cast(lengthPrefix[1]) >> 8); + + if (payloadLen > size) + { + LOG_ERROR("record length %zu exceeds read buffer size %zu.", payloadLen, size); + } + else + { + if (readFixedLength(buffer, payloadLen, &readBytes, timeout)) /* payload */ + { + * errorCode = 0; + } + } + } + } +LOG_INFO("Received %zu bytes", readBytes); +{ + String x; + for (int i=0; i < readBytes; ++i) + { + char buf[20]; + sprintf(buf, "%02x ", buffer[i]); + x += buf; + } + LOG_INFO("%s", x.c_str()); +} + return readBytes; +} + +bool CustomRosTransportTcp::readFixedLength(uint8_t* buffer, size_t length, size_t * readCount, int timeout) +{ + size_t remaining = length; + + SimpleTimer readTimer; + readTimer.start(timeout); + + while ((0 < remaining) && (false == readTimer.isTimeout())) + { + int count = m_tcpClient.read(buffer, remaining); + if (-1 == count) + { + break; + } + + remaining -= count; + buffer += count; + +#if defined(TARGET_NATIVE) + /* + * SimpleTimer requires simulation time to progress + * otherwise this loop will block until a packet was received. + * TODO This is a workaround which needs to be discussed further. + */ + Board::getInstance().stepTime(); +#endif + } + + if (nullptr != readCount) + { + * readCount = length - remaining; + } + + return remaining ? false : true; +} + + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h new file mode 100644 index 00000000..f1338f2c --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h @@ -0,0 +1,147 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Custom Micro-ROS transport using TCP over Arduino WifiClient. + * @author Norbert Schulz + * + * @addtogroup Application + * + * @{ + */ +#ifndef CUSTOM_ROS_TRANSPORT_TCP_H +#define CUSTOM_ROS_TRANSPORT_TCP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "CustomRosTransportBase.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ +typedef class CustomRosTransportTcp CustomRosTransport; + +/** + * Micro-ROS custom transport adaption for TCP. + * + * TCP protocol uses a record format with a 16 bit length field before the + * payload. + * + * +----------------+----------------+-----..........------+ + * | 1 Byte | 1 Bytes | "Lenght" Bytes | + * +----------------+----------------+-----..........------+ + * | Length [0..7] | Length [8..15] | < payload > | + * +----------------+----------------+-----..........------+ + */ +class CustomRosTransportTcp : public CustomRosTransportBase +{ +public: + /** + * Constructs a custom Micro-ROS transport. + */ + CustomRosTransportTcp() : CustomRosTransportBase(), m_tcpClient() + { + } + + /** + * Destroys custom Micro-ROS transport. + * + */ + virtual ~CustomRosTransportTcp() final + { + } + +private: + /** + * Open and initialize the custom transport. + * + * @return A boolean indicating if the opening was successful. + */ + bool open(void) final; + + /** + * Close the custom transport. + * + * @return A boolean indicating if the closing was successful. + */ + bool close(void) final; + + /** + * Write data to the custom transport. + * + * @param[in] buffer The buffer to write. + * @param[in] size The size of the buffer. + * @param[out] errorCode The error code. + * + * @return The number of bytes written. + */ + size_t write(const uint8_t* buffer, size_t size, uint8_t* errorCode) final; + + /** + * Read data from the custom transport. + * + * @param[out] buffer The buffer to read into. + * @param[in] size The size of the buffer. + * @param[in] timeout The timeout in milliseconds. + * @param[out] errorCode The error code. + * + * @return The number of bytes read. + */ + size_t read(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) final; + + /** + * Try to read a fixed number of bytes. + * + * @param[out] buffer The buffer to read into. + * @param[in] size The size of the buffer. + * @param[in] readCount return bytes read if not a nullptr. + * @param[in] timeout The timeout in milliseconds. + * + * @return true of all requested bytes received. + */ + bool readFixedLength(uint8_t* buffer, size_t length, size_t * readCount, int timeout); + +private: + WiFiClient m_tcpClient; /**< TCP client */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* CUSTOM_ROS_TRANSPORT_TCP_H */ +/** @} */ diff --git a/lib/APPTurtle/src/CustomRosTransport.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp similarity index 75% rename from lib/APPTurtle/src/CustomRosTransport.cpp rename to lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp index 6abdb2c6..c89b7bae 100644 --- a/lib/APPTurtle/src/CustomRosTransport.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp @@ -32,7 +32,8 @@ /****************************************************************************** * Includes *****************************************************************************/ -#include "CustomRosTransport.h" +#include "CustomRosTransportUdp.h" + #include #include #include @@ -53,17 +54,6 @@ * Prototypes *****************************************************************************/ -/** - * Unwrap pointer to CustomRosTransport from uxrCustomTransport structure. - * - * This is used by the static C-Languge entry points to forward the request - * to the matching C++ member function. - * - * @param[in] transport The custom transport data structure pointer. - * - * @return The this pointer to transport owning CustomRosTransport class. - */ -static inline CustomRosTransport* toThis(const uxrCustomTransport* transport); /****************************************************************************** * Local Variables @@ -73,31 +63,6 @@ static inline CustomRosTransport* toThis(const uxrCustomTransport* transport); * Public Methods *****************************************************************************/ -bool CustomRosTransport::open(uxrCustomTransport* transport) -{ - CustomRosTransport * tp = toThis(transport); - return (nullptr != tp) ? tp->open() : false; -} - -bool CustomRosTransport::close(uxrCustomTransport* transport) -{ - CustomRosTransport * tp = toThis(transport); - return (nullptr != tp) ? tp->close() : false; -} - -size_t CustomRosTransport::write(uxrCustomTransport* transport, const uint8_t* buffer, size_t size, uint8_t* errorCode) -{ - CustomRosTransport * tp = toThis(transport); - return (nullptr != tp) ? tp->write(buffer, size, errorCode) : 0U; -} - -size_t CustomRosTransport::read(uxrCustomTransport* transport, uint8_t* buffer, size_t size, int timeout, - uint8_t* errorCode) -{ - CustomRosTransport * tp = toThis(transport); - return (nullptr != tp) ? tp->read(buffer, size, timeout, errorCode) : 0U; -} - /****************************************************************************** * Protected Methods *****************************************************************************/ @@ -106,7 +71,7 @@ size_t CustomRosTransport::read(uxrCustomTransport* transport, uint8_t* buffer, * Private Methods *****************************************************************************/ -bool CustomRosTransport::open(void) +bool CustomRosTransportUdp::open(void) { bool isOpen = false; const int UDP_OK = 1; @@ -123,14 +88,14 @@ bool CustomRosTransport::open(void) return isOpen; } -bool CustomRosTransport::close() +bool CustomRosTransportUdp::close() { m_udpClient.stop(); return true; } -size_t CustomRosTransport::write(const uint8_t* buffer, size_t size, uint8_t* errorCode) +size_t CustomRosTransportUdp::write(const uint8_t* buffer, size_t size, uint8_t* errorCode) { size_t sent = 0U; @@ -185,7 +150,7 @@ size_t CustomRosTransport::write(const uint8_t* buffer, size_t size, uint8_t* er return sent; } -size_t CustomRosTransport::read(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) +size_t CustomRosTransportUdp::read(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) { size_t readBytes = 0U; @@ -236,19 +201,3 @@ size_t CustomRosTransport::read(uint8_t* buffer, size_t size, int timeout, uint8 /****************************************************************************** * Local Functions *****************************************************************************/ - -static inline CustomRosTransport* toThis(const uxrCustomTransport* transport) -{ - CustomRosTransport* transportClass = nullptr; - - if (nullptr == transport) - { - LOG_FATAL("Invalid uxrCustomTransport pointer."); - } - else - { - transportClass = reinterpret_cast(transport->args); - } - - return transportClass; -} diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h new file mode 100644 index 00000000..dae322d5 --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h @@ -0,0 +1,127 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Custom Micro-ROS transport using UDP over Arduino WiFiUdp. + * @author Gabryel Reyes + * + * @addtogroup Application + * + * @{ + */ +#ifndef CUSTOM_ROS_TRANSPORT_UDP_H +#define CUSTOM_ROS_TRANSPORT_UDP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "CustomRosTransportBase.h" + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ +typedef class CustomRosTransportUdp CustomRosTransport; + +/** + * Micro-ROS custom transport adaption. + * + * The static functions are used as these are called from C-language + */ +class CustomRosTransportUdp : public CustomRosTransportBase +{ +public: + /** + * Constructs a custom Micro-ROS transport. + */ + CustomRosTransportUdp() : CustomRosTransportBase(), m_udpClient() + { + } + + /** + * Destroys custom Micro-ROS transport. + * + */ + virtual ~CustomRosTransportUdp() final + { + } + +private: + /** + * Open and initialize the custom transport. + * + * @return A boolean indicating if the opening was successful. + */ + bool open(void) final; + + /** + * Close the custom transport. + * + * @return A boolean indicating if the closing was successful. + */ + bool close(void) final; + + /** + * Write data to the custom transport. + * + * @param[in] buffer The buffer to write. + * @param[in] size The size of the buffer. + * @param[out] errorCode The error code. + * + * @return The number of bytes written. + */ + size_t write(const uint8_t* buffer, size_t size, uint8_t* errorCode) final; + + /** + * Read data from the custom transport. + * + * @param[out] buffer The buffer to read into. + * @param[in] size The size of the buffer. + * @param[in] timeout The timeout in milliseconds. + * @param[out] errorCode The error code. + * + * @return The number of bytes read. + */ + size_t read(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) final; + + WiFiUDP m_udpClient; /**< UDP client */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* CUSTOM_ROS_TRANSPORT_UDP_H */ +/** @} */ diff --git a/lib/WiFiNative/src/WiFiClient.h b/lib/WiFiNative/src/WiFiClient.h new file mode 100644 index 00000000..9bdaf9c5 --- /dev/null +++ b/lib/WiFiNative/src/WiFiClient.h @@ -0,0 +1,148 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief Wifi Client for Native platform. Emulation of Arduino WiFiClient. + * @author Norbert Schulz + * + * @addtogroup HALSim + * + * @{ + */ +#ifndef WIFI_CLIENT_H +#define WIFI_CLIENT_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include +#include + +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** + * WiFiClient client class compatible with Arduino. + * + * Only a subset of API functions is supported, based on actual demand. + */ +class WiFiClient +{ + public: + + /** + * Default constructor. + */ + WiFiClient() : + m_socket(SOCK_INVAL), + m_servaddr() + { + } + + /** + * Default Destructor. + */ + virtual ~WiFiClient() + { + } + + /** + * Establish a connection. + * + * @param[in] addr Port number. + * @param[in] port Port number. + * @return If successful returns 1, otherwise 0. + */ + uint8_t connect(const IPAddress& addr, uint16_t port); + + /** + * Indicate if client is connected. + * + * @return If successful returns 1, otherwise 0. + */ + uint8_t connected() + { + return (SOCK_INVAL != m_socket) ? 1U : 0U; + } + + /** + * Stop the WiF Client. + */ + void stop(); + + /** + * Write bytes to stream. + * + * @param[in] buffer Byte Array to send. + * @param[in] size Length of Buffer. + * @returns Number of bytes written + */ + size_t write(const uint8_t* buffer, size_t size); + + + /** + * Check if there are available bytes in the Stream. + * + * @returns Number of available bytes. + */ + int available() const; + + /** + * Read bytes from the incoming packet into a buffer. + * + * @param[out] buffer Buffer to read into. + * @param[in] size Length of the buffer. + * + * @return Number of bytes read or -1 on error. + */ + int read(uint8_t* buffer, size_t size); + +private: + int m_socket; /**< Socket file descriptor. */ + struct sockaddr_in m_servaddr; /**< Server address. */ + + + static const int SOCK_INVAL = -1; /**< invalid socket value */ +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* WIFI_CLIENT_H */ +/** @} */ diff --git a/lib/WiFiNative/src/WifiClient.cpp b/lib/WiFiNative/src/WifiClient.cpp new file mode 100644 index 00000000..717d4af4 --- /dev/null +++ b/lib/WiFiNative/src/WifiClient.cpp @@ -0,0 +1,174 @@ +/* MIT License + * + * Copyright (c) 2023 - 2024 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @brief WifiClient class for ArduinoNative. + * @author Norbert Schulz + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "WiFiClient.h" + +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +uint8_t WiFiClient::connect(const IPAddress& addr, uint16_t port) +{ + uint8_t retval = 0; + + if (SOCK_INVAL != m_socket) + { + stop(); + } + + if (SOCK_INVAL != (m_socket = ::socket(AF_INET, SOCK_STREAM, IPPROTO_TCP))) + { + m_servaddr.sin_family = AF_INET; + m_servaddr.sin_addr.s_addr = htonl(addr); + m_servaddr.sin_port = htons(port); + + if (0 == ::connect( + m_socket, + reinterpret_cast(&m_servaddr), + sizeof(m_servaddr) )) + { + const int one = 1; + + ::setsockopt(m_socket, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); + ::fcntl(m_socket, F_SETFL, O_NONBLOCK); + retval = 1; /* success*/ + } + else + { + stop(); + } + } + + return retval; +} + +void WiFiClient::stop() +{ + if (SOCK_INVAL != m_socket) + { + ::shutdown(m_socket, SHUT_RDWR); + ::close(m_socket); + m_socket = SOCK_INVAL; + } +} + + +size_t WiFiClient::write(const uint8_t* buffer, size_t size) +{ + size_t remaining = size; + + if ((0 < size) && (SOCK_INVAL != m_socket)) + { + while (0 < remaining) { + ssize_t written = ::send(m_socket, buffer, remaining, 0); + + if (0 > written) /* error */ + { + break; + /* TODO: Signal error upwards - how ?*/ + } + + remaining -= written; + buffer += written; + } + } + + return size - remaining; +} + +int WiFiClient::available() const +{ + int count = 0; + + if (SOCK_INVAL != m_socket) + { + ioctl(m_socket, FIONREAD, &count); + } + + return count; +} + +int WiFiClient::read(uint8_t* buffer, size_t size) +{ + int retval = -1; + + + if (SOCK_INVAL != m_socket) + { + fd_set readFdSet; + FD_ZERO(&readFdSet); + FD_SET(m_socket, &readFdSet); + + struct timeval tv; + tv.tv_sec = 100 / 1000; + tv.tv_usec = (100 % 1000) * 1000; + + int selectRet = select(1, &readFdSet, NULL, NULL, &tv); + + ssize_t result = ::recv(m_socket, buffer, size, 0); + if (-1 == result) + { + if ((EAGAIN == errno) || (EWOULDBLOCK == errno)) + { + retval = 0; /* No error, just no data received yet. */ + } + } + else + { + retval = result; /* Success! */ + } + } + + return retval; +} diff --git a/platformio.ini b/platformio.ini index 4873778d..0cc3b882 100644 --- a/platformio.ini +++ b/platformio.ini @@ -58,7 +58,8 @@ lib_ignore = MainTestNative UDPNative WiFiNative -extra_scripts = + WifiClientNative +extra_scripts = monitor_speed = 115200 monitor_filters = esp32_exception_decoder @@ -72,6 +73,9 @@ monitor_filters = esp32_exception_decoder platform = native @ ~1.2.1 build_flags = -std=c++11 + -O0 + -ggdb + -D TRANSPORT_USE_TCP=0 -D TARGET_NATIVE -D LOG_DEBUG_ENABLE=1 -D CONFIG_LOG_SEVERITY=Logging::LOG_LEVEL_DEBUG From 780f0e98dc3ee76e9c8a87765bc00bad255c46f9 Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Tue, 29 Oct 2024 10:33:33 +0100 Subject: [PATCH 02/10] ROS2: added Transport::getProtocolName() Used to show if UDP or TCP protocol is used. --- lib/APPTurtle/src/Transports/CustomRosTransportBase.h | 6 ++++++ lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp | 8 ++++++++ lib/APPTurtle/src/Transports/CustomRosTransportTcp.h | 6 ++++++ lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp | 10 ++++++++++ lib/APPTurtle/src/Transports/CustomRosTransportUdp.h | 6 ++++++ 5 files changed, 36 insertions(+) diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportBase.h b/lib/APPTurtle/src/Transports/CustomRosTransportBase.h index 49562abd..a4e13085 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportBase.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportBase.h @@ -165,6 +165,12 @@ class CustomRosTransportBase */ static size_t read(uxrCustomTransport* transport, uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode); + /** + * Get protocol name used by this trandport. + * @return protocol name + */ + virtual const String& getProtocolName() const = 0; + protected: /** * Open and initialize the custom transport. diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp index 76978f63..705f456e 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp @@ -57,6 +57,10 @@ /****************************************************************************** * Local Variables *****************************************************************************/ +/** + * Name of implemented protocol. + */ +static const String gProtocolName("TCP"); /****************************************************************************** * Public Methods @@ -215,6 +219,10 @@ bool CustomRosTransportTcp::readFixedLength(uint8_t* buffer, size_t length, size return remaining ? false : true; } +const String& CustomRosTransportTcp::getProtocolName() const +{ + return gProtocolName; +} /****************************************************************************** * External Functions diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h index f1338f2c..086b111f 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h @@ -85,6 +85,12 @@ class CustomRosTransportTcp : public CustomRosTransportBase { } + /** + * Get protocol name used by this trandport. + * @return protocol name + */ + virtual const String& getProtocolName() const final; + private: /** * Open and initialize the custom transport. diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp index c89b7bae..55cb25ec 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp @@ -59,6 +59,11 @@ * Local Variables *****************************************************************************/ +/** + * Name of implemented protocol. + */ +static const String gProtocolName("UDP"); + /****************************************************************************** * Public Methods *****************************************************************************/ @@ -194,6 +199,11 @@ size_t CustomRosTransportUdp::read(uint8_t* buffer, size_t size, int timeout, ui return readBytes; } +const String& CustomRosTransportUdp::getProtocolName() const +{ + return gProtocolName; +} + /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h index dae322d5..c782f91f 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h @@ -78,6 +78,12 @@ class CustomRosTransportUdp : public CustomRosTransportBase { } + /** + * Get protocol name used by this trandport. + * @return protocol name + */ + virtual const String& getProtocolName() const final; + private: /** * Open and initialize the custom transport. From e048a5dc5e99dd5051d0d014cf348e35e97cabc6 Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Tue, 29 Oct 2024 17:20:01 +0100 Subject: [PATCH 03/10] ROS2 TCP: Use poll() to handle non blocking recv Solution is instable and highly timing sensitive. Operation is working for a few twist messages but ping breaks down unexpectingly. --- data/config/config.json | 2 +- lib/APPTurtle/src/MicroRosClient.cpp | 17 +++- lib/APPTurtle/src/MicroRosClient.h | 2 +- .../src/Transports/CustomRosTransportTcp.cpp | 34 +++----- lib/WiFiNative/src/WiFiClient.h | 15 ++-- lib/WiFiNative/src/WifiClient.cpp | 85 +++++++++---------- platformio.ini | 2 +- 7 files changed, 70 insertions(+), 87 deletions(-) diff --git a/data/config/config.json b/data/config/config.json index 5601d32b..5c45521a 100644 --- a/data/config/config.json +++ b/data/config/config.json @@ -27,6 +27,6 @@ }, "microROSAgent": { "host": "127.0.0.1", - "port": "8888" + "port": "8889" } } \ No newline at end of file diff --git a/lib/APPTurtle/src/MicroRosClient.cpp b/lib/APPTurtle/src/MicroRosClient.cpp index b6a3e489..de3f7b7b 100644 --- a/lib/APPTurtle/src/MicroRosClient.cpp +++ b/lib/APPTurtle/src/MicroRosClient.cpp @@ -288,7 +288,10 @@ void MicroRosClient::waitingForAgentState() if ((false == m_timer.isTimerRunning()) || (true == m_timer.isTimeout())) { String ipAddStr = m_customRosTransport.getIPAddressAsStr(); - LOG_INFO("Ping Micro-ROS agent %s:%u ...", ipAddStr.c_str(), m_customRosTransport.getPort()); + LOG_INFO("Ping Micro-ROS agent %s:%s:%u ...", + m_customRosTransport.getProtocolName().c_str(), + ipAddStr.c_str(), + m_customRosTransport.getPort()); if (RMW_RET_OK == rmw_uros_ping_agent(MICRO_ROS_AGENT_PING_TIMEOUT, MICRO_ROS_AGENT_PING_ATTEMPTS)) { @@ -311,7 +314,10 @@ void MicroRosClient::connectingState() { String ipAddStr = m_customRosTransport.getIPAddressAsStr(); - LOG_INFO("Connecting to Micro-ROS agent %s:%u ...", ipAddStr.c_str(), m_customRosTransport.getPort()); + LOG_INFO("Connecting to Micro-ROS agent %s:%s:%u ...", + m_customRosTransport.getProtocolName().c_str(), + ipAddStr.c_str(), + m_customRosTransport.getPort()); if (false == createEntities()) { @@ -319,8 +325,11 @@ void MicroRosClient::connectingState() } else { - LOG_INFO("Connected with Micro-ROS agent %s:%u.", ipAddStr.c_str(), m_customRosTransport.getPort()); - + LOG_INFO("Connected with Micro-ROS agent %s:%s:%u ...", + m_customRosTransport.getProtocolName().c_str(), + ipAddStr.c_str(), + m_customRosTransport.getPort()); + subscribe(); /* Periodically verify that the connection is still established. */ diff --git a/lib/APPTurtle/src/MicroRosClient.h b/lib/APPTurtle/src/MicroRosClient.h index 7a0093c4..45e4a501 100644 --- a/lib/APPTurtle/src/MicroRosClient.h +++ b/lib/APPTurtle/src/MicroRosClient.h @@ -118,7 +118,7 @@ class MicroRosClient /** * The Micro-ROS agent ping operation timeout is ms. */ - static const int32_t MICRO_ROS_AGENT_PING_TIMEOUT = 100; + static const int32_t MICRO_ROS_AGENT_PING_TIMEOUT = 200; /** * The Micro-ROS agent ping operation attempts. Keep 1, because the diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp index 705f456e..444e3ab7 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp @@ -93,6 +93,7 @@ bool CustomRosTransportTcp::open(void) bool CustomRosTransportTcp::close() { m_tcpClient.stop(); + LOG_DEBUG("tcp stream closed.\n"); return true; } @@ -122,18 +123,6 @@ size_t CustomRosTransportTcp::write(const uint8_t* buffer, size_t size, uint8_t* } } - LOG_INFO("Write %zu bytes", size); -{ - String x; - for (int i=0; i < size; ++i) - { - char buf[20]; - sprintf(buf, "%02x ", buffer[i]); - x += buf; - } - LOG_INFO("%s", x.c_str()); -} - return sent; } @@ -168,18 +157,8 @@ size_t CustomRosTransportTcp::read(uint8_t* buffer, size_t size, int timeout, ui } } } - } -LOG_INFO("Received %zu bytes", readBytes); -{ - String x; - for (int i=0; i < readBytes; ++i) - { - char buf[20]; - sprintf(buf, "%02x ", buffer[i]); - x += buf; } - LOG_INFO("%s", x.c_str()); -} + return readBytes; } @@ -188,9 +167,11 @@ bool CustomRosTransportTcp::readFixedLength(uint8_t* buffer, size_t length, size size_t remaining = length; SimpleTimer readTimer; + bool timeOver = false; + readTimer.start(timeout); - while ((0 < remaining) && (false == readTimer.isTimeout())) + while ((0 < remaining) && (false == timeOver)) { int count = m_tcpClient.read(buffer, remaining); if (-1 == count) @@ -209,8 +190,13 @@ bool CustomRosTransportTcp::readFixedLength(uint8_t* buffer, size_t length, size */ Board::getInstance().stepTime(); #endif + if (readTimer.isTimeout()) + { + timeOver = true; + } } + if (nullptr != readCount) { * readCount = length - remaining; diff --git a/lib/WiFiNative/src/WiFiClient.h b/lib/WiFiNative/src/WiFiClient.h index 9bdaf9c5..63aeede2 100644 --- a/lib/WiFiNative/src/WiFiClient.h +++ b/lib/WiFiNative/src/WiFiClient.h @@ -46,7 +46,7 @@ #include #include -#include +#include /****************************************************************************** * Macros @@ -59,7 +59,7 @@ /** * WiFiClient client class compatible with Arduino. * - * Only a subset of API functions is supported, based on actual demand. + * Only a subset of API functions is supported, based on the actual demand. */ class WiFiClient { @@ -69,8 +69,7 @@ class WiFiClient * Default constructor. */ WiFiClient() : - m_socket(SOCK_INVAL), - m_servaddr() + m_poll {SOCK_INVAL, POLLIN, 0} { } @@ -97,7 +96,7 @@ class WiFiClient */ uint8_t connected() { - return (SOCK_INVAL != m_socket) ? 1U : 0U; + return (SOCK_INVAL != m_poll.fd) ? 1U : 0U; } /** @@ -133,11 +132,9 @@ class WiFiClient int read(uint8_t* buffer, size_t size); private: - int m_socket; /**< Socket file descriptor. */ - struct sockaddr_in m_servaddr; /**< Server address. */ + struct pollfd m_poll; /**< The poll/socket structure. */ - - static const int SOCK_INVAL = -1; /**< invalid socket value */ + static const int SOCK_INVAL = -1; /**< The invalid socket value */ }; /****************************************************************************** diff --git a/lib/WiFiNative/src/WifiClient.cpp b/lib/WiFiNative/src/WifiClient.cpp index 717d4af4..17e3d50f 100644 --- a/lib/WiFiNative/src/WifiClient.cpp +++ b/lib/WiFiNative/src/WifiClient.cpp @@ -38,8 +38,8 @@ #include #include #include -#include #include +#include /****************************************************************************** * Macros @@ -62,26 +62,29 @@ uint8_t WiFiClient::connect(const IPAddress& addr, uint16_t port) { uint8_t retval = 0; - if (SOCK_INVAL != m_socket) + if (connected()) { stop(); } - if (SOCK_INVAL != (m_socket = ::socket(AF_INET, SOCK_STREAM, IPPROTO_TCP))) + if (SOCK_INVAL != (m_poll.fd = ::socket(AF_INET, SOCK_STREAM, IPPROTO_TCP))) { - m_servaddr.sin_family = AF_INET; - m_servaddr.sin_addr.s_addr = htonl(addr); - m_servaddr.sin_port = htons(port); + struct sockaddr_in serverAddr; + + memset(&serverAddr, 0, sizeof(serverAddr)); + serverAddr.sin_family = AF_INET; + serverAddr.sin_addr.s_addr = htonl(addr); + serverAddr.sin_port = htons(port); if (0 == ::connect( - m_socket, - reinterpret_cast(&m_servaddr), - sizeof(m_servaddr) )) + m_poll.fd, + reinterpret_cast(&serverAddr), + sizeof(serverAddr) )) { const int one = 1; - ::setsockopt(m_socket, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); - ::fcntl(m_socket, F_SETFL, O_NONBLOCK); + ::setsockopt(m_poll.fd, SOL_TCP, TCP_NODELAY, &one, sizeof(one)); + //::fcntl(m_socket, F_SETFL, O_NONBLOCK); retval = 1; /* success*/ } else @@ -95,11 +98,10 @@ uint8_t WiFiClient::connect(const IPAddress& addr, uint16_t port) void WiFiClient::stop() { - if (SOCK_INVAL != m_socket) + if (connected()) { - ::shutdown(m_socket, SHUT_RDWR); - ::close(m_socket); - m_socket = SOCK_INVAL; + ::close(m_poll.fd); + m_poll.fd = SOCK_INVAL; } } @@ -108,10 +110,10 @@ size_t WiFiClient::write(const uint8_t* buffer, size_t size) { size_t remaining = size; - if ((0 < size) && (SOCK_INVAL != m_socket)) + if ((0 < size) && connected()) { while (0 < remaining) { - ssize_t written = ::send(m_socket, buffer, remaining, 0); + ssize_t written = ::send(m_poll.fd, buffer, remaining, 0); if (0 > written) /* error */ { @@ -127,48 +129,37 @@ size_t WiFiClient::write(const uint8_t* buffer, size_t size) return size - remaining; } -int WiFiClient::available() const -{ - int count = 0; - - if (SOCK_INVAL != m_socket) - { - ioctl(m_socket, FIONREAD, &count); - } - - return count; -} - int WiFiClient::read(uint8_t* buffer, size_t size) { int retval = -1; - if (SOCK_INVAL != m_socket) + if (connected()) { - fd_set readFdSet; - FD_ZERO(&readFdSet); - FD_SET(m_socket, &readFdSet); - - struct timeval tv; - tv.tv_sec = 100 / 1000; - tv.tv_usec = (100 % 1000) * 1000; - - int selectRet = select(1, &readFdSet, NULL, NULL, &tv); - - ssize_t result = ::recv(m_socket, buffer, size, 0); - if (-1 == result) + if (-1 != ::poll(&m_poll, 1, 10)) { - if ((EAGAIN == errno) || (EWOULDBLOCK == errno)) + if (0 != (POLLIN & m_poll.revents)) { - retval = 0; /* No error, just no data received yet. */ + ssize_t result = ::recv(m_poll.fd, buffer, size, 0); + if (-1 != result) + { + retval = result; /* Success! */ + } + else + { + perror("recv"); + } } - } + else + { + retval = 0; + } + } else { - retval = result; /* Success! */ + perror("poll"); } } return retval; -} +} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 0cc3b882..891f6f51 100644 --- a/platformio.ini +++ b/platformio.ini @@ -75,7 +75,7 @@ build_flags = -std=c++11 -O0 -ggdb - -D TRANSPORT_USE_TCP=0 + -D TRANSPORT_USE_TCP=1 -D TARGET_NATIVE -D LOG_DEBUG_ENABLE=1 -D CONFIG_LOG_SEVERITY=Logging::LOG_LEVEL_DEBUG From e39d8eefddf77359fa797ba7891e6d7d91c0e9aa Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Wed, 30 Oct 2024 14:37:51 +0100 Subject: [PATCH 04/10] ROS2 TCP transport reading fixed A state machine was needed to read the record from fragmented socket stream reads. --- .../src/Transports/CustomRosTransportBase.cpp | 1 + .../src/Transports/CustomRosTransportTcp.cpp | 211 +++++++++++++++--- .../src/Transports/CustomRosTransportTcp.h | 128 +++++++++-- .../src/Transports/CustomRosTransportUdp.cpp | 10 +- .../src/Transports/CustomRosTransportUdp.h | 15 +- lib/WiFiNative/src/WifiClient.cpp | 5 +- 6 files changed, 306 insertions(+), 64 deletions(-) diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp index 1344576c..cc7378f2 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp @@ -27,6 +27,7 @@ /** * @brief Custom Transport class with C-language interface for microros. * @author Norbert Schulz + */ /****************************************************************************** * Includes diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp index 444e3ab7..e3b758bc 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp @@ -57,10 +57,16 @@ /****************************************************************************** * Local Variables *****************************************************************************/ -/** - * Name of implemented protocol. - */ -static const String gProtocolName("TCP"); + +const String CustomRosTransportTcp::m_protocolName("TCP"); + +const CustomRosTransportTcp::ReadFunc CustomRosTransportTcp::m_readFunction[MAX] = +{ + &CustomRosTransportTcp::readSizePrefix, + &CustomRosTransportTcp::readPendingSizePrefix, + &CustomRosTransportTcp::readPayload, + &CustomRosTransportTcp::readFinish +}; /****************************************************************************** * Public Methods @@ -76,7 +82,7 @@ static const String gProtocolName("TCP"); bool CustomRosTransportTcp::open(void) { - bool isOpen = false; + bool isOpen = false; if (!m_tcpClient.connect(m_address, m_port)) { @@ -93,7 +99,6 @@ bool CustomRosTransportTcp::open(void) bool CustomRosTransportTcp::close() { m_tcpClient.stop(); - LOG_DEBUG("tcp stream closed.\n"); return true; } @@ -107,19 +112,23 @@ size_t CustomRosTransportTcp::write(const uint8_t* buffer, size_t size, uint8_t* } else { - uint8_t lengthPrefix[0]; + uint8_t lengthPrefix[2]; lengthPrefix[0] = static_cast(size & 0xFF); - lengthPrefix[1] = static_cast((size>>8) & 0xFF); + lengthPrefix[1] = static_cast((size >> 8) & 0xFF); - m_tcpClient.write(lengthPrefix, 2); - if ((sent = m_tcpClient.write(buffer, size)) != size) + if (sizeof(lengthPrefix) != m_tcpClient.write(lengthPrefix, sizeof(lengthPrefix))) + { + LOG_ERROR("Write prefix error (size=%zu, sent=%zu)", size, sent); + *errorCode = 1; + } + else if ((sent = m_tcpClient.write(buffer, size)) != size) { - LOG_ERROR("Write error (size=%zu, sent=%zu)", size, sent); - * errorCode = 1; + LOG_ERROR("Write data error (size=%zu, sent=%zu)", size, sent); + *errorCode = 1; } else { - * errorCode = 0; + *errorCode = 0; } } @@ -136,25 +145,58 @@ size_t CustomRosTransportTcp::read(uint8_t* buffer, size_t size, int timeout, ui } else { - uint8_t lengthPrefix[2]; /* The first 2 bytes hold the payload length that follows. */ - - * errorCode = 1; - - if (readFixedLength(lengthPrefix, sizeof(lengthPrefix), nullptr, timeout)) /* size prefix */ + bool loop = true; + + /** + * Run receive state machine. + * The read function relationships are: + * + * read() -> [state]->read() -> readInternal() -> WifiClient + * + * @startuml "TCP Reader Relationships" + * Actor MicroRos order 10 + * participant CustomRosTransportTcp order 20 + * participant WifiClient order 20 + * + * MicroRos-> CustomRosTransportTcp : read() + * activate CustomRosTransportTcp + * group loop StateMachine [while loop] + * group alt state + * CustomRosTransportTcp -> CustomRosTransportTcp : read(&loop) + * activate CustomRosTransportTcp + * CustomRosTransportTcp -> CustomRosTransportTcp : readInternal() + * activate CustomRosTransportTcp + * CustomRosTransportTcp -> WifiClient : read + * activate WifiClient + * return + * return + * return + * end + * end + * return + * @enduml + */ + while (loop) { - size_t payloadLen; - payloadLen = static_cast(lengthPrefix[0]) + (static_cast(lengthPrefix[1]) >> 8); + loop = (this->*m_readFunction[m_inputState])(timeout, errorCode); + } - if (payloadLen > size) + if (InputState::FINISH == m_inputState) + { + if (readBytes > size) { - LOG_ERROR("record length %zu exceeds read buffer size %zu.", payloadLen, size); + /* internal error, request buffer would be overrun */ + close(); + * errorCode = 3U; } - else + else { - if (readFixedLength(buffer, payloadLen, &readBytes, timeout)) /* payload */ - { - * errorCode = 0; - } + readBytes = m_payloadLen; + memcpy(buffer, m_inputBuf, readBytes); + + m_inputState = InputState::INIT; + m_received = 0U; + m_payloadLen = 0U; } } } @@ -162,12 +204,14 @@ size_t CustomRosTransportTcp::read(uint8_t* buffer, size_t size, int timeout, ui return readBytes; } -bool CustomRosTransportTcp::readFixedLength(uint8_t* buffer, size_t length, size_t * readCount, int timeout) +size_t CustomRosTransportTcp::readInternal(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) { - size_t remaining = length; + size_t remaining = size; SimpleTimer readTimer; - bool timeOver = false; + bool timeOver = false; + + *errorCode = 0U; readTimer.start(timeout); @@ -176,6 +220,8 @@ bool CustomRosTransportTcp::readFixedLength(uint8_t* buffer, size_t length, size int count = m_tcpClient.read(buffer, remaining); if (-1 == count) { + *errorCode = 1U; /* Set error flag */ + remaining = size; /* Return 0 on error.*/ break; } @@ -196,18 +242,113 @@ bool CustomRosTransportTcp::readFixedLength(uint8_t* buffer, size_t length, size } } + return size - remaining; +} + +bool CustomRosTransportTcp::readSizePrefix(int timeout, uint8_t* errorCode) +{ + bool loop = true; + + uint8_t prefix[2]; - if (nullptr != readCount) + m_received = 0U; + + switch (readInternal(prefix, sizeof(prefix), timeout, errorCode)) { - * readCount = length - remaining; + case 0: /* nothing received */ + if (0 != *errorCode) + { + close(); + } + loop = false; + break; + + case 1: /* only low byte out of the 2 length prefix bytes received. */ + m_payloadLen = static_cast(prefix[0]); + m_inputState = InputState::PREFIX_1; + loop = true; + break; + + case 2: /* 2 byte prefix received */ + m_payloadLen = static_cast(prefix[0]) + (static_cast(prefix[1]) << 8); + m_received = 0U; + m_inputState = InputState::PLAY_LOAD; + loop = true; + break; + + default: /* should never be possible but ...*/ + *errorCode = 2; + loop = false; + break; } - return remaining ? false : true; + return loop; } -const String& CustomRosTransportTcp::getProtocolName() const +bool CustomRosTransportTcp::readPendingSizePrefix(int timeout, uint8_t* errorCode) { - return gProtocolName; + bool loop = true; + uint8_t prefix[1]; + + switch (readInternal(prefix, sizeof(prefix), timeout, errorCode)) + { + case 0: /* nothing received */ + if (0U != *errorCode) + { + close(); + } + loop = false; + break; + + case 1: /* High byte received. */ + m_payloadLen |= (static_cast(prefix[0]) << 8); + m_received = 0U; + m_inputState = InputState::PLAY_LOAD; + loop = true; + break; + + default: /* should never be possible but ...*/ + *errorCode = 2; + loop = false; + break; + } + + return loop; +} + +bool CustomRosTransportTcp::readPayload(int timeout, uint8_t* errorCode) +{ + bool loop = true; + size_t readByteCnt = readInternal(m_inputBuf + m_received, m_payloadLen - m_received, timeout, errorCode); + + if (0U < readByteCnt) + { + m_received += readByteCnt; + if (m_received == m_payloadLen) + { + /* record completed */ + m_inputState = InputState::FINISH; + } + } + else + { + if (0U != *errorCode) + { + close(); + } + loop = false; + } + + return loop; +} + +bool CustomRosTransportTcp::readFinish(int timeout, uint8_t* errorCode) +{ + (void)timeout; + (void)errorCode; + + bool loop = false; + return loop; } /****************************************************************************** diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h index 086b111f..ddbdb5ac 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h @@ -53,6 +53,12 @@ /****************************************************************************** * Types and Classes *****************************************************************************/ + +/** + * Map this transport to the class name used in MicroRosClient. + * The used transport is a compile time decision and this typedef + * avoids the use of #ifdef's. + */ typedef class CustomRosTransportTcp CustomRosTransport; /** @@ -61,11 +67,14 @@ typedef class CustomRosTransportTcp CustomRosTransport; * TCP protocol uses a record format with a 16 bit length field before the * payload. * - * +----------------+----------------+-----..........------+ + * +----------------+----------------+---------------------+ * | 1 Byte | 1 Bytes | "Lenght" Bytes | - * +----------------+----------------+-----..........------+ + * +----------------+----------------+---------------------+ * | Length [0..7] | Length [8..15] | < payload > | - * +----------------+----------------+-----..........------+ + * +----------------+----------------+---------------------+ + * + * A state machine is used for reading the record format. + * */ class CustomRosTransportTcp : public CustomRosTransportBase { @@ -73,7 +82,12 @@ class CustomRosTransportTcp : public CustomRosTransportBase /** * Constructs a custom Micro-ROS transport. */ - CustomRosTransportTcp() : CustomRosTransportBase(), m_tcpClient() + CustomRosTransportTcp() : + CustomRosTransportBase(), + m_tcpClient(), + m_inputState(InputState::INIT), + m_payloadLen(0U), + m_received(0U) { } @@ -89,25 +103,28 @@ class CustomRosTransportTcp : public CustomRosTransportBase * Get protocol name used by this trandport. * @return protocol name */ - virtual const String& getProtocolName() const final; + virtual const String& getProtocolName() const final + { + return m_protocolName; + } private: /** - * Open and initialize the custom transport. + * Open and initialize API for the custom transport. * * @return A boolean indicating if the opening was successful. */ bool open(void) final; /** - * Close the custom transport. + * Close API for the custom transport. * * @return A boolean indicating if the closing was successful. */ bool close(void) final; /** - * Write data to the custom transport. + * Write data API for the custom transport. * * @param[in] buffer The buffer to write. * @param[in] size The size of the buffer. @@ -118,7 +135,7 @@ class CustomRosTransportTcp : public CustomRosTransportBase size_t write(const uint8_t* buffer, size_t size, uint8_t* errorCode) final; /** - * Read data from the custom transport. + * Read data API of the custom transport. * * @param[out] buffer The buffer to read into. * @param[in] size The size of the buffer. @@ -130,19 +147,100 @@ class CustomRosTransportTcp : public CustomRosTransportBase size_t read(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) final; /** - * Try to read a fixed number of bytes. - * + * Internal read wrapper with timeout handling loop. + * This function is used by the input state dependend read handlers. + * * @param[out] buffer The buffer to read into. * @param[in] size The size of the buffer. - * @param[in] readCount return bytes read if not a nullptr. * @param[in] timeout The timeout in milliseconds. + * @param[out] errorCode Set the error code to != 0 on error. + * + * @return The number of bytes read. + */ + size_t readInternal(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode); + + /** + * Try to read 2 byte size prefix (low, high) in InputState::INIT + * + * @param[in] timeout The timeout in milliseconds. + * @param[out] errorCode Set the error code to != 0 on error. + */ + bool readSizePrefix(int timeout, uint8_t* errorCode); + + /** + * Try to read 2nd byte of size prefix (low, high) in InputState::PREFIX_1 + * + * Used in the rare event that low level read only provided one byte. * - * @return true of all requested bytes received. + * @param[in] timeout The timeout in milliseconds. + * @param[out] errorCode Set the error code to != 0 on error. + * + * @return true if statemachine shall reloop. */ - bool readFixedLength(uint8_t* buffer, size_t length, size_t * readCount, int timeout); + bool readPendingSizePrefix(int timeout, uint8_t* errorCode); + + /** + * Try payload bytes in InputState::PLAY_LOAD + * + * Used in the rare event that low level read only provided one byte. + * + * @param[in] timeout The timeout in milliseconds. + * @param[out] errorCode Set the error code to != 0 on error. + * + * @return true if statemachine shall reloop. + */ + bool readPayload(int timeout, uint8_t* errorCode); + + /** + * Handle message complete in InputState::FINISH + * + * Used in the rare event that low level read only provided one byte. + * + * @param[in] timeout The timeout in milliseconds. + * @param[out] loop Indicate if state machine shall reloop. + * @param[out] errorCode Set the error code to != 0 on error. + * + * @return true if statemachine shall reloop. + */ + bool readFinish(int timeout, uint8_t* errorCode); private: - WiFiClient m_tcpClient; /**< TCP client */ + WiFiClient m_tcpClient; /**< The TCP client. */ + static const String m_protocolName; /**< This protocol name. */ + + /* Read buffer handling from streaming sockets to implement framing. + * + * Custom transports with framing support need a customized agent. + * That is why the framing is implemented here to work with the + * "standard" tcpv4 agent protocol. + */ + enum InputState { + INIT, /**< Input buffer ready for new record. */ + PREFIX_1, /**< First byte of 2 -Byte size prefix received. */ + PLAY_LOAD, /**< Collecting payload bytes. */ + FINISH, /**< Payload record is complete. */ + MAX /**< Enum Range value, not a true state. */ + }; + + /** State dependent read function pointers + * + * @param[in] timeout Read timout in milliseconds. + * @param[out] errorCode Read error code if != 0 + * + * return true if state machine shall reloop. + */ + typedef bool (CustomRosTransportTcp::*ReadFunc)(int timeout, uint8_t* errorCode); + + /** Read functions ponter array indexed by InputState. */ + static const ReadFunc m_readFunction[MAX]; + + InputState m_inputState; /**< Actual input buffer status */ + + size_t m_payloadLen; /**< Number of bytes to read as payload. */ + size_t m_received; /**< Number of bytes received already */ + + static const size_t MTU = 1024U; /**< Maximum supported transmission size.*/ + uint8_t m_inputBuf[MTU]; /**< Buffer for input record reading. */ }; /****************************************************************************** diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp index 55cb25ec..d3c2a3ac 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.cpp @@ -59,10 +59,7 @@ * Local Variables *****************************************************************************/ -/** - * Name of implemented protocol. - */ -static const String gProtocolName("UDP"); + const String CustomRosTransportUdp::m_protocolName("UDP"); /****************************************************************************** * Public Methods @@ -199,11 +196,6 @@ size_t CustomRosTransportUdp::read(uint8_t* buffer, size_t size, int timeout, ui return readBytes; } -const String& CustomRosTransportUdp::getProtocolName() const -{ - return gProtocolName; -} - /****************************************************************************** * External Functions *****************************************************************************/ diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h index c782f91f..8d5e6959 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h @@ -53,6 +53,12 @@ /****************************************************************************** * Types and Classes *****************************************************************************/ + +/** + * Map this transport to the class name used in MicroRosClient. + * The used transport is a compile time decision and this typedef + * avoids the use of #ifdef's. + */ typedef class CustomRosTransportUdp CustomRosTransport; /** @@ -82,7 +88,10 @@ class CustomRosTransportUdp : public CustomRosTransportBase * Get protocol name used by this trandport. * @return protocol name */ - virtual const String& getProtocolName() const final; + virtual const String& getProtocolName() const final + { + return m_protocolName; + } private: /** @@ -122,7 +131,9 @@ class CustomRosTransportUdp : public CustomRosTransportBase */ size_t read(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) final; - WiFiUDP m_udpClient; /**< UDP client */ + WiFiUDP m_udpClient; /**< UDP client */ + static const String m_protocolName; /**< This protocol name. */ + }; /****************************************************************************** diff --git a/lib/WiFiNative/src/WifiClient.cpp b/lib/WiFiNative/src/WifiClient.cpp index 17e3d50f..65489b5f 100644 --- a/lib/WiFiNative/src/WifiClient.cpp +++ b/lib/WiFiNative/src/WifiClient.cpp @@ -36,7 +36,6 @@ #include #include -#include #include #include #include @@ -84,7 +83,8 @@ uint8_t WiFiClient::connect(const IPAddress& addr, uint16_t port) const int one = 1; ::setsockopt(m_poll.fd, SOL_TCP, TCP_NODELAY, &one, sizeof(one)); - //::fcntl(m_socket, F_SETFL, O_NONBLOCK); + ::fcntl(m_poll.fd, F_SETFL, O_NONBLOCK); + retval = 1; /* success*/ } else @@ -118,7 +118,6 @@ size_t WiFiClient::write(const uint8_t* buffer, size_t size) if (0 > written) /* error */ { break; - /* TODO: Signal error upwards - how ?*/ } remaining -= written; From 422464bf0b8b14984421617b39ecc770b854193b Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Wed, 30 Oct 2024 14:55:18 +0100 Subject: [PATCH 05/10] doxygen fixes --- .../src/Transports/CustomRosTransportTcp.cpp | 23 ------------------- .../src/Transports/CustomRosTransportTcp.h | 8 ++++--- .../src/Transports/CustomRosTransportUdp.h | 2 +- 3 files changed, 6 insertions(+), 27 deletions(-) diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp index e3b758bc..758bddab 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp @@ -152,29 +152,6 @@ size_t CustomRosTransportTcp::read(uint8_t* buffer, size_t size, int timeout, ui * The read function relationships are: * * read() -> [state]->read() -> readInternal() -> WifiClient - * - * @startuml "TCP Reader Relationships" - * Actor MicroRos order 10 - * participant CustomRosTransportTcp order 20 - * participant WifiClient order 20 - * - * MicroRos-> CustomRosTransportTcp : read() - * activate CustomRosTransportTcp - * group loop StateMachine [while loop] - * group alt state - * CustomRosTransportTcp -> CustomRosTransportTcp : read(&loop) - * activate CustomRosTransportTcp - * CustomRosTransportTcp -> CustomRosTransportTcp : readInternal() - * activate CustomRosTransportTcp - * CustomRosTransportTcp -> WifiClient : read - * activate WifiClient - * return - * return - * return - * end - * end - * return - * @enduml */ while (loop) { diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h index ddbdb5ac..3d5a36ba 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h @@ -57,7 +57,7 @@ /** * Map this transport to the class name used in MicroRosClient. * The used transport is a compile time decision and this typedef - * avoids the use of #ifdef's. + * avoids the use of ifdef's. */ typedef class CustomRosTransportTcp CustomRosTransport; @@ -164,6 +164,8 @@ class CustomRosTransportTcp : public CustomRosTransportBase * * @param[in] timeout The timeout in milliseconds. * @param[out] errorCode Set the error code to != 0 on error. + * + * @return true if statemachine shall reloop. */ bool readSizePrefix(int timeout, uint8_t* errorCode); @@ -197,7 +199,6 @@ class CustomRosTransportTcp : public CustomRosTransportBase * Used in the rare event that low level read only provided one byte. * * @param[in] timeout The timeout in milliseconds. - * @param[out] loop Indicate if state machine shall reloop. * @param[out] errorCode Set the error code to != 0 on error. * * @return true if statemachine shall reloop. @@ -208,7 +209,8 @@ class CustomRosTransportTcp : public CustomRosTransportBase WiFiClient m_tcpClient; /**< The TCP client. */ static const String m_protocolName; /**< This protocol name. */ - /* Read buffer handling from streaming sockets to implement framing. + /** + * Read buffer handling from streaming sockets to implement framing. * * Custom transports with framing support need a customized agent. * That is why the framing is implemented here to work with the diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h index 8d5e6959..6929aa26 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h @@ -57,7 +57,7 @@ /** * Map this transport to the class name used in MicroRosClient. * The used transport is a compile time decision and this typedef - * avoids the use of #ifdef's. + * avoids the use of ifdef's. */ typedef class CustomRosTransportUdp CustomRosTransport; From 1f66f647b0a4b6bcdfb736085ec5f984589a57db Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Wed, 30 Oct 2024 15:04:05 +0100 Subject: [PATCH 06/10] Clang Tidy warnings --- .vscode/settings.json | 12 ++++++++++++ lib/APPTurtle/src/Transports/CustomRosTransportTcp.h | 3 ++- 2 files changed, 14 insertions(+), 1 deletion(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..8258dcf5 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,12 @@ +{ + "files.associations": { + "*.ipp": "cpp", + "bitset": "cpp", + "string_view": "cpp", + "regex": "cpp", + "condition_variable": "cpp", + "future": "cpp", + "cstdint": "cpp", + "*.tcc": "cpp" + } +} \ No newline at end of file diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h index 3d5a36ba..a07803bf 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h @@ -87,7 +87,8 @@ class CustomRosTransportTcp : public CustomRosTransportBase m_tcpClient(), m_inputState(InputState::INIT), m_payloadLen(0U), - m_received(0U) + m_received(0U), + m_inputBuf() { } From a427430076a4c146dc5372cc80e2e71159592d24 Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Wed, 30 Oct 2024 15:09:28 +0100 Subject: [PATCH 07/10] Removal of unwanted vscode/settings.json Add config.json change instead (default port) --- .vscode/settings.json | 12 ------------ data/config/config.json | 2 +- 2 files changed, 1 insertion(+), 13 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 8258dcf5..00000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "files.associations": { - "*.ipp": "cpp", - "bitset": "cpp", - "string_view": "cpp", - "regex": "cpp", - "condition_variable": "cpp", - "future": "cpp", - "cstdint": "cpp", - "*.tcc": "cpp" - } -} \ No newline at end of file diff --git a/data/config/config.json b/data/config/config.json index 5c45521a..5601d32b 100644 --- a/data/config/config.json +++ b/data/config/config.json @@ -27,6 +27,6 @@ }, "microROSAgent": { "host": "127.0.0.1", - "port": "8889" + "port": "8888" } } \ No newline at end of file From ab1f87386d1d583e8e9cb23fd559b51cd58a773f Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Wed, 30 Oct 2024 15:34:55 +0100 Subject: [PATCH 08/10] Update ROS2 documentation + TCP support documented --- doc/ROS2/setup/Agent.md | 26 +++++++++++++++++++++++++- lib/APPTurtle/src/CustomRosTransport.h | 2 +- lib/WiFiNative/src/WifiClient.cpp | 2 -- 3 files changed, 26 insertions(+), 4 deletions(-) diff --git a/doc/ROS2/setup/Agent.md b/doc/ROS2/setup/Agent.md index 7a764d16..1e2257ac 100644 --- a/doc/ROS2/setup/Agent.md +++ b/doc/ROS2/setup/Agent.md @@ -6,6 +6,7 @@ Sources: [here](https://micro-xrce-dds.docs.eprosima.com/en/latest/index.html) * [Installation](#installation) * [Using the agent with the serial interface](#using-the-agent-with-the-serial-interface) * [Using the agent with the UDP interface](#using-the-agent-with-the-udp-interface) + * [Using the agent with the TCP interface](#using-the-agent-with-the-tcp-interface) * [Troubleshooting on WSL environment](#troubleshooting-on-wsl-environment) * [Testing the node](#testing-the-node) @@ -37,9 +38,30 @@ Once the Agent and the Client are connected, the terminal should show something [1723186868.574254] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x64C59DFF, datawriter_id: 0x000(5), publisher_id: 0x000(3) ``` +## Using the agent with the TCP interface + +Start the MicroXRCEAgent binary to listen to **TCP** connections: + +```bash +./MicroXRCEAgent tcp4 -p 8888 -v 6 +[1730295182.773705] info | TCPv4AgentLinux.cpp | init | running... | port: 8888 +[1730295182.774347] info | Root.cpp | set_verbose_level | logger setup | verbose_level: 6 +[1730295187.775068] debug | TCPv4AgentLinux.cpp | recv_message | [==>> TCP <<==] | client_key: 0x00000000, len: 16, data: +0000: 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00 +[1730295187.775861] debug | TCPv4AgentLinux.cpp | send_message | [** <> **] | client_key: 0x00000000, len: 36, data: +0000: 80 00 00 00 06 01 1C 00 00 0A FF FD 00 00 01 0D 58 52 43 45 01 00 01 0F 00 01 0D 00 01 00 00 00 +0020: 00 00 00 00 +[1730295188.777062] debug | TCPv4AgentLinux.cpp | recv_message | [==>> TCP <<==] | client_key: 0x00000000, len: 24, data: +... +``` + ## Using the agent with the UDP interface -Start the MicroXRCEAgent binary to listen to UDP connections +> [!WARNING] +> Note: UDP ports on WSL are not working properly if you need to access them outside of the WSL VM. Use TCP with WSL instead. + +Start the MicroXRCEAgent binary to listen to UDP connections: + ```bash ./MicroXRCEAgent udp4 -p 8888 -v 6 [1724834512.980210] info | UDPv4AgentLinux.cpp | init | running... | port: 8888 @@ -56,6 +78,8 @@ Start the MicroXRCEAgent binary to listen to UDP connections UDP connections [setup wsl](./wsl.md#exposing-wsl-udp-ports-to-the-network) (Requires Win 11). +Update: 2024-10-30: TCP transport is available in DroidControlShip, making UDP usage optional. Use TCP on WSL to avoid the trouble. + ## Testing the node In order to test your node, you can use `ros2 topic list` to list all topics used, or `ros2 topic echo ` to listen to incoming data in a specific topic. diff --git a/lib/APPTurtle/src/CustomRosTransport.h b/lib/APPTurtle/src/CustomRosTransport.h index e0076882..b84584f3 100644 --- a/lib/APPTurtle/src/CustomRosTransport.h +++ b/lib/APPTurtle/src/CustomRosTransport.h @@ -25,7 +25,7 @@ DESCRIPTION *******************************************************************************/ /** - * @brief Custom Micro-ROS transport class selector + * @brief Custom Micro-ROS transport class selector. * @author Norbert Schulz * * @addtogroup Application diff --git a/lib/WiFiNative/src/WifiClient.cpp b/lib/WiFiNative/src/WifiClient.cpp index 65489b5f..a526d113 100644 --- a/lib/WiFiNative/src/WifiClient.cpp +++ b/lib/WiFiNative/src/WifiClient.cpp @@ -105,7 +105,6 @@ void WiFiClient::stop() } } - size_t WiFiClient::write(const uint8_t* buffer, size_t size) { size_t remaining = size; @@ -132,7 +131,6 @@ int WiFiClient::read(uint8_t* buffer, size_t size) { int retval = -1; - if (connected()) { if (-1 != ::poll(&m_poll, 1, 10)) From 839938ffd2cb338992901e07deb7fa6b9603a975 Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Thu, 31 Oct 2024 16:49:38 +0100 Subject: [PATCH 09/10] Applied review feedback https://github.com/BlueAndi/DroidControlShip/pull/185 --- config/buildmode.ini | 32 +++++++ doc/ROS2/setup/Agent.md | 4 +- .../src/Transports/CustomRosTransportBase.h | 6 +- .../src/Transports/CustomRosTransportTcp.cpp | 80 ++++++++++------- .../src/Transports/CustomRosTransportTcp.h | 85 +++++++++--------- .../src/Transports/CustomRosTransportUdp.h | 8 +- lib/WiFiNative/src/WiFiClient.h | 14 ++- lib/WiFiNative/src/WifiClient.cpp | 86 +++++++++++++------ platformio.ini | 34 ++++++-- 9 files changed, 230 insertions(+), 119 deletions(-) create mode 100644 config/buildmode.ini diff --git a/config/buildmode.ini b/config/buildmode.ini new file mode 100644 index 00000000..cbef9ae3 --- /dev/null +++ b/config/buildmode.ini @@ -0,0 +1,32 @@ +; ******************************************************************************** +; Build mode log configurations +; * debug: Standard debugging purposes during development. +; * release: The release build contains only the relevant user informations. +; * trace: Hardcore debugging by following e.g. method call hierarchy. +; ******************************************************************************** +[mode:debug_log] +build_flags = + -D LOG_DEBUG_ENABLE=1 + -D LOG_TRACE_ENABLE=0 + -D CONFIG_LOG_SEVERITY=Logging::LOG_LEVEL_DEBUG + +[mode:release_log] +build_flags = + -D LOG_DEBUG_ENABLE=0 + -D LOG_TRACE_ENABLE=0 + -D CONFIG_LOG_SEVERITY=Logging::LOG_LEVEL_INFO + +[mode:trace_log] +build_flags = + -D LOG_DEBUG_ENABLE=1 + -D LOG_TRACE_ENABLE=1 + -D CONFIG_LOG_SEVERITY=Logging::LOG_LEVEL_TRACE + +[mode:debug_native] +build_flags = + -O0 + -ggdb + +[mode:release_native] +build_flags = + -O2 \ No newline at end of file diff --git a/doc/ROS2/setup/Agent.md b/doc/ROS2/setup/Agent.md index 1e2257ac..165bc7d7 100644 --- a/doc/ROS2/setup/Agent.md +++ b/doc/ROS2/setup/Agent.md @@ -58,7 +58,9 @@ Start the MicroXRCEAgent binary to listen to **TCP** connections: ## Using the agent with the UDP interface > [!WARNING] -> Note: UDP ports on WSL are not working properly if you need to access them outside of the WSL VM. Use TCP with WSL instead. +> Note: UDP ports on WSL are not working properly if you need to access them outside of the WSL VM (Status 2024-10-31). Use TCP with WSL instead. +> See [https://github.com/micro-ROS/micro-ROS-Agent/issues/194](https://github.com/micro-ROS/micro-ROS-Agent/issues/194) for further details. +> The mentioned ```netsh`` tool for port proxy forwarding only supports TCP. Start the MicroXRCEAgent binary to listen to UDP connections: diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportBase.h b/lib/APPTurtle/src/Transports/CustomRosTransportBase.h index a4e13085..a84fc84f 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportBase.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportBase.h @@ -72,7 +72,7 @@ class CustomRosTransportBase * Destroys custom Micro-ROS transport. * */ - ~CustomRosTransportBase() + virtual ~CustomRosTransportBase() { } @@ -166,8 +166,8 @@ class CustomRosTransportBase static size_t read(uxrCustomTransport* transport, uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode); /** - * Get protocol name used by this trandport. - * @return protocol name + * Get protocol name used by this transport. + * @return Protocol name */ virtual const String& getProtocolName() const = 0; diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp index 758bddab..03aafe91 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp @@ -60,9 +60,10 @@ const String CustomRosTransportTcp::m_protocolName("TCP"); -const CustomRosTransportTcp::ReadFunc CustomRosTransportTcp::m_readFunction[MAX] = +const CustomRosTransportTcp::ReadFunc +CustomRosTransportTcp::m_readFunction[InputState::INPUT_STATE_MAX] = { - &CustomRosTransportTcp::readSizePrefix, + &CustomRosTransportTcp::readSizePrefix, &CustomRosTransportTcp::readPendingSizePrefix, &CustomRosTransportTcp::readPayload, &CustomRosTransportTcp::readFinish @@ -84,7 +85,7 @@ bool CustomRosTransportTcp::open(void) { bool isOpen = false; - if (!m_tcpClient.connect(m_address, m_port)) + if (0U == m_tcpClient.connect(m_address, m_port)) { LOG_ERROR("TCP connect error"); } @@ -113,22 +114,22 @@ size_t CustomRosTransportTcp::write(const uint8_t* buffer, size_t size, uint8_t* else { uint8_t lengthPrefix[2]; - lengthPrefix[0] = static_cast(size & 0xFF); - lengthPrefix[1] = static_cast((size >> 8) & 0xFF); + lengthPrefix[0] = static_cast(size & 0xFFU); + lengthPrefix[1] = static_cast((size >> 8U) & 0xFFU); if (sizeof(lengthPrefix) != m_tcpClient.write(lengthPrefix, sizeof(lengthPrefix))) { LOG_ERROR("Write prefix error (size=%zu, sent=%zu)", size, sent); - *errorCode = 1; + *errorCode = 1U; } else if ((sent = m_tcpClient.write(buffer, size)) != size) { LOG_ERROR("Write data error (size=%zu, sent=%zu)", size, sent); - *errorCode = 1; + *errorCode = 1U; } else { - *errorCode = 0; + *errorCode = 0U; } } @@ -148,30 +149,45 @@ size_t CustomRosTransportTcp::read(uint8_t* buffer, size_t size, int timeout, ui bool loop = true; /** - * Run receive state machine. + * Run receive state machine. * The read function relationships are: - * + * * read() -> [state]->read() -> readInternal() -> WifiClient */ - while (loop) + while (true == loop) { - loop = (this->*m_readFunction[m_inputState])(timeout, errorCode); + + if (InputState::INPUT_STATE_MAX > m_inputState) + { + ReadFunc stateReadFunc = m_readFunction[m_inputState]; /**< Get state dependend input reader. */ + + /* Read state dependend message portion. */ + loop = (this->*stateReadFunc)(timeout, errorCode); + } + else + { + LOG_FATAL("Invalid state %d", static_cast(m_inputState)); + *errorCode = 4U; + loop = false; + } } - if (InputState::FINISH == m_inputState) + if (InputState::INPUT_STATE_FINISH == m_inputState) { if (readBytes > size) { - /* internal error, request buffer would be overrun */ + /* Internal error, request buffer would be overrun. */ close(); - * errorCode = 3U; + *errorCode = 5U; } - else + else { + /* Provide payload to destination buffer. */ readBytes = m_payloadLen; memcpy(buffer, m_inputBuf, readBytes); - m_inputState = InputState::INIT; + /* Reset state machine*/ + m_inputState = InputState::INPUT_STATE_INIT; m_received = 0U; m_payloadLen = 0U; } @@ -192,7 +208,7 @@ size_t CustomRosTransportTcp::readInternal(uint8_t* buffer, size_t size, int tim readTimer.start(timeout); - while ((0 < remaining) && (false == timeOver)) + while ((0U < remaining) && (false == timeOver)) { int count = m_tcpClient.read(buffer, remaining); if (-1 == count) @@ -232,29 +248,29 @@ bool CustomRosTransportTcp::readSizePrefix(int timeout, uint8_t* errorCode) switch (readInternal(prefix, sizeof(prefix), timeout, errorCode)) { - case 0: /* nothing received */ - if (0 != *errorCode) + case 0U: /* nothing received */ + if (0U != *errorCode) { close(); } loop = false; break; - case 1: /* only low byte out of the 2 length prefix bytes received. */ + case 1U: /* only low byte out of the 2 length prefix bytes received. */ m_payloadLen = static_cast(prefix[0]); - m_inputState = InputState::PREFIX_1; + m_inputState = InputState::INPUT_STATE_PREFIX_1; loop = true; break; - case 2: /* 2 byte prefix received */ + case 2U: /* 2 byte prefix received */ m_payloadLen = static_cast(prefix[0]) + (static_cast(prefix[1]) << 8); m_received = 0U; - m_inputState = InputState::PLAY_LOAD; + m_inputState = InputState::INPUT_STATE_PLAYLOAD; loop = true; break; default: /* should never be possible but ...*/ - *errorCode = 2; + *errorCode = 2U; loop = false; break; } @@ -264,12 +280,12 @@ bool CustomRosTransportTcp::readSizePrefix(int timeout, uint8_t* errorCode) bool CustomRosTransportTcp::readPendingSizePrefix(int timeout, uint8_t* errorCode) { - bool loop = true; + bool loop = true; uint8_t prefix[1]; switch (readInternal(prefix, sizeof(prefix), timeout, errorCode)) { - case 0: /* nothing received */ + case 0U: /* nothing received */ if (0U != *errorCode) { close(); @@ -277,15 +293,15 @@ bool CustomRosTransportTcp::readPendingSizePrefix(int timeout, uint8_t* errorCod loop = false; break; - case 1: /* High byte received. */ + case 1U: /* High byte received. */ m_payloadLen |= (static_cast(prefix[0]) << 8); m_received = 0U; - m_inputState = InputState::PLAY_LOAD; + m_inputState = InputState::INPUT_STATE_PLAYLOAD; loop = true; break; default: /* should never be possible but ...*/ - *errorCode = 2; + *errorCode = 2U; loop = false; break; } @@ -295,7 +311,7 @@ bool CustomRosTransportTcp::readPendingSizePrefix(int timeout, uint8_t* errorCod bool CustomRosTransportTcp::readPayload(int timeout, uint8_t* errorCode) { - bool loop = true; + bool loop = true; size_t readByteCnt = readInternal(m_inputBuf + m_received, m_payloadLen - m_received, timeout, errorCode); if (0U < readByteCnt) @@ -304,7 +320,7 @@ bool CustomRosTransportTcp::readPayload(int timeout, uint8_t* errorCode) if (m_received == m_payloadLen) { /* record completed */ - m_inputState = InputState::FINISH; + m_inputState = InputState::INPUT_STATE_FINISH; } } else diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h index a07803bf..b9699cb8 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h @@ -54,7 +54,7 @@ * Types and Classes *****************************************************************************/ -/** +/** * Map this transport to the class name used in MicroRosClient. * The used transport is a compile time decision and this typedef * avoids the use of ifdef's. @@ -66,15 +66,15 @@ typedef class CustomRosTransportTcp CustomRosTransport; * * TCP protocol uses a record format with a 16 bit length field before the * payload. - * + * * +----------------+----------------+---------------------+ * | 1 Byte | 1 Bytes | "Lenght" Bytes | * +----------------+----------------+---------------------+ * | Length [0..7] | Length [8..15] | < payload > | * +----------------+----------------+---------------------+ - * - * A state machine is used for reading the record format. - * + * + * A state machine is used for reading the record format. + * */ class CustomRosTransportTcp : public CustomRosTransportBase { @@ -82,10 +82,10 @@ class CustomRosTransportTcp : public CustomRosTransportBase /** * Constructs a custom Micro-ROS transport. */ - CustomRosTransportTcp() : - CustomRosTransportBase(), + CustomRosTransportTcp() : + CustomRosTransportBase(), m_tcpClient(), - m_inputState(InputState::INIT), + m_inputState(InputState::INPUT_STATE_INIT), m_payloadLen(0U), m_received(0U), m_inputBuf() @@ -96,15 +96,15 @@ class CustomRosTransportTcp : public CustomRosTransportBase * Destroys custom Micro-ROS transport. * */ - virtual ~CustomRosTransportTcp() final + ~CustomRosTransportTcp() { } - /** - * Get protocol name used by this trandport. - * @return protocol name + /** + * Get protocol name used by this transport. + * @return Protocol name */ - virtual const String& getProtocolName() const final + const String& getProtocolName() const final { return m_protocolName; } @@ -162,7 +162,7 @@ class CustomRosTransportTcp : public CustomRosTransportBase /** * Try to read 2 byte size prefix (low, high) in InputState::INIT - * + * * @param[in] timeout The timeout in milliseconds. * @param[out] errorCode Set the error code to != 0 on error. * @@ -172,9 +172,9 @@ class CustomRosTransportTcp : public CustomRosTransportBase /** * Try to read 2nd byte of size prefix (low, high) in InputState::PREFIX_1 - * + * * Used in the rare event that low level read only provided one byte. - * + * * @param[in] timeout The timeout in milliseconds. * @param[out] errorCode Set the error code to != 0 on error. * @@ -184,66 +184,67 @@ class CustomRosTransportTcp : public CustomRosTransportBase /** * Try payload bytes in InputState::PLAY_LOAD - * + * * Used in the rare event that low level read only provided one byte. - * + * * @param[in] timeout The timeout in milliseconds. * @param[out] errorCode Set the error code to != 0 on error. - * + * * @return true if statemachine shall reloop. */ bool readPayload(int timeout, uint8_t* errorCode); /** * Handle message complete in InputState::FINISH - * + * * Used in the rare event that low level read only provided one byte. - * + * * @param[in] timeout The timeout in milliseconds. * @param[out] errorCode Set the error code to != 0 on error. - * + * * @return true if statemachine shall reloop. */ bool readFinish(int timeout, uint8_t* errorCode); private: - WiFiClient m_tcpClient; /**< The TCP client. */ - static const String m_protocolName; /**< This protocol name. */ - + WiFiClient m_tcpClient; /**< The TCP client. */ + static const String m_protocolName; /**< This protocol name. */ + /** * Read buffer handling from streaming sockets to implement framing. * * Custom transports with framing support need a customized agent. * That is why the framing is implemented here to work with the - * "standard" tcpv4 agent protocol. + * "standard" tcpv4 agent protocol. */ - enum InputState { - INIT, /**< Input buffer ready for new record. */ - PREFIX_1, /**< First byte of 2 -Byte size prefix received. */ - PLAY_LOAD, /**< Collecting payload bytes. */ - FINISH, /**< Payload record is complete. */ - MAX /**< Enum Range value, not a true state. */ + enum InputState + { + INPUT_STATE_INIT = 0, /**< Input buffer ready for new record. */ + INPUT_STATE_PREFIX_1, /**< First byte of 2 -Byte size prefix received. */ + INPUT_STATE_PLAYLOAD, /**< Collecting payload bytes. */ + INPUT_STATE_FINISH, /**< Payload record is complete. */ + INPUT_STATE_MAX /**< Enum Range value, not a true state. */ }; - /** State dependent read function pointers + /** State dependent read function pointers * * @param[in] timeout Read timout in milliseconds. * @param[out] errorCode Read error code if != 0 - * - * return true if state machine shall reloop. - */ + * + * return true if state machine shall reloop. + */ typedef bool (CustomRosTransportTcp::*ReadFunc)(int timeout, uint8_t* errorCode); /** Read functions ponter array indexed by InputState. */ - static const ReadFunc m_readFunction[MAX]; + static const ReadFunc m_readFunction[InputState::INPUT_STATE_MAX]; - InputState m_inputState; /**< Actual input buffer status */ + InputState m_inputState; /**< Actual input buffer status */ - size_t m_payloadLen; /**< Number of bytes to read as payload. */ - size_t m_received; /**< Number of bytes received already */ + size_t m_payloadLen; /**< Number of bytes to read as payload. */ + size_t m_received; /**< Number of bytes received already */ - static const size_t MTU = 1024U; /**< Maximum supported transmission size.*/ - uint8_t m_inputBuf[MTU]; /**< Buffer for input record reading. */ + static const size_t MTU = 1024U; /**< Maximum supported transmission size.*/ + uint8_t m_inputBuf[MTU]; /**< Buffer for input record reading. */ }; /****************************************************************************** diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h index 6929aa26..19da3fa1 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h @@ -80,15 +80,15 @@ class CustomRosTransportUdp : public CustomRosTransportBase * Destroys custom Micro-ROS transport. * */ - virtual ~CustomRosTransportUdp() final + ~CustomRosTransportUdp() { } /** - * Get protocol name used by this trandport. - * @return protocol name + * Get protocol name used by this transport. + * @return Protocol name */ - virtual const String& getProtocolName() const final + const String& getProtocolName() const final { return m_protocolName; } diff --git a/lib/WiFiNative/src/WiFiClient.h b/lib/WiFiNative/src/WiFiClient.h index 63aeede2..b5cdec44 100644 --- a/lib/WiFiNative/src/WiFiClient.h +++ b/lib/WiFiNative/src/WiFiClient.h @@ -106,10 +106,16 @@ class WiFiClient /** * Write bytes to stream. + * + * The write uses non blocking socket mode, but retries on a + * EWOULDBLOCK result. The constants SOCK_WRITE_RETRY and + * SOCK_WRITE_TMO_US define how often to retry and the + * time delay in between. * * @param[in] buffer Byte Array to send. * @param[in] size Length of Buffer. - * @returns Number of bytes written + * + * @returns Number of bytes written or 0 on error. */ size_t write(const uint8_t* buffer, size_t size); @@ -132,9 +138,11 @@ class WiFiClient int read(uint8_t* buffer, size_t size); private: - struct pollfd m_poll; /**< The poll/socket structure. */ + struct pollfd m_poll; /**< The poll/socket structure. */ - static const int SOCK_INVAL = -1; /**< The invalid socket value */ + static const int SOCK_INVAL = -1; /**< The invalid socket value. */ + static const uint32_t SOCK_WRITE_RETRY = 4U; /**< How often to retry sending. */ + static const uint32_t SOCK_WRITE_TMO_US = 250U; /**< Delay between write attemps. */ }; /****************************************************************************** diff --git a/lib/WiFiNative/src/WifiClient.cpp b/lib/WiFiNative/src/WifiClient.cpp index a526d113..f24c94b3 100644 --- a/lib/WiFiNative/src/WifiClient.cpp +++ b/lib/WiFiNative/src/WifiClient.cpp @@ -33,6 +33,7 @@ * Includes *****************************************************************************/ #include "WiFiClient.h" +#include #include #include @@ -52,16 +53,15 @@ * Local Variables *****************************************************************************/ - /****************************************************************************** * Public Methods *****************************************************************************/ uint8_t WiFiClient::connect(const IPAddress& addr, uint16_t port) { - uint8_t retval = 0; + uint8_t retval = 0U; - if (connected()) + if (0U != connected()) { stop(); } @@ -71,36 +71,54 @@ uint8_t WiFiClient::connect(const IPAddress& addr, uint16_t port) struct sockaddr_in serverAddr; memset(&serverAddr, 0, sizeof(serverAddr)); - serverAddr.sin_family = AF_INET; + serverAddr.sin_family = AF_INET; serverAddr.sin_addr.s_addr = htonl(addr); - serverAddr.sin_port = htons(port); + serverAddr.sin_port = htons(port); - if (0 == ::connect( - m_poll.fd, - reinterpret_cast(&serverAddr), - sizeof(serverAddr) )) + if (0 == ::connect(m_poll.fd, reinterpret_cast(&serverAddr), sizeof(serverAddr))) { const int one = 1; - - ::setsockopt(m_poll.fd, SOL_TCP, TCP_NODELAY, &one, sizeof(one)); - ::fcntl(m_poll.fd, F_SETFL, O_NONBLOCK); - retval = 1; /* success*/ + if (-1 == ::setsockopt(m_poll.fd, SOL_TCP, TCP_NODELAY, &one, sizeof(one))) + { + LOG_ERROR("%s:%s", "setsockopt", strerror(errno)); + } + else if (-1 == ::fcntl(m_poll.fd, F_SETFL, ::fcntl(m_poll.fd, F_GETFL) | O_NONBLOCK)) + { + LOG_ERROR("%s:%s", "fcntl", strerror(errno)); + } + else + { + retval = 1U; /* success*/ + } } else { - stop(); + LOG_ERROR("%s:%s", "connect", strerror(errno)); + } + + if (0U == retval) + { + stop(); /* One of the system calls above failed. */ } } + else + { + LOG_ERROR("%s:%s", "socket", strerror(errno)); + } return retval; } void WiFiClient::stop() { - if (connected()) + if (0U != connected()) { - ::close(m_poll.fd); + if (0 != ::close(m_poll.fd)) + { + LOG_ERROR("%s:%s", "close", strerror(errno)); + } + m_poll.fd = SOCK_INVAL; } } @@ -108,19 +126,31 @@ void WiFiClient::stop() size_t WiFiClient::write(const uint8_t* buffer, size_t size) { size_t remaining = size; - - if ((0 < size) && connected()) + uint32_t retries = 0; + + if ((0U < size) && (0U != connected())) { - while (0 < remaining) { - ssize_t written = ::send(m_poll.fd, buffer, remaining, 0); - - if (0 > written) /* error */ + while ((0U < remaining) && (retries < SOCK_WRITE_RETRY)) + { + ssize_t written = ::send(m_poll.fd, buffer, remaining, MSG_DONTWAIT); + if (0 > written) { - break; + if ((EAGAIN == errno) || (EWOULDBLOCK == errno)) + { + written = 0; /* Not an error, just retry indication. */ + usleep(SOCK_WRITE_TMO_US); + } + else + { + LOG_ERROR("%s:%s", "send", strerror(errno)); + break; + } } remaining -= written; buffer += written; + + ++retries; } } @@ -131,20 +161,20 @@ int WiFiClient::read(uint8_t* buffer, size_t size) { int retval = -1; - if (connected()) + if (0U != connected()) { if (-1 != ::poll(&m_poll, 1, 10)) { if (0 != (POLLIN & m_poll.revents)) { - ssize_t result = ::recv(m_poll.fd, buffer, size, 0); + ssize_t result = ::recv(m_poll.fd, buffer, size, 0); if (-1 != result) { - retval = result; /* Success! */ + retval = result; /* Success! */ } else { - perror("recv"); + LOG_ERROR("%s:%s", "recv", strerror(errno)); } } else @@ -154,7 +184,7 @@ int WiFiClient::read(uint8_t* buffer, size_t size) } else { - perror("poll"); + LOG_ERROR("%s:%s", "poll", strerror(errno)); } } diff --git a/platformio.ini b/platformio.ini index 891f6f51..2c7133d3 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,6 +14,30 @@ [platformio] default_envs = ConvoyLeaderTarget, ConvoyLeaderSim +extra_configs = + config/buildmode.ini + +; ******************************************************************************** +; Select native build mode here! +; ******************************************************************************** +[mode:selected_native] +#extends = mode:debug_native +#build_flags = +# ${mode:debug_native.build_flags} + +extends = mode:release_native +build_flags = + ${mode:release_native.build_flags} + + +; ******************************************************************************** +; Select esp32 build mode here! +; ******************************************************************************** +[mode:selected_esp32] +extends = mode:debug_log +build_flags = + ${mode:debug_log.build_flags} + ; ***************************************************************************** ; Static check configuration ; ***************************************************************************** @@ -33,14 +57,14 @@ check_skip_packages = yes ; Target environment for ZumoComSystem. ; ***************************************************************************** [target:esp32] +extends = mode:selected_esp32 platform = espressif32 @ ~6.9.0 board = esp32doit-devkit-v1 board_build.filesystem = littlefs framework = arduino build_flags = + ${mode:selected_esp32.build_flags} -Wl,-Map,firmware.map - -D LOG_DEBUG_ENABLE=1 - -D CONFIG_LOG_SEVERITY=Logging::LOG_LEVEL_DEBUG -D CONFIG_FILE_PATH="\"/config/config.json\"" lib_deps = HALInterfaces @@ -71,14 +95,12 @@ monitor_filters = esp32_exception_decoder ; ***************************************************************************** [target:Sim] platform = native @ ~1.2.1 +extends = mode:selected_native build_flags = + ${mode:selected_native.build_flags} -std=c++11 - -O0 - -ggdb -D TRANSPORT_USE_TCP=1 -D TARGET_NATIVE - -D LOG_DEBUG_ENABLE=1 - -D CONFIG_LOG_SEVERITY=Logging::LOG_LEVEL_DEBUG -lmosquitto -I./lib/Webots/include/c -I./lib/Webots/include/cpp From eaf8b6f23f0f7f7832c33eb4c773d1b5ab6d6692 Mon Sep 17 00:00:00 2001 From: Norbert Schulz Date: Sun, 3 Nov 2024 14:06:28 +0100 Subject: [PATCH 10/10] ROS2 TCP: Improve fix comments review feedback --- lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp | 7 ++++--- lib/APPTurtle/src/Transports/CustomRosTransportTcp.h | 2 +- lib/WiFiNative/src/WifiClient.cpp | 3 +++ 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp index 03aafe91..1c3c9137 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp @@ -148,7 +148,7 @@ size_t CustomRosTransportTcp::read(uint8_t* buffer, size_t size, int timeout, ui { bool loop = true; - /** + /* * Run receive state machine. * The read function relationships are: * @@ -159,9 +159,10 @@ size_t CustomRosTransportTcp::read(uint8_t* buffer, size_t size, int timeout, ui if (InputState::INPUT_STATE_MAX > m_inputState) { - ReadFunc stateReadFunc = m_readFunction[m_inputState]; /**< Get state dependend input reader. */ + /* Get state dependend input reader function. */ + ReadFunc stateReadFunc = m_readFunction[m_inputState]; - /* Read state dependend message portion. */ + /* Read message portion based on reveiver state. */ loop = (this->*stateReadFunc)(timeout, errorCode); } else diff --git a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h index b9699cb8..05f38699 100644 --- a/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h @@ -231,7 +231,7 @@ class CustomRosTransportTcp : public CustomRosTransportBase * @param[in] timeout Read timout in milliseconds. * @param[out] errorCode Read error code if != 0 * - * return true if state machine shall reloop. + * @return true if state machine shall reloop. */ typedef bool (CustomRosTransportTcp::*ReadFunc)(int timeout, uint8_t* errorCode); diff --git a/lib/WiFiNative/src/WifiClient.cpp b/lib/WiFiNative/src/WifiClient.cpp index f24c94b3..a3649646 100644 --- a/lib/WiFiNative/src/WifiClient.cpp +++ b/lib/WiFiNative/src/WifiClient.cpp @@ -63,6 +63,9 @@ uint8_t WiFiClient::connect(const IPAddress& addr, uint16_t port) if (0U != connected()) { + /* Connect is called on an already connected client. + * Handle it as re-connect by closing the former socket connection. + */ stop(); }