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 7a764d16..165bc7d7 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,32 @@ 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 (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: + ```bash ./MicroXRCEAgent udp4 -p 8888 -v 6 [1724834512.980210] info | UDPv4AgentLinux.cpp | init | running... | port: 8888 @@ -56,6 +80,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 abf4ba09..b84584f3 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..de3f7b7b 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."); } @@ -284,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)) { @@ -307,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()) { @@ -315,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/CustomRosTransportBase.cpp b/lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp new file mode 100644 index 00000000..cc7378f2 --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportBase.cpp @@ -0,0 +1,129 @@ +/* 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..a84fc84f --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportBase.h @@ -0,0 +1,226 @@ +/* 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. + * + */ + virtual ~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); + + /** + * Get protocol name used by this transport. + * @return Protocol name + */ + virtual const String& getProtocolName() const = 0; + +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..1c3c9137 --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.cpp @@ -0,0 +1,354 @@ +/* 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 + *****************************************************************************/ + +const String CustomRosTransportTcp::m_protocolName("TCP"); + +const CustomRosTransportTcp::ReadFunc +CustomRosTransportTcp::m_readFunction[InputState::INPUT_STATE_MAX] = +{ + &CustomRosTransportTcp::readSizePrefix, + &CustomRosTransportTcp::readPendingSizePrefix, + &CustomRosTransportTcp::readPayload, + &CustomRosTransportTcp::readFinish +}; + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +bool CustomRosTransportTcp::open(void) +{ + bool isOpen = false; + + if (0U == 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[2]; + 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 = 1U; + } + else if ((sent = m_tcpClient.write(buffer, size)) != size) + { + LOG_ERROR("Write data error (size=%zu, sent=%zu)", size, sent); + *errorCode = 1U; + } + else + { + *errorCode = 0U; + } + } + + 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 + { + bool loop = true; + + /* + * Run receive state machine. + * The read function relationships are: + * + * read() -> [state]->read() -> readInternal() -> WifiClient + */ + while (true == loop) + { + + if (InputState::INPUT_STATE_MAX > m_inputState) + { + /* Get state dependend input reader function. */ + ReadFunc stateReadFunc = m_readFunction[m_inputState]; + + /* Read message portion based on reveiver state. */ + loop = (this->*stateReadFunc)(timeout, errorCode); + } + else + { + LOG_FATAL("Invalid state %d", static_cast(m_inputState)); + *errorCode = 4U; + loop = false; + } + } + + if (InputState::INPUT_STATE_FINISH == m_inputState) + { + if (readBytes > size) + { + /* Internal error, request buffer would be overrun. */ + close(); + *errorCode = 5U; + } + else + { + /* Provide payload to destination buffer. */ + readBytes = m_payloadLen; + memcpy(buffer, m_inputBuf, readBytes); + + /* Reset state machine*/ + m_inputState = InputState::INPUT_STATE_INIT; + m_received = 0U; + m_payloadLen = 0U; + } + } + } + + return readBytes; +} + +size_t CustomRosTransportTcp::readInternal(uint8_t* buffer, size_t size, int timeout, uint8_t* errorCode) +{ + size_t remaining = size; + + SimpleTimer readTimer; + bool timeOver = false; + + *errorCode = 0U; + + readTimer.start(timeout); + + while ((0U < remaining) && (false == timeOver)) + { + int count = m_tcpClient.read(buffer, remaining); + if (-1 == count) + { + *errorCode = 1U; /* Set error flag */ + remaining = size; /* Return 0 on error.*/ + 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 (readTimer.isTimeout()) + { + timeOver = true; + } + } + + return size - remaining; +} + +bool CustomRosTransportTcp::readSizePrefix(int timeout, uint8_t* errorCode) +{ + bool loop = true; + + uint8_t prefix[2]; + + m_received = 0U; + + switch (readInternal(prefix, sizeof(prefix), timeout, errorCode)) + { + case 0U: /* nothing received */ + if (0U != *errorCode) + { + close(); + } + loop = false; + break; + + case 1U: /* only low byte out of the 2 length prefix bytes received. */ + m_payloadLen = static_cast(prefix[0]); + m_inputState = InputState::INPUT_STATE_PREFIX_1; + loop = true; + break; + + case 2U: /* 2 byte prefix received */ + m_payloadLen = static_cast(prefix[0]) + (static_cast(prefix[1]) << 8); + m_received = 0U; + m_inputState = InputState::INPUT_STATE_PLAYLOAD; + loop = true; + break; + + default: /* should never be possible but ...*/ + *errorCode = 2U; + loop = false; + break; + } + + return loop; +} + +bool CustomRosTransportTcp::readPendingSizePrefix(int timeout, uint8_t* errorCode) +{ + bool loop = true; + uint8_t prefix[1]; + + switch (readInternal(prefix, sizeof(prefix), timeout, errorCode)) + { + case 0U: /* nothing received */ + if (0U != *errorCode) + { + close(); + } + loop = false; + break; + + case 1U: /* High byte received. */ + m_payloadLen |= (static_cast(prefix[0]) << 8); + m_received = 0U; + m_inputState = InputState::INPUT_STATE_PLAYLOAD; + loop = true; + break; + + default: /* should never be possible but ...*/ + *errorCode = 2U; + 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::INPUT_STATE_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; +} + +/****************************************************************************** + * 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..05f38699 --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportTcp.h @@ -0,0 +1,255 @@ +/* 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 + *****************************************************************************/ + +/** + * 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; + +/** + * 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 > | + * +----------------+----------------+---------------------+ + * + * A state machine is used for reading the record format. + * + */ +class CustomRosTransportTcp : public CustomRosTransportBase +{ +public: + /** + * Constructs a custom Micro-ROS transport. + */ + CustomRosTransportTcp() : + CustomRosTransportBase(), + m_tcpClient(), + m_inputState(InputState::INPUT_STATE_INIT), + m_payloadLen(0U), + m_received(0U), + m_inputBuf() + { + } + + /** + * Destroys custom Micro-ROS transport. + * + */ + ~CustomRosTransportTcp() + { + } + + /** + * Get protocol name used by this transport. + * @return Protocol name + */ + const String& getProtocolName() const final + { + return m_protocolName; + } + +private: + /** + * Open and initialize API for the custom transport. + * + * @return A boolean indicating if the opening was successful. + */ + bool open(void) final; + + /** + * Close API for the custom transport. + * + * @return A boolean indicating if the closing was successful. + */ + bool close(void) final; + + /** + * Write data API for 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 API of 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; + + /** + * 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] 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. + * + * @return true if statemachine shall reloop. + */ + 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. + * + * @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 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] 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. */ + + /** + * 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 + { + 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 + * + * @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[InputState::INPUT_STATE_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. */ +}; + +/****************************************************************************** + * 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..d3c2a3ac 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,51 +54,17 @@ * 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 *****************************************************************************/ + const String CustomRosTransportUdp::m_protocolName("UDP"); + /****************************************************************************** * 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 +73,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 +90,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 +152,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 +203,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..19da3fa1 --- /dev/null +++ b/lib/APPTurtle/src/Transports/CustomRosTransportUdp.h @@ -0,0 +1,144 @@ +/* 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 + *****************************************************************************/ + +/** + * 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; + +/** + * 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. + * + */ + ~CustomRosTransportUdp() + { + } + + /** + * Get protocol name used by this transport. + * @return Protocol name + */ + const String& getProtocolName() const final + { + return m_protocolName; + } + +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 */ + static const String m_protocolName; /**< This protocol name. */ + +}; + +/****************************************************************************** + * 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..b5cdec44 --- /dev/null +++ b/lib/WiFiNative/src/WiFiClient.h @@ -0,0 +1,153 @@ +/* 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 the actual demand. + */ +class WiFiClient +{ + public: + + /** + * Default constructor. + */ + WiFiClient() : + m_poll {SOCK_INVAL, POLLIN, 0} + { + } + + /** + * 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_poll.fd) ? 1U : 0U; + } + + /** + * Stop the WiF Client. + */ + void stop(); + + /** + * 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 or 0 on error. + */ + 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: + struct pollfd m_poll; /**< The poll/socket structure. */ + + 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. */ +}; + +/****************************************************************************** + * 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..a3649646 --- /dev/null +++ b/lib/WiFiNative/src/WifiClient.cpp @@ -0,0 +1,195 @@ +/* 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 = 0U; + + if (0U != connected()) + { + /* Connect is called on an already connected client. + * Handle it as re-connect by closing the former socket connection. + */ + stop(); + } + + if (SOCK_INVAL != (m_poll.fd = ::socket(AF_INET, SOCK_STREAM, IPPROTO_TCP))) + { + 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_poll.fd, reinterpret_cast(&serverAddr), sizeof(serverAddr))) + { + const int one = 1; + + 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 + { + 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 (0U != connected()) + { + if (0 != ::close(m_poll.fd)) + { + LOG_ERROR("%s:%s", "close", strerror(errno)); + } + + m_poll.fd = SOCK_INVAL; + } +} + +size_t WiFiClient::write(const uint8_t* buffer, size_t size) +{ + size_t remaining = size; + uint32_t retries = 0; + + if ((0U < size) && (0U != connected())) + { + while ((0U < remaining) && (retries < SOCK_WRITE_RETRY)) + { + ssize_t written = ::send(m_poll.fd, buffer, remaining, MSG_DONTWAIT); + if (0 > written) + { + 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; + } + } + + return size - remaining; +} + +int WiFiClient::read(uint8_t* buffer, size_t size) +{ + int retval = -1; + + 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); + if (-1 != result) + { + retval = result; /* Success! */ + } + else + { + LOG_ERROR("%s:%s", "recv", strerror(errno)); + } + } + else + { + retval = 0; + } + } + else + { + LOG_ERROR("%s:%s", "poll", strerror(errno)); + } + } + + return retval; +} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 4873778d..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 @@ -58,7 +82,8 @@ lib_ignore = MainTestNative UDPNative WiFiNative -extra_scripts = + WifiClientNative +extra_scripts = monitor_speed = 115200 monitor_filters = esp32_exception_decoder @@ -70,11 +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 + -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