From 3b59906a388f4745b7cabdcc289a048c8ea6eb87 Mon Sep 17 00:00:00 2001 From: Guo Date: Wed, 8 Jul 2026 13:22:48 -0500 Subject: [PATCH 01/17] copy from embeddedRTPS --- components/rtps_embedded/CMakeLists.txt | 32 + .../rtps_embedded/include/rtps/ThreadPool.h | 99 ++++ .../include/rtps/common/Locator.h | 196 ++++++ .../rtps_embedded/include/rtps/common/types.h | 297 ++++++++++ .../include/rtps/communication/PacketInfo.h | 61 ++ .../rtps/communication/TcpipCoreLock.h | 38 ++ .../rtps/communication/UdpConnection.h | 68 +++ .../include/rtps/communication/UdpDriver.h | 65 ++ .../rtps_embedded/include/rtps/config.h | 38 ++ .../include/rtps/config_desktop.h | 100 ++++ .../rtps_embedded/include/rtps/config_esp32.h | 101 ++++ .../include/rtps/discovery/BuiltInEndpoints.h | 45 ++ .../rtps/discovery/ParticipantProxyData.h | 187 ++++++ .../include/rtps/discovery/SEDPAgent.h | 120 ++++ .../include/rtps/discovery/SPDPAgent.h | 87 +++ .../include/rtps/discovery/TopicData.h | 109 ++++ .../include/rtps/entities/Domain.h | 98 +++ .../include/rtps/entities/Participant.h | 127 ++++ .../include/rtps/entities/Reader.h | 151 +++++ .../include/rtps/entities/ReaderProxy.h | 58 ++ .../include/rtps/entities/StatefulReader.h | 62 ++ .../include/rtps/entities/StatefulReader.tpp | 283 +++++++++ .../include/rtps/entities/StatefulWriter.h | 91 +++ .../include/rtps/entities/StatefulWriter.tpp | 559 ++++++++++++++++++ .../include/rtps/entities/StatelessReader.h | 44 ++ .../include/rtps/entities/StatelessWriter.h | 68 +++ .../include/rtps/entities/StatelessWriter.tpp | 228 +++++++ .../include/rtps/entities/Writer.h | 113 ++++ .../include/rtps/entities/WriterProxy.h | 77 +++ .../include/rtps/messages/MessageFactory.h | 219 +++++++ .../include/rtps/messages/MessageReceiver.h | 74 +++ .../include/rtps/messages/MessageTypes.h | 444 ++++++++++++++ components/rtps_embedded/include/rtps/rtps.h | 39 ++ .../include/rtps/storages/CacheChange.h | 68 +++ .../rtps/storages/HistoryCacheWithDeletion.h | 282 +++++++++ .../include/rtps/storages/MemoryPool.h | 192 ++++++ .../include/rtps/storages/PBufWrapper.h | 84 +++ .../rtps/storages/SimpleHistoryCache.h | 178 ++++++ .../rtps/storages/ThreadSafeCircularBuffer.h | 83 +++ .../storages/ThreadSafeCircularBuffer.tpp | 130 ++++ .../include/rtps/utils/Diagnostics.h | 85 +++ .../rtps_embedded/include/rtps/utils/Lock.h | 61 ++ .../rtps_embedded/include/rtps/utils/Log.h | 47 ++ .../include/rtps/utils/constants.h | 28 + .../rtps_embedded/include/rtps/utils/hash.h | 39 ++ .../include/rtps/utils/printutils.h | 29 + .../include/rtps/utils/sysFunctions.h | 42 ++ .../include/rtps/utils/udpUtils.h | 111 ++++ components/rtps_embedded/src/ThreadPool.cpp | 325 ++++++++++ .../src/communication/UdpDriver.cpp | 153 +++++ .../src/discovery/ParticipantProxyData.cpp | 216 +++++++ .../rtps_embedded/src/discovery/SEDPAgent.cpp | 516 ++++++++++++++++ .../rtps_embedded/src/discovery/SPDPAgent.cpp | 372 ++++++++++++ .../rtps_embedded/src/discovery/TopicData.cpp | 246 ++++++++ .../rtps_embedded/src/entities/Domain.cpp | 551 +++++++++++++++++ .../src/entities/Participant.cpp | 542 +++++++++++++++++ .../rtps_embedded/src/entities/Reader.cpp | 165 ++++++ .../src/entities/StatelessReader.cpp | 81 +++ .../rtps_embedded/src/entities/Writer.cpp | 147 +++++ .../src/messages/MessageReceiver.cpp | 239 ++++++++ .../src/messages/MessageTypes.cpp | 216 +++++++ components/rtps_embedded/src/rtps.cpp | 119 ++++ .../src/storages/PBufWrapper.cpp | 163 +++++ .../rtps_embedded/src/utils/Diagnostics.cpp | 53 ++ components/rtps_embedded/src/utils/Lock.cpp | 15 + 65 files changed, 9956 insertions(+) create mode 100644 components/rtps_embedded/CMakeLists.txt create mode 100644 components/rtps_embedded/include/rtps/ThreadPool.h create mode 100644 components/rtps_embedded/include/rtps/common/Locator.h create mode 100644 components/rtps_embedded/include/rtps/common/types.h create mode 100644 components/rtps_embedded/include/rtps/communication/PacketInfo.h create mode 100644 components/rtps_embedded/include/rtps/communication/TcpipCoreLock.h create mode 100644 components/rtps_embedded/include/rtps/communication/UdpConnection.h create mode 100644 components/rtps_embedded/include/rtps/communication/UdpDriver.h create mode 100644 components/rtps_embedded/include/rtps/config.h create mode 100644 components/rtps_embedded/include/rtps/config_desktop.h create mode 100644 components/rtps_embedded/include/rtps/config_esp32.h create mode 100644 components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h create mode 100644 components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h create mode 100644 components/rtps_embedded/include/rtps/discovery/SEDPAgent.h create mode 100644 components/rtps_embedded/include/rtps/discovery/SPDPAgent.h create mode 100644 components/rtps_embedded/include/rtps/discovery/TopicData.h create mode 100644 components/rtps_embedded/include/rtps/entities/Domain.h create mode 100644 components/rtps_embedded/include/rtps/entities/Participant.h create mode 100644 components/rtps_embedded/include/rtps/entities/Reader.h create mode 100644 components/rtps_embedded/include/rtps/entities/ReaderProxy.h create mode 100644 components/rtps_embedded/include/rtps/entities/StatefulReader.h create mode 100644 components/rtps_embedded/include/rtps/entities/StatefulReader.tpp create mode 100644 components/rtps_embedded/include/rtps/entities/StatefulWriter.h create mode 100644 components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp create mode 100644 components/rtps_embedded/include/rtps/entities/StatelessReader.h create mode 100644 components/rtps_embedded/include/rtps/entities/StatelessWriter.h create mode 100644 components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp create mode 100644 components/rtps_embedded/include/rtps/entities/Writer.h create mode 100644 components/rtps_embedded/include/rtps/entities/WriterProxy.h create mode 100644 components/rtps_embedded/include/rtps/messages/MessageFactory.h create mode 100644 components/rtps_embedded/include/rtps/messages/MessageReceiver.h create mode 100644 components/rtps_embedded/include/rtps/messages/MessageTypes.h create mode 100644 components/rtps_embedded/include/rtps/rtps.h create mode 100644 components/rtps_embedded/include/rtps/storages/CacheChange.h create mode 100644 components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h create mode 100644 components/rtps_embedded/include/rtps/storages/MemoryPool.h create mode 100644 components/rtps_embedded/include/rtps/storages/PBufWrapper.h create mode 100644 components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h create mode 100644 components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h create mode 100644 components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp create mode 100644 components/rtps_embedded/include/rtps/utils/Diagnostics.h create mode 100644 components/rtps_embedded/include/rtps/utils/Lock.h create mode 100644 components/rtps_embedded/include/rtps/utils/Log.h create mode 100644 components/rtps_embedded/include/rtps/utils/constants.h create mode 100644 components/rtps_embedded/include/rtps/utils/hash.h create mode 100644 components/rtps_embedded/include/rtps/utils/printutils.h create mode 100644 components/rtps_embedded/include/rtps/utils/sysFunctions.h create mode 100644 components/rtps_embedded/include/rtps/utils/udpUtils.h create mode 100644 components/rtps_embedded/src/ThreadPool.cpp create mode 100644 components/rtps_embedded/src/communication/UdpDriver.cpp create mode 100644 components/rtps_embedded/src/discovery/ParticipantProxyData.cpp create mode 100644 components/rtps_embedded/src/discovery/SEDPAgent.cpp create mode 100644 components/rtps_embedded/src/discovery/SPDPAgent.cpp create mode 100644 components/rtps_embedded/src/discovery/TopicData.cpp create mode 100644 components/rtps_embedded/src/entities/Domain.cpp create mode 100644 components/rtps_embedded/src/entities/Participant.cpp create mode 100644 components/rtps_embedded/src/entities/Reader.cpp create mode 100644 components/rtps_embedded/src/entities/StatelessReader.cpp create mode 100644 components/rtps_embedded/src/entities/Writer.cpp create mode 100644 components/rtps_embedded/src/messages/MessageReceiver.cpp create mode 100644 components/rtps_embedded/src/messages/MessageTypes.cpp create mode 100644 components/rtps_embedded/src/rtps.cpp create mode 100644 components/rtps_embedded/src/storages/PBufWrapper.cpp create mode 100644 components/rtps_embedded/src/utils/Diagnostics.cpp create mode 100644 components/rtps_embedded/src/utils/Lock.cpp diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt new file mode 100644 index 000000000..a3ee053d8 --- /dev/null +++ b/components/rtps_embedded/CMakeLists.txt @@ -0,0 +1,32 @@ +idf_component_register( + SRCS + "src/rtps.cpp" + "src/ThreadPool.cpp" + "src/communication/UdpDriver.cpp" + "src/discovery/ParticipantProxyData.cpp" + "src/discovery/SEDPAgent.cpp" + "src/discovery/SPDPAgent.cpp" + "src/discovery/TopicData.cpp" + "src/entities/Domain.cpp" + "src/entities/Participant.cpp" + "src/entities/Reader.cpp" + "src/entities/StatelessReader.cpp" + "src/entities/Writer.cpp" + "src/messages/MessageReceiver.cpp" + "src/messages/MessageTypes.cpp" + "src/storages/PBufWrapper.cpp" + "src/utils/Diagnostics.cpp" + "src/utils/Lock.cpp" + "thirdparty/Micro-CDR/src/c/common.c" + "thirdparty/Micro-CDR/src/c/types/array.c" + "thirdparty/Micro-CDR/src/c/types/basic.c" + "thirdparty/Micro-CDR/src/c/types/sequence.c" + "thirdparty/Micro-CDR/src/c/types/string.c" + INCLUDE_DIRS + "include" + REQUIRES + base_component cdr task socket +) + +# target_compile_features(${COMPONENT_LIB} PUBLIC cxx_std_17) +# target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER=\"rtps/config_esp32.h\") diff --git a/components/rtps_embedded/include/rtps/ThreadPool.h b/components/rtps_embedded/include/rtps/ThreadPool.h new file mode 100644 index 000000000..9ef5ed766 --- /dev/null +++ b/components/rtps_embedded/include/rtps/ThreadPool.h @@ -0,0 +1,99 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_THREADPOOL_H +#define RTPS_THREADPOOL_H + +#include "lwip/sys.h" +#include "rtps/communication/PacketInfo.h" +#include "rtps/communication/UdpDriver.h" +#include "rtps/config.h" +#include "rtps/storages/PBufWrapper.h" +#include "rtps/storages/ThreadSafeCircularBuffer.h" + +#include + +namespace rtps { + +class Writer; + +class ThreadPool { +public: + using receiveJumppad_fp = void (*)(void *callee, const PacketInfo &packet); + + ThreadPool(receiveJumppad_fp receiveCallback, void *callee); + + ~ThreadPool(); + + bool startThreads(); + void stopThreads(); + + void clearQueues(); + bool addWorkload(Writer *workload); + bool addNewPacket(PacketInfo &&packet); + + static void readCallback(void *arg, udp_pcb *pcb, pbuf *p, + const ip_addr_t *addr, Ip4Port_t port); + + bool addBuiltinPort(const Ip4Port_t &port); + +private: + receiveJumppad_fp m_receiveJumppad; + void *m_callee; + bool m_running = false; + std::array m_writers; + std::array m_readers; + + std::array m_builtinPorts; + size_t m_builtinPortsIdx = 0; + + sys_sem_t m_readerNotificationSem; + sys_sem_t m_writerNotificationSem; + + void updateDiagnostics(); + + using BufferUsertrafficOutgoing = ThreadSafeCircularBuffer< + Writer *, Config::THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC>; + using BufferMetatrafficOutgoing = ThreadSafeCircularBuffer< + Writer *, Config::THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC>; + using BufferUsertrafficIncoming = ThreadSafeCircularBuffer< + PacketInfo, Config::THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC>; + using BufferMetatrafficIncoming = ThreadSafeCircularBuffer< + PacketInfo, Config::THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC>; + + BufferUsertrafficOutgoing m_outgoingUserTraffic; + BufferMetatrafficOutgoing m_outgoingMetaTraffic; + + BufferUsertrafficIncoming m_incomingUserTraffic; + BufferMetatrafficIncoming m_incomingMetaTraffic; + + bool isBuiltinPort(const Ip4Port_t &port); + static void writerThreadFunction(void *arg); + static void readerThreadFunction(void *arg); + void doWriterWork(); + void doReaderWork(); +}; +} // namespace rtps + +#endif // RTPS_THREADPOOL_H diff --git a/components/rtps_embedded/include/rtps/common/Locator.h b/components/rtps_embedded/include/rtps/common/Locator.h new file mode 100644 index 000000000..b1c35df7e --- /dev/null +++ b/components/rtps_embedded/include/rtps/common/Locator.h @@ -0,0 +1,196 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_LOCATOR_T_H +#define RTPS_LOCATOR_T_H + +#include "rtps/communication/UdpDriver.h" +#include "rtps/utils/udpUtils.h" +#include "ucdr/microcdr.h" + +#include "lwip/netif.h" + +#include + +namespace rtps { + +inline std::array getLocalIp4AddressBytes() { + std::array ip = {Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], + Config::IP_ADDRESS[2], Config::IP_ADDRESS[3]}; + if (netif_default != nullptr) { + const ip4_addr_t *iface_ip = netif_ip4_addr(netif_default); + if (iface_ip != nullptr) { + ip[0] = ip4_addr1(iface_ip); + ip[1] = ip4_addr2(iface_ip); + ip[2] = ip4_addr3(iface_ip); + ip[3] = ip4_addr4(iface_ip); + } + } + return ip; +} + +enum class LocatorKind_t : int32_t { + LOCATOR_KIND_INVALID = -1, + LOCATOR_KIND_RESERVED = 0, + LOCATOR_KIND_UDPv4 = 1, + LOCATOR_KIND_UDPv6 = 2 +}; + +const uint32_t LOCATOR_PORT_INVALID = 0; +const std::array LOCATOR_ADDRESS_INVALID = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + +/* + * This representation corresponds to the RTPS wire format + */ +struct FullLengthLocator { + LocatorKind_t kind = LocatorKind_t::LOCATOR_KIND_INVALID; + uint32_t port = LOCATOR_PORT_INVALID; + std::array address = + LOCATOR_ADDRESS_INVALID; // TODO make private such that kind and address + // always match? + + static FullLengthLocator createUDPv4Locator(uint8_t a, uint8_t b, uint8_t c, + uint8_t d, uint32_t port) { + FullLengthLocator locator; + locator.kind = LocatorKind_t::LOCATOR_KIND_UDPv4; + locator.address = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, a, b, c, d}; + locator.port = port; + return locator; + } + + void setInvalid() { kind = LocatorKind_t::LOCATOR_KIND_INVALID; } + + bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; } + + bool readFromUcdrBuffer(ucdrBuffer &buffer) { + if (ucdr_buffer_remaining(&buffer) < sizeof(FullLengthLocator)) { + return false; + } else { + ucdr_deserialize_array_uint8_t(&buffer, reinterpret_cast(this), + sizeof(FullLengthLocator)); + return true; + } + } + + bool serializeIntoUdcrBuffer(ucdrBuffer &buffer) { + if (ucdr_buffer_remaining(&buffer) < sizeof(FullLengthLocator)) { + return false; + } else { + ucdr_serialize_array_uint8_t(&buffer, reinterpret_cast(this), + sizeof(FullLengthLocator)); + } + } + + ip4_addr_t getIp4Address() const { + return transformIP4ToU32(address[12], address[13], address[14], + address[15]); + } + + bool isSameAddress(ip4_addr_t *address) { + ip4_addr_t ownaddress = getIp4Address(); + return ip4_addr_cmp(&ownaddress, address); + } + + inline bool isSameSubnet() const { + return UdpDriver::isSameSubnet(getIp4Address()); + } + + inline bool isMulticastAddress() const { + return UdpDriver::isMulticastAddress(getIp4Address()); + } + + inline uint32_t getLocatorPort() { return static_cast(port); } + +} __attribute__((packed)); + +inline FullLengthLocator +getBuiltInUnicastLocator(ParticipantId_t participantId) { + const auto ip = getLocalIp4AddressBytes(); + return FullLengthLocator::createUDPv4Locator( + ip[0], ip[1], ip[2], ip[3], getBuiltInUnicastPort(participantId)); +} + +inline FullLengthLocator getBuiltInMulticastLocator() { + return FullLengthLocator::createUDPv4Locator(239, 255, 0, 1, + getBuiltInMulticastPort()); +} + +inline FullLengthLocator getUserUnicastLocator(ParticipantId_t participantId) { + const auto ip = getLocalIp4AddressBytes(); + return FullLengthLocator::createUDPv4Locator( + ip[0], ip[1], ip[2], ip[3], getUserUnicastPort(participantId)); +} + +inline FullLengthLocator +getUserMulticastLocator() { // this would be a unicastaddress, as + // defined in config + const auto ip = getLocalIp4AddressBytes(); + return FullLengthLocator::createUDPv4Locator( + ip[0], ip[1], ip[2], ip[3], getUserMulticastPort()); +} + +inline FullLengthLocator getDefaultSendMulticastLocator() { + return FullLengthLocator::createUDPv4Locator(239, 255, 0, 1, + getBuiltInMulticastPort()); +} + +/* + * This representation omits unnecessary 12 bytes of the full RTPS wire format + */ +struct LocatorIPv4 { + LocatorKind_t kind = LocatorKind_t::LOCATOR_KIND_INVALID; + std::array address = {0}; + uint32_t port = LOCATOR_PORT_INVALID; + + LocatorIPv4() = default; + LocatorIPv4(const FullLengthLocator &locator) { + address[0] = locator.address[12]; + address[1] = locator.address[13]; + address[2] = locator.address[14]; + address[3] = locator.address[15]; + port = locator.port; + kind = locator.kind; + } + + ip4_addr_t getIp4Address() const { + return transformIP4ToU32(address[0], address[1], address[2], address[3]); + } + + void setInvalid() { kind = LocatorKind_t::LOCATOR_KIND_INVALID; } + + bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; } + + inline bool isSameSubnet() const { + return UdpDriver::isSameSubnet(getIp4Address()); + } + + inline bool isMulticastAddress() const { + return UdpDriver::isMulticastAddress(getIp4Address()); + } +}; + +} // namespace rtps + +#endif // RTPS_LOCATOR_T_H diff --git a/components/rtps_embedded/include/rtps/common/types.h b/components/rtps_embedded/include/rtps/common/types.h new file mode 100644 index 000000000..35123c0da --- /dev/null +++ b/components/rtps_embedded/include/rtps/common/types.h @@ -0,0 +1,297 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_TYPES_H +#define RTPS_TYPES_H + +#include "lwip/ip4_addr.h" + +#include +#include +#include +#include + +// TODO subnamespaces +namespace rtps { + +// TODO move types to where they are needed! + +typedef uint16_t Ip4Port_t; +typedef uint16_t DataSize_t; +typedef int8_t ParticipantId_t; // With UDP only 120 possible + +enum class EntityKind_t : uint8_t { + USER_DEFINED_UNKNOWN = 0x00, + // No user define participant + USER_DEFINED_WRITER_WITH_KEY = 0x02, + USER_DEFINED_WRITER_WITHOUT_KEY = 0x03, + USER_DEFINED_READER_WITHOUT_KEY = 0x04, + USER_DEFINED_READER_WITH_KEY = 0x07, + + BUILD_IN_UNKNOWN = 0xc0, + BUILD_IN_PARTICIPANT = 0xc1, + BUILD_IN_WRITER_WITH_KEY = 0xc2, + BUILD_IN_WRITER_WITHOUT_KEY = 0xc3, + BUILD_IN_READER_WITHOUT_KEY = 0xc4, + BUILD_IN_READER_WITH_KEY = 0xc7, + + VENDOR_SPEC_UNKNOWN = 0x40, + VENDOR_SPEC_PARTICIPANT = 0x41, + VENDOR_SPEC_WRITER_WITH_KEY = 0x42, + VENDOR_SPEC_WRITER_WITHOUT_KEY = 0x43, + VENDOR_SPEC_READER_WITHOUT_KEY = 0x44, + VENDOR_SPEC_READER_WITH_KEY = 0x47 +}; + +enum class TopicKind_t : uint8_t { NO_KEY = 1, WITH_KEY = 2 }; + +enum class ChangeKind_t : uint8_t { + INVALID, + ALIVE, + NOT_ALIVE_DISPOSED, + NOT_ALIVE_UNREGISTERED +}; + +enum class ReliabilityKind_t : uint32_t { + BEST_EFFORT = 1, + RELIABLE = 2 // Specification says 3 but eprosima sends 2 +}; + +enum class DurabilityKind_t : uint32_t { + VOLATILE = 0, + TRANSIENT_LOCAL = 1, + TRANSIENT = 2, + PERSISTENT = 3 +}; + +struct GuidPrefix_t { + std::array id; + + bool operator==(const GuidPrefix_t &other) const { + return this->id == other.id; + } +}; + +struct EntityId_t { + std::array entityKey; + EntityKind_t entityKind; + + bool operator==(const EntityId_t &other) const { + return this->entityKey == other.entityKey && + this->entityKind == other.entityKind; + } + + bool operator!=(const EntityId_t &other) const { return !(*this == other); } +}; + +struct Guid_t { + GuidPrefix_t prefix; + EntityId_t entityId; + + bool operator==(const Guid_t &other) const { + return this->prefix == other.prefix && this->entityId == other.entityId; + } + + static uint32_t sum(const Guid_t &other) { + uint32_t ret = 0; + for (const auto &i : other.prefix.id) { + ret += i; + } + for (const auto &i : other.entityId.entityKey) { + ret += i; + } + return ret; + } +}; + +// Described as long but there wasn't any definition. Other than 32 bit does not +// conform the default values +struct Time_t { + int32_t seconds; // time in seconds + uint32_t fraction; // time in sec/2^32 (?) + + static Time_t create(int32_t s, uint32_t ns) { + static constexpr double factor = + (static_cast(1) << 32) / 1000000000.; + auto fraction = static_cast(ns * factor); + return Time_t{s, fraction}; + } +}; + +struct VendorId_t { + std::array vendorId; +}; + +struct SequenceNumber_t { + int32_t high; + uint32_t low; + + bool operator==(const SequenceNumber_t &other) const { + return high == other.high && low == other.low; + } + + bool operator!=(const SequenceNumber_t &other) const { + return !(*this == other); + } + + bool operator<(const SequenceNumber_t &other) const { + return high < other.high || (high == other.high && low < other.low); + } + + bool operator>(const SequenceNumber_t &other) const { + return high > other.high || (high == other.high && low > other.low); + } + + bool operator<=(const SequenceNumber_t &other) const { + return *this == other || *this < other; + } + + SequenceNumber_t &operator++() { + ++low; + if (low == 0) { + ++high; + } + return *this; + } + + SequenceNumber_t &operator--() { + if (low == 0) { + --high; + low = std::numeric_limits::max(); + } else { + --low; + } + + return *this; + } + + SequenceNumber_t operator++(int) { + SequenceNumber_t tmp(*this); + ++*this; + return tmp; + } +}; + +#define SNS_MAX_NUM_BITS 256 +#define SNS_NUM_BYTES (SNS_MAX_NUM_BITS / 8) +static_assert(!(SNS_MAX_NUM_BITS % 32) && SNS_MAX_NUM_BITS != 0, + "SNS_MAX_NUM_BITS must be multiple of 32"); + +struct SequenceNumberSet { + + SequenceNumberSet() = default; + explicit SequenceNumberSet(const SequenceNumber_t &firstMissing) + : base(firstMissing) {} + + SequenceNumber_t base = {0, 0}; + // Cannot be static because of packed + uint32_t numBits = 0; + std::array bitMap{}; + + // We only need 1 byte because atm we don't store packets. + bool isSet(uint32_t bit) const { + if (bit >= SNS_MAX_NUM_BITS) { + return true; + } + const auto bucket = static_cast(bit / 32); + const auto pos = static_cast(bit % 32); + return (bitMap[bucket] & (1 << (31 - pos))) != 0; + } +}; + +struct FragmentNumber_t { + uint32_t value; +}; + +struct Count_t { + int32_t value; +}; + +struct ProtocolVersion_t { + uint8_t major; + uint8_t minor; +}; + +typedef Time_t Duration_t; // TODO + +enum class ChangeForReaderStatusKind { + UNSENT, + UNACKNOWLEDGED, + REQURESTED, + ACKNOWLEDGED, + UNDERWAY +}; + +enum class ChangeFromWriterStatusKind { LOST, MISSING, RECEIVED, UNKNOWN }; + +struct InstanceHandle_t { // TODO + uint64_t value; +}; + +struct ParticipantMessageData { // TODO +}; + +/* Default Values */ +const EntityId_t ENTITYID_UNKNOWN{}; +const EntityId_t ENTITYID_BUILD_IN_PARTICIPANT = { + {00, 00, 01}, EntityKind_t::BUILD_IN_PARTICIPANT}; +const EntityId_t ENTITYID_SEDP_BUILTIN_TOPIC_WRITER = { + {00, 00, 02}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_TOPIC_READER = { + {00, 00, 02}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER = { + {00, 00, 03}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER = { + {00, 00, 03}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER = { + {00, 00, 04}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER = { + {00, 00, 04}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER = { + {00, 01, 00}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER = { + {00, 01, 00}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; +const EntityId_t ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER = { + {00, 02, 00}, EntityKind_t::BUILD_IN_WRITER_WITH_KEY}; +const EntityId_t ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER = { + {00, 02, 00}, EntityKind_t::BUILD_IN_READER_WITH_KEY}; + +const GuidPrefix_t GUIDPREFIX_UNKNOWN{}; +const Guid_t GUID_UNKNOWN{}; +const GuidPrefix_t GUID_RANDOM{9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9}; + +const ParticipantId_t PARTICIPANT_ID_INVALID = -1; + +const ProtocolVersion_t PROTOCOLVERSION = {2, 2}; + +const SequenceNumber_t SEQUENCENUMBER_UNKNOWN = {-1, 0}; + +const Time_t TIME_ZERO = {}; +const Time_t TIME_INVALID = {-1, 0xFFFFFFFF}; +const Time_t TIME_INFINITY = {0x7FFFFFFF, 0xFFFFFFFF}; + +const VendorId_t VENDOR_UNKNOWN = {}; +} // namespace rtps + +#endif // RTPS_TYPES_H diff --git a/components/rtps_embedded/include/rtps/communication/PacketInfo.h b/components/rtps_embedded/include/rtps/communication/PacketInfo.h new file mode 100644 index 000000000..240158a3c --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/PacketInfo.h @@ -0,0 +1,61 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#ifndef RTPS_PACKETINFO_H +#define RTPS_PACKETINFO_H + +#include "rtps/common/types.h" +#include "rtps/storages/PBufWrapper.h" + +namespace rtps { + +struct PacketInfo { + Ip4Port_t srcPort; // TODO Do we need that? + ip4_addr_t destAddr; + Ip4Port_t destPort; + PBufWrapper buffer; + + void copyTriviallyCopyable(const PacketInfo &other) { + this->srcPort = other.srcPort; + this->destPort = other.destPort; + this->destAddr = other.destAddr; + } + + PacketInfo() = default; + ~PacketInfo() = default; + + PacketInfo &operator=(const PacketInfo &other) = delete; + + PacketInfo &operator=(PacketInfo &&other) noexcept { + copyTriviallyCopyable(other); + this->buffer = std::move(other.buffer); + return *this; + } +}; +} // namespace rtps + +#endif // RTPS_PACKETINFO_H diff --git a/components/rtps_embedded/include/rtps/communication/TcpipCoreLock.h b/components/rtps_embedded/include/rtps/communication/TcpipCoreLock.h new file mode 100644 index 000000000..fcd26bb05 --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/TcpipCoreLock.h @@ -0,0 +1,38 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_TCPIPCORELOCK_H +#define RTPS_TCPIPCORELOCK_H + +#include + +namespace rtps { +class TcpipCoreLock { +public: + TcpipCoreLock() { LOCK_TCPIP_CORE(); } + ~TcpipCoreLock() { UNLOCK_TCPIP_CORE(); } +}; +} // namespace rtps + +#endif // RTPS_TCPIPCORELOCK_H diff --git a/components/rtps_embedded/include/rtps/communication/UdpConnection.h b/components/rtps_embedded/include/rtps/communication/UdpConnection.h new file mode 100644 index 000000000..b95ac6fe6 --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/UdpConnection.h @@ -0,0 +1,68 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_UDPCONNECTION_H +#define RTPS_UDPCONNECTION_H + +#include "TcpipCoreLock.h" +#include "lwip/udp.h" + +namespace rtps { + +struct UdpConnection { + udp_pcb *pcb = nullptr; + uint16_t port = 0; + + UdpConnection() = default; // Required for static allocation + + explicit UdpConnection(uint16_t port) : port(port) { + LOCK_TCPIP_CORE(); + pcb = udp_new(); + UNLOCK_TCPIP_CORE(); + } + + UdpConnection &operator=(UdpConnection &&other) noexcept { + port = other.port; + + if (pcb != nullptr) { + LOCK_TCPIP_CORE(); + udp_remove(pcb); + UNLOCK_TCPIP_CORE(); + } + pcb = other.pcb; + other.pcb = nullptr; + return *this; + } + + ~UdpConnection() { + if (pcb != nullptr) { + LOCK_TCPIP_CORE(); + udp_remove(pcb); + UNLOCK_TCPIP_CORE(); + pcb = nullptr; + } + } +}; +} // namespace rtps +#endif // RTPS_UDPCONNECTION_H diff --git a/components/rtps_embedded/include/rtps/communication/UdpDriver.h b/components/rtps_embedded/include/rtps/communication/UdpDriver.h new file mode 100644 index 000000000..80fcd399c --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/UdpDriver.h @@ -0,0 +1,65 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_UDPDRIVER_H +#define RTPS_UDPDRIVER_H + +#include "UdpConnection.h" +#include "lwip/udp.h" +#include "rtps/common/types.h" +#include "rtps/communication/PacketInfo.h" +#include "rtps/config.h" +#include "rtps/storages/PBufWrapper.h" + +#include + +namespace rtps { + +class UdpDriver { + +public: + typedef void (*udpRxFunc_fp)(void *arg, udp_pcb *pcb, pbuf *p, + const ip_addr_t *addr, Ip4Port_t port); + + UdpDriver(udpRxFunc_fp callback, void *args); + + const rtps::UdpConnection *createUdpConnection(Ip4Port_t receivePort); + bool joinMultiCastGroup(ip4_addr_t addr) const; + void sendPacket(PacketInfo &info); + + static bool isSameSubnet(ip4_addr_t addr); + static bool isMulticastAddress(ip4_addr_t addr); + +private: + std::array m_conns; + std::size_t m_numConns = 0; + udpRxFunc_fp m_rxCallback = nullptr; + void *m_callbackArgs = nullptr; + + bool sendPacket(const UdpConnection &conn, ip4_addr_t &destAddr, + Ip4Port_t destPort, pbuf &buffer); +}; +} // namespace rtps + +#endif // RTPS_UDPDRIVER_H diff --git a/components/rtps_embedded/include/rtps/config.h b/components/rtps_embedded/include/rtps/config.h new file mode 100644 index 000000000..89b8aac94 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config.h @@ -0,0 +1,38 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_H +#define RTPS_CONFIG_H + +#ifdef RTPS_CONFIG_HEADER +#include RTPS_CONFIG_HEADER +#else +#if defined(ESP_PLATFORM) +#include "rtps/config_esp32.h" +#else +#include "rtps/config_stm.h" +#endif +#endif + +#endif // RTPS_CONFIG_H diff --git a/components/rtps_embedded/include/rtps/config_desktop.h b/components/rtps_embedded/include/rtps/config_desktop.h new file mode 100644 index 000000000..0d011e7e6 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config_desktop.h @@ -0,0 +1,100 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_H +#define RTPS_CONFIG_H + +#include "rtps/common/types.h" + +namespace rtps { + +#define IS_LITTLE_ENDIAN 1 + +namespace Config { +const VendorId_t VENDOR_ID = {13, 37}; +const std::array IP_ADDRESS = { + 192, 168, 1, 2}; // Needs to be set in lwipcfg.h too. +const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 9}; + +const uint8_t DOMAIN_ID = 0; // 230 possible with UDP +const uint8_t MAX_NUM_PARTICIPANTS = 2; +const uint8_t NUM_STATELESS_WRITERS = + MAX_NUM_PARTICIPANTS + 1; // Required + Additional +const uint8_t NUM_STATELESS_READERS = + MAX_NUM_PARTICIPANTS + 1; // Required + Additional +const uint8_t NUM_STATEFUL_READERS = + 4; // 1-4 required per participant depending on what they do and to whom + // they match +const uint8_t NUM_STATEFUL_WRITERS = + 4; // 1-4 required per participant depending on what they do and to whom + // they match +const uint8_t NUM_WRITERS_PER_PARTICIPANT = 4; +const uint8_t NUM_READERS_PER_PARTICIPANT = 4; +const uint8_t NUM_WRITER_PROXIES_PER_READER = 3; +const uint8_t NUM_READER_PROXIES_PER_WRITER = 3; + +const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 100; +const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 10; + +const uint8_t HISTORY_SIZE = 10; + +const uint8_t MAX_TYPENAME_LENGTH = 20; +const uint8_t MAX_TOPICNAME_LENGTH = 20; + +const int HEARTBEAT_STACKSIZE = 1200; // byte +const int THREAD_POOL_WRITER_STACKSIZE = 1100; // byte +const int THREAD_POOL_READER_STACKSIZE = 1600; // byte +const uint16_t SPDP_WRITER_STACKSIZE = 550; // byte + +const uint16_t SF_WRITER_HB_PERIOD_MS = 500; +const uint16_t SPDP_RESEND_PERIOD_MS = 10000; +const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = + 2; // skip x SPDP rounds before checking liveliness +const uint8_t SPDP_WRITER_PRIO = 3; +const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 5; +const uint8_t SPDP_MAX_NUM_LOCATORS = 5; +const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = { + 100, 0}; // Default lease duration for remote participants, usually + // overwritten by remote info +const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { + 180, + 0}; // Absolute maximum lease duration, ignoring remote participant info + +const int MAX_NUM_UDP_CONNECTIONS = 10; + +const int THREAD_POOL_NUM_WRITERS = 2; +const int THREAD_POOL_NUM_READERS = 2; +const int THREAD_POOL_WRITER_PRIO = 3; +const int THREAD_POOL_READER_PRIO = 3; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH = 10; + +constexpr int OVERALL_HEAP_SIZE = + THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE + + THREAD_POOL_NUM_READERS * THREAD_POOL_READER_STACKSIZE + + MAX_NUM_PARTICIPANTS * SPDP_WRITER_STACKSIZE + + NUM_STATEFUL_WRITERS * HEARTBEAT_STACKSIZE; +} // namespace Config +} // namespace rtps + +#endif // RTPS_CONFIG_H diff --git a/components/rtps_embedded/include/rtps/config_esp32.h b/components/rtps_embedded/include/rtps/config_esp32.h new file mode 100644 index 000000000..5cdd2fff8 --- /dev/null +++ b/components/rtps_embedded/include/rtps/config_esp32.h @@ -0,0 +1,101 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONFIG_ESP32_H +#define RTPS_CONFIG_ESP32_H + +#include "rtps/common/types.h" + +namespace rtps { + +#define IS_LITTLE_ENDIAN 1 +#define OS_IS_FREERTOS + +namespace Config { +const VendorId_t VENDOR_ID = {13, 37}; +const std::array IP_ADDRESS = { + 192, 168, 4, 1}; // Fallback: must match DHCPS server netif IP. +const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 13}; + +const uint8_t DOMAIN_ID = 0; // 230 possible with UDP +const uint8_t NUM_STATELESS_WRITERS = 5; +const uint8_t NUM_STATELESS_READERS = 5; +const uint8_t NUM_STATEFUL_READERS = 2; +const uint8_t NUM_STATEFUL_WRITERS = 2; +const uint8_t MAX_NUM_PARTICIPANTS = 1; +const uint8_t NUM_WRITERS_PER_PARTICIPANT = 10; +const uint8_t NUM_READERS_PER_PARTICIPANT = 10; +const uint8_t NUM_WRITER_PROXIES_PER_READER = 6; +const uint8_t NUM_READER_PROXIES_PER_WRITER = 6; + +const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 50; +const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 50; + +const uint8_t MAX_NUM_READER_CALLBACKS = 5; + +const uint8_t HISTORY_SIZE_STATELESS = 2; +const uint8_t HISTORY_SIZE_STATEFUL = 10; + +const uint8_t MAX_TYPENAME_LENGTH = 64; +const uint8_t MAX_TOPICNAME_LENGTH = 64; + +const int HEARTBEAT_STACKSIZE = 3072; // byte +const int THREAD_POOL_WRITER_STACKSIZE = 4096; // byte +const int THREAD_POOL_READER_STACKSIZE = 4096; // byte +const uint16_t SPDP_WRITER_STACKSIZE = 2048; // byte + +const uint16_t SF_WRITER_HB_PERIOD_MS = 4000; +const uint16_t SPDP_RESEND_PERIOD_MS = 250; +const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = + 2; // skip x SPDP rounds before checking liveliness +const uint8_t SPDP_WRITER_PRIO = 5; +const uint8_t SPDP_MAX_NUMBER_FOUND_PARTICIPANTS = 10; +const uint8_t SPDP_MAX_NUM_LOCATORS = 1; +const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = { + 5, 0}; // Default lease duration for remote participants, usually + // overwritten by remote info +const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { + 90, + 0}; // Absolute maximum lease duration, ignoring remote participant info + +const Duration_t SPDP_LEASE_DURATION = {5, 0}; + +const int MAX_NUM_UDP_CONNECTIONS = 10; + +const int THREAD_POOL_NUM_WRITERS = 1; +const int THREAD_POOL_NUM_READERS = 1; +const int THREAD_POOL_WRITER_PRIO = 5; +const int THREAD_POOL_READER_PRIO = 5; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC = 60; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC = 60; + +constexpr int OVERALL_HEAP_SIZE = + THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE + + THREAD_POOL_NUM_READERS * THREAD_POOL_READER_STACKSIZE + + MAX_NUM_PARTICIPANTS * SPDP_WRITER_STACKSIZE + + NUM_STATEFUL_WRITERS * HEARTBEAT_STACKSIZE; +} // namespace Config +} // namespace rtps + +#endif // RTPS_CONFIG_ESP32_H diff --git a/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h new file mode 100644 index 000000000..eb3a32c86 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h @@ -0,0 +1,45 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_BUILTINENDPOINTS_H +#define RTPS_BUILTINENDPOINTS_H + +#include "rtps/entities/StatefulReader.h" +#include "rtps/entities/StatelessReader.h" +#include "rtps/entities/StatelessWriter.h" +#include "rtps/entities/Writer.h" + +namespace rtps { + +struct BuiltInEndpoints { + Writer *spdpWriter = nullptr; + Reader *spdpReader = nullptr; + Writer *sedpPubWriter = nullptr; + Reader *sedpPubReader = nullptr; + Writer *sedpSubWriter = nullptr; + Reader *sedpSubReader = nullptr; +}; +} // namespace rtps + +#endif // RTPS_BUILTINENDPOINTS_H diff --git a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h new file mode 100644 index 000000000..6b251bd02 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h @@ -0,0 +1,187 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PARTICIPANTPROXYDATA_H +#define RTPS_PARTICIPANTPROXYDATA_H + +#include "rtps/config.h" +#include "rtps/messages/MessageTypes.h" +#include "ucdr/microcdr.h" +#include +#if defined(unix) || defined(__unix__) +#include +#endif +#include + +namespace rtps { + +class Participant; +using SMElement::ParameterId; + +typedef uint32_t BuiltinEndpointSet_t; + +class ParticipantProxyData { +public: + ParticipantProxyData() { onAliveSignal(); } + ParticipantProxyData(Guid_t guid); + + ProtocolVersion_t m_protocolVersion = PROTOCOLVERSION; + Guid_t m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; + VendorId_t m_vendorId = VENDOR_UNKNOWN; + bool m_expectsInlineQos = false; + BuiltinEndpointSet_t m_availableBuiltInEndpoints; + std::array + m_metatrafficUnicastLocatorList; + std::array + m_metatrafficMulticastLocatorList; + std::array + m_defaultUnicastLocatorList; + std::array + m_defaultMulticastLocatorList; + Count_t m_manualLivelinessCount{1}; + Duration_t m_leaseDuration = Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION; +#if defined(unix) || defined(__unix__) + std::chrono::time_point + m_lastLivelinessReceivedTimestamp; +#else + TickType_t m_lastLivelinessReceivedTickCount = 0; +#endif + void reset(); + + bool readFromUcdrBuffer(ucdrBuffer &buffer, Participant *participant); + + inline bool hasParticipantWriter(); + inline bool hasParticipantReader(); + inline bool hasPublicationWriter(); + inline bool hasPublicationReader(); + inline bool hasSubscriptionWriter(); + inline bool hasSubscriptionReader(); + + inline void onAliveSignal(); + inline bool isAlive(); + inline uint32_t getAliveSignalAgeInMilliseconds(); + +private: + bool readLocatorIntoList( + ucdrBuffer &buffer, + std::array &list); + + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER = 1 << 0; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR = + 1 << 1; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PUBLICATION_ANNOUNCER = 1 << 2; + static const BuiltinEndpointSet_t DISC_BUILTIN_ENDPOINT_PUBLICATION_DETECTOR = + 1 << 3; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_ANNOUNCER = 1 << 4; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_DETECTOR = 1 << 5; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_PROXY_ANNOUNCER = 1 << 6; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_PROXY_DETECTOR = 1 << 7; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_STATE_ANNOUNCER = 1 << 8; + static const BuiltinEndpointSet_t + DISC_BUILTIN_ENDPOINT_PARTICIPANT_STATE_DETECTOR = 1 << 9; + static const BuiltinEndpointSet_t + BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_WRITER = 1 << 10; + static const BuiltinEndpointSet_t + BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_READER = 1 << 11; +}; + +// Needs to be in header because they are marked with inline +bool ParticipantProxyData::hasParticipantWriter() { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER) == 1; +} + +bool ParticipantProxyData::hasParticipantReader() { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR) != 0; +} + +bool ParticipantProxyData::hasPublicationWriter() { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_PUBLICATION_ANNOUNCER) != 0; +} + +bool ParticipantProxyData::hasPublicationReader() { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_PUBLICATION_DETECTOR) != 0; +} + +bool ParticipantProxyData::hasSubscriptionWriter() { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_ANNOUNCER) != 0; +} + +bool ParticipantProxyData::hasSubscriptionReader() { + return (m_availableBuiltInEndpoints & + DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_DETECTOR) != 0; +} + +void ParticipantProxyData::onAliveSignal() { +#if defined(unix) || defined(__unix__) + m_lastLivelinessReceivedTimestamp = std::chrono::high_resolution_clock::now(); +#else + m_lastLivelinessReceivedTickCount = xTaskGetTickCount(); +#endif +} + +uint32_t ParticipantProxyData::getAliveSignalAgeInMilliseconds() { +#if defined(unix) || defined(__unix__) + auto now = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration = + now - m_lastLivelinessReceivedTimestamp; + return duration.count(); +#else + return (xTaskGetTickCount() - m_lastLivelinessReceivedTickCount) * + (1000 / configTICK_RATE_HZ); +#endif +} + +/* + * Returns true if last heartbeat within lease duration, else false + */ +bool ParticipantProxyData::isAlive() { + uint32_t lease_in_ms = + m_leaseDuration.seconds * 1000 + m_leaseDuration.fraction * 1e-6; + + uint32_t max_lease_in_ms = + Config::SPDP_MAX_REMOTE_LEASE_DURATION.seconds * 1000 + + Config::SPDP_MAX_REMOTE_LEASE_DURATION.fraction * 1e-6; + + auto heatbeat_age_in_ms = getAliveSignalAgeInMilliseconds(); + + if (heatbeat_age_in_ms > std::min(lease_in_ms, max_lease_in_ms)) { + return false; + } + return true; +} + +} // namespace rtps +#endif // RTPS_PARTICIPANTPROXYDATA_H diff --git a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h new file mode 100644 index 000000000..4b6928c97 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h @@ -0,0 +1,120 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_SEDPAGENT_H +#define RTPS_SEDPAGENT_H + +#include "rtps/discovery/BuiltInEndpoints.h" +#include "rtps/discovery/TopicData.h" + +namespace rtps { + +class Participant; +class ReaderCacheChange; +class Writer; +class Reader; + +class SEDPAgent { +public: + void init(Participant &part, const BuiltInEndpoints &endpoints); + bool addWriter(Writer &writer); + bool addReader(Reader &reader); + bool deleteReader(Reader *reader); + bool deleteWriter(Writer *reader); + + void registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), + void *args); + void registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), + void *args); + void removeUnmatchedEntitiesOfParticipant(const GuidPrefix_t &guidPrefix); + void removeUnmatchedEntity(const Guid_t &guid); + + uint32_t getNumRemoteUnmatchedReaders(); + uint32_t getNumRemoteUnmatchedWriters(); + +protected: // For testing purposes + void handlePublisherReaderMessage(const TopicData &writerData, + const ReaderCacheChange &change); + void handleSubscriptionReaderMessage(const TopicData &writerData, + const ReaderCacheChange &change); + +private: + Participant *m_part; + SemaphoreHandle_t m_mutex; + uint8_t m_buffer[600]; // TODO check size, currently changed from 300 to 600 + // (FastDDS gives too many options) + BuiltInEndpoints m_endpoints; + /* + * If we add readers later on, remote participants will not send matching + * writer proxies again (and vice versa). This is done only once during + * discovery. Therefore, we need to keep track of remote endpoints. Topic and + * type are represented as hash values to save memory. + */ + MemoryPool + m_unmatchedRemoteWriters; + size_t m_numUnmatchedRemoteWriters = 0; + MemoryPool + m_unmatchedRemoteReaders; + size_t m_numMatchedRemoteReaders = 0; + + void tryMatchUnmatchedEndpoints(); + void addUnmatchedRemoteWriter(const TopicData &writerData); + void addUnmatchedRemoteReader(const TopicData &readerData); + void addUnmatchedRemoteWriter(const TopicDataCompressed &writerData); + void addUnmatchedRemoteReader(const TopicDataCompressed &readerData); + + void handleRemoteEndpointDeletion(const TopicData &topic, + const ReaderCacheChange &change); + + void (*mfp_onNewPublisherCallback)(void *arg) = nullptr; + void *m_onNewPublisherArgs = nullptr; + void (*mfp_onNewSubscriberCallback)(void *arg) = nullptr; + void *m_onNewSubscriberArgs = nullptr; + + static void jumppadPublisherReader(void *callee, + const ReaderCacheChange &cacheChange); + static void jumppadSubscriptionReader(void *callee, + const ReaderCacheChange &cacheChange); + + static void jumppadTakeProxyOfDisposedReader(const Reader *reader, + const WriterProxy &proxy, + void *arg); + static void jumppadTakeProxyOfDisposedWriter(const Writer *writer, + const ReaderProxy &proxy, + void *arg); + + void handlePublisherReaderMessage(const ReaderCacheChange &change); + void handleSubscriptionReaderMessage(const ReaderCacheChange &change); + + template bool deleteEndpoint(A *endpoint, Writer *sedp_endpoint); + + template + bool announceEndpointDeletion(A *local_endpoint, Writer *sedp_endpoint); + + template + bool disposeEndpointInSEDPHistory(A *local_endpoint, Writer *sedp_writer); +}; +} // namespace rtps + +#endif // RTPS_SEDPAGENT_H diff --git a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h new file mode 100644 index 000000000..b14ca6f22 --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h @@ -0,0 +1,87 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_SPDP_H +#define RTPS_SPDP_H + +#include "lwip/sys.h" +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/discovery/BuiltInEndpoints.h" +#include "rtps/discovery/ParticipantProxyData.h" +#include "rtps/utils/Log.h" +#include "ucdr/microcdr.h" + +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SPDP_LOG(...) \ + if (true) { \ + printf("[SPDP] "); \ + printf(__VA_ARGS__); \ + printf("\r\n"); \ + } +#else +#define SPDP_LOG(...) // +#endif + +namespace rtps { +class Participant; +class Writer; +class Reader; +class ReaderCacheChange; + +class SPDPAgent { +public: + void init(Participant &participant, BuiltInEndpoints &endpoints); + void start(); + void stop(); + SemaphoreHandle_t m_mutex; + +private: + Participant *mp_participant = nullptr; + BuiltInEndpoints m_buildInEndpoints; + bool m_running = false; + std::array m_outputBuffer{}; // TODO check required size + std::array m_inputBuffer{}; + ParticipantProxyData m_proxyDataBuffer{}; + ucdrBuffer m_microbuffer{}; + uint8_t m_cycleHB = 0; + + bool initialized = false; + static void receiveCallback(void *callee, + const ReaderCacheChange &cacheChange); + void handleSPDPPackage(const ReaderCacheChange &cacheChange); + void configureEndianessAndOptions(ucdrBuffer &buffer); + void processProxyData(); + bool addProxiesForBuiltInEndpoints(); + + void addInlineQos(); + void addParticipantParameters(); + void endCurrentList(); + + static void runBroadcast(void *args); +}; +} // namespace rtps + +#endif // RTPS_SPDP_H diff --git a/components/rtps_embedded/include/rtps/discovery/TopicData.h b/components/rtps_embedded/include/rtps/discovery/TopicData.h new file mode 100644 index 000000000..24c6a2c1b --- /dev/null +++ b/components/rtps_embedded/include/rtps/discovery/TopicData.h @@ -0,0 +1,109 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DISCOVEREDWRITERDATA_H +#define RTPS_DISCOVEREDWRITERDATA_H + +#define SUPPRESS_UNICAST 0 + +#include "rtps/config.h" +#include "rtps/utils/hash.h" +#include "ucdr/microcdr.h" +#include +#include + +namespace rtps { + +struct BuiltInTopicKey { + std::array value; +}; + +struct TopicData { + Guid_t endpointGuid; + char typeName[Config::MAX_TYPENAME_LENGTH]; + char topicName[Config::MAX_TOPICNAME_LENGTH]; + ReliabilityKind_t reliabilityKind; + DurabilityKind_t durabilityKind; + FullLengthLocator unicastLocator; + FullLengthLocator multicastLocator; + + uint8_t statusInfo; + bool statusInfoValid; + // Use Case: Remotes communicates id of deleted endpoint through key_hash + // parameter + EntityId_t entityIdFromKeyHash; + bool entityIdFromKeyHashValid; + + TopicData() + : endpointGuid(GUID_UNKNOWN), typeName{'\0'}, topicName{'\0'}, + reliabilityKind(ReliabilityKind_t::BEST_EFFORT), + durabilityKind(DurabilityKind_t::VOLATILE) { + rtps::FullLengthLocator someLocator = + rtps::FullLengthLocator::createUDPv4Locator( + 192, 168, 0, 42, rtps::getUserUnicastPort(0)); + unicastLocator = someLocator; + multicastLocator = FullLengthLocator(); + }; + + TopicData(Guid_t guid, ReliabilityKind_t reliability, FullLengthLocator loc) + : endpointGuid(guid), typeName{'\0'}, topicName{'\0'}, + reliabilityKind(reliability), + durabilityKind(DurabilityKind_t::VOLATILE), unicastLocator(loc) {} + + + bool matchesTopicOf(const TopicData &other); + + bool readFromUcdrBuffer(ucdrBuffer &buffer); + bool serializeIntoUcdrBuffer(ucdrBuffer &buffer) const; + + bool isDisposedFlagSet() const; + bool isUnregisteredFlagSet() const; +}; + +struct TopicDataCompressed { + Guid_t endpointGuid; + std::size_t topicHash; + std::size_t typeHash; + bool is_reliable; + LocatorIPv4 unicastLocator; + LocatorIPv4 multicastLocator; + + TopicDataCompressed() = default; + TopicDataCompressed(const TopicData &topic_data) { + endpointGuid = topic_data.endpointGuid; + topicHash = + hashCharArray(topic_data.topicName, Config::MAX_TOPICNAME_LENGTH); + typeHash = hashCharArray(topic_data.typeName, Config::MAX_TYPENAME_LENGTH); + is_reliable = (topic_data.reliabilityKind == ReliabilityKind_t::RELIABLE) + ? true + : false; + unicastLocator = topic_data.unicastLocator; + multicastLocator = topic_data.multicastLocator; + } + + bool matchesTopicOf(const TopicData &topic_data) const; +}; +} // namespace rtps + +#endif // RTPS_DISCOVEREDWRITERDATA_H diff --git a/components/rtps_embedded/include/rtps/entities/Domain.h b/components/rtps_embedded/include/rtps/entities/Domain.h new file mode 100644 index 000000000..bdc981db3 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Domain.h @@ -0,0 +1,98 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DOMAIN_H +#define RTPS_DOMAIN_H + +#include "rtps/ThreadPool.h" +#include "rtps/config.h" +#include "rtps/entities/Participant.h" +#include "rtps/entities/StatefulReader.h" +#include "rtps/entities/StatefulWriter.h" +#include "rtps/entities/StatelessReader.h" +#include "rtps/entities/StatelessWriter.h" +#include "rtps/storages/PBufWrapper.h" +#include + +namespace rtps { +class Domain { +public: + Domain(); + ~Domain(); + + bool completeInit(); + void stop(); + + Participant *createParticipant(); + Writer *createWriter(Participant &part, const char *topicName, + const char *typeName, bool reliable, + bool enforceUnicast = false); + Reader *createReader(Participant &part, const char *topicName, + const char *typeName, bool reliable, + ip4_addr_t mcastaddress = {0}); + + Writer *writerExists(Participant &part, const char *topicName, + const char *typeName, bool reliable); + Reader *readerExists(Participant &part, const char *topicName, + const char *typeName, bool reliable); + + bool deleteWriter(Participant &part, Writer *writer); + bool deleteReader(Participant &part, Reader *reader); + + void printInfo(); + +private: + friend class SizeInspector; + ThreadPool m_threadPool; + UdpDriver m_transport; + std::array m_participants; + const uint8_t PARTICIPANT_START_ID = 0; + ParticipantId_t m_nextParticipantId = PARTICIPANT_START_ID; + + std::array m_statelessWriters; + std::array m_statelessReaders; + std::array m_statefulReaders; + std::array m_statefulWriters; + template B *getNextUnusedEndpoint(A &a) { + for (unsigned int i = 0; i < a.size(); i++) { + if (!a[i].isInitialized()) { + return &(a[i]); + } + } + return nullptr; + } + + bool m_initComplete = false; + SemaphoreHandle_t m_mutex; + + void receiveCallback(const PacketInfo &packet); + GuidPrefix_t generateGuidPrefix(ParticipantId_t id) const; + void createBuiltinWritersAndReaders(Participant &part); + void registerPort(const Participant &part); + void registerMulticastPort(FullLengthLocator mcastLocator); + static void receiveJumppad(void *callee, const PacketInfo &packet); +}; +} // namespace rtps + +#endif // RTPS_DOMAIN_H diff --git a/components/rtps_embedded/include/rtps/entities/Participant.h b/components/rtps_embedded/include/rtps/entities/Participant.h new file mode 100644 index 000000000..c9ca72b92 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Participant.h @@ -0,0 +1,127 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_PARTICIPANT_H +#define RTPS_PARTICIPANT_H + +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/discovery/SEDPAgent.h" +#include "rtps/discovery/SPDPAgent.h" +#include "rtps/messages/MessageReceiver.h" + +namespace rtps { + +class Writer; +class Reader; + +class Participant { +public: + GuidPrefix_t m_guidPrefix; + ParticipantId_t m_participantId; + + Participant(); + explicit Participant(const GuidPrefix_t &guidPrefix, + ParticipantId_t participantId); + + // Not allowed because the message receiver contains a pointer to the + // participant + Participant(const Participant &) = delete; + Participant(Participant &&) = delete; + Participant &operator=(const Participant &) = delete; + Participant &operator=(Participant &&) = delete; + + ~Participant(); + bool isValid(); + + void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId); + + std::array getNextUserEntityKey(); + + // Actually the only two function that should be used by the user + bool registerOnNewPublisherMatchedCallback(void (*callback)(void *arg), + void *args); + bool registerOnNewSubscriberMatchedCallback(void (*callback)(void *arg), + void *args); + + //! Not-thread-safe function to add a writer + Writer *addWriter(Writer *writer); + bool isWritersFull(); + bool deleteWriter(Writer *writer); + + //! Not-thread-safe function to add a reader + Reader *addReader(Reader *reader); + bool isReadersFull(); + bool deleteReader(Reader *reader); + + //! (Probably) Thread safe if writers cannot be removed + Writer *getWriter(EntityId_t id); + Writer *getMatchingWriter(const TopicData &topicData); + Writer *getMatchingWriter(const TopicDataCompressed &topicData); + + //! (Probably) Thread safe if readers cannot be removed + Reader *getReader(EntityId_t id); + Reader *getReaderByWriterId(const Guid_t &guid); + Reader *getMatchingReader(const TopicData &topicData); + Reader *getMatchingReader(const TopicDataCompressed &topicData); + + bool addNewRemoteParticipant(const ParticipantProxyData &remotePart); + bool removeRemoteParticipant(const GuidPrefix_t &prefix); + void removeAllProxiesOfParticipant(const GuidPrefix_t &prefix); + void removeProxyFromAllEndpoints(const Guid_t &guid); + + const ParticipantProxyData *findRemoteParticipant(const GuidPrefix_t &prefix); + void refreshRemoteParticipantLiveliness(const GuidPrefix_t &prefix); + uint32_t getRemoteParticipantCount(); + MessageReceiver *getMessageReceiver(); + bool checkAndResetHeartbeats(); + + bool hasReaderWithMulticastLocator(ip4_addr_t address); + + void addBuiltInEndpoints(BuiltInEndpoints &endpoints); + void newMessage(const uint8_t *data, DataSize_t size); + + SPDPAgent &getSPDPAgent(); + void printInfo(); + +private: + friend class SizeInspector; + MessageReceiver m_receiver; + bool m_hasBuilInEndpoints = false; + std::array m_nextUserEntityId{{0, 0, 1}}; + std::array m_writers = { + nullptr}; + std::array m_readers = { + nullptr}; + + SemaphoreHandle_t m_mutex; + MemoryPool + m_remoteParticipants; + + SPDPAgent m_spdpAgent; + SEDPAgent m_sedpAgent; +}; +} // namespace rtps + +#endif // RTPS_PARTICIPANT_H diff --git a/components/rtps_embedded/include/rtps/entities/Reader.h b/components/rtps_embedded/include/rtps/entities/Reader.h new file mode 100644 index 000000000..f66ff037d --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Reader.h @@ -0,0 +1,151 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_READER_H +#define RTPS_READER_H + +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/discovery/TopicData.h" +#include "rtps/entities/WriterProxy.h" +#include "rtps/storages/MemoryPool.h" +#include "rtps/storages/PBufWrapper.h" +#if defined(ESP_PLATFORM) +#include "freertos/semphr.h" +#else +#include "semphr.h" +#endif +#include + +namespace rtps { + +struct SubmessageHeartbeat; +struct SubmessageGap; + +class ReaderCacheChange { +private: + const uint8_t *data; + +public: + const ChangeKind_t kind; + const DataSize_t size; + const Guid_t writerGuid; + const SequenceNumber_t sn; + + ReaderCacheChange(ChangeKind_t kind, Guid_t &writerGuid, SequenceNumber_t sn, + const uint8_t *data, DataSize_t size) + : data(data), kind(kind), size(size), writerGuid(writerGuid), sn(sn){}; + + ~ReaderCacheChange() = + default; // No need to free data. It's not owned by this object + // Not allowed because this class doesn't own the ptr and the user isn't + // allowed to use it outside the Scope of the callback + ReaderCacheChange(const ReaderCacheChange &other) = delete; + ReaderCacheChange(ReaderCacheChange &&other) = delete; + ReaderCacheChange &operator=(const ReaderCacheChange &other) = delete; + ReaderCacheChange &operator=(ReaderCacheChange &&other) = delete; + + bool copyInto(uint8_t *buffer, DataSize_t destSize) const { + if (destSize < size) { + return false; + } else { + memcpy(buffer, data, size); + return true; + } + } + + const uint8_t *getData() const { return data; } + + const DataSize_t getDataSize() const { return size; } +}; + +typedef void (*ddsReaderCallback_fp)(void *callee, + const ReaderCacheChange &cacheChange); + +class Reader { +public: + using callbackFunction_t = void (*)(void *, const ReaderCacheChange &); + using callbackIdentifier_t = uint32_t; + + TopicData m_attributes; + virtual void newChange(const ReaderCacheChange &cacheChange) = 0; + virtual callbackIdentifier_t registerCallback(callbackFunction_t cb, + void *arg); + virtual bool removeCallback(callbackIdentifier_t identifier); + uint8_t getNumCallbacks(); + + virtual bool onNewHeartbeat(const SubmessageHeartbeat &msg, + const GuidPrefix_t &remotePrefix) = 0; + virtual bool onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) = 0; + virtual bool addNewMatchedWriter(const WriterProxy &newProxy) = 0; + virtual bool removeProxy(const Guid_t &guid); + virtual void removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix); + bool isInitialized() { return m_is_initialized_; } + virtual void reset(); + bool isProxy(const Guid_t &guid); + WriterProxy *getProxy(Guid_t guid); + uint32_t getProxiesCount(); + + void setSEDPSequenceNumber(const SequenceNumber_t &sn); + const SequenceNumber_t &getSEDPSequenceNumber(); + + using dumpProxyCallback = void (*)(const Reader *reader, const WriterProxy &, + void *arg); + + int dumpAllProxies(dumpProxyCallback target, void *arg); + + virtual bool sendPreemptiveAckNack(const WriterProxy &writer); + +protected: + void executeCallbacks(const ReaderCacheChange &cacheChange); + bool initMutex(); + + SequenceNumber_t m_sedp_sequence_number; + + bool m_is_initialized_ = false; + Reader(); + virtual ~Reader() = default; + MemoryPool m_proxies; + + callbackIdentifier_t m_callback_identifier = 1; + + uint8_t m_callback_count = 0; + using callbackElement_t = struct { + callbackFunction_t function; + void *arg; + callbackIdentifier_t identifier; + }; + + std::array m_callbacks; + + // Guards manipulation of the proxies array + SemaphoreHandle_t m_proxies_mutex = nullptr; + + // Guards manipulation of callback array + SemaphoreHandle_t m_callback_mutex = nullptr; +}; +} // namespace rtps + +#endif // RTPS_READER_H diff --git a/components/rtps_embedded/include/rtps/entities/ReaderProxy.h b/components/rtps_embedded/include/rtps/entities/ReaderProxy.h new file mode 100644 index 000000000..fb6860836 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/ReaderProxy.h @@ -0,0 +1,58 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_READERPROXY_H +#define RTPS_READERPROXY_H + +#include "rtps/common/types.h" +#include "rtps/discovery/ParticipantProxyData.h" + +namespace rtps { +struct ReaderProxy { + Guid_t remoteReaderGuid; + Count_t ackNackCount = {0}; + LocatorIPv4 remoteLocator; + bool is_reliable = false; + LocatorIPv4 remoteMulticastLocator; + bool useMulticast = false; + bool suppressUnicast = false; + bool unknown_eid = false; + bool finalFlag = false; + SequenceNumber_t lastAckNackSequenceNumber = {0, 1}; + + ReaderProxy() + : remoteReaderGuid({GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}), + ackNackCount{0}, remoteLocator(LocatorIPv4()), finalFlag(false){}; + ReaderProxy(const Guid_t &guid, const LocatorIPv4 &loc, bool reliable) + : remoteReaderGuid(guid), ackNackCount{0}, remoteLocator(loc), + is_reliable(reliable), finalFlag(false){}; + ReaderProxy(const Guid_t &guid, const LocatorIPv4 &loc, + const LocatorIPv4 &mcastloc, bool reliable) + : remoteReaderGuid(guid), ackNackCount{0}, remoteLocator(loc), + is_reliable(reliable), remoteMulticastLocator(mcastloc), finalFlag(false){}; +}; + +} // namespace rtps + +#endif // RTPS_READERPROXY_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.h b/components/rtps_embedded/include/rtps/entities/StatefulReader.h new file mode 100644 index 000000000..55497f943 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.h @@ -0,0 +1,62 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATEFULREADER_H +#define RTPS_STATEFULREADER_H + +#include "lwip/sys.h" +#include "rtps/communication/UdpDriver.h" +#include "rtps/config.h" +#include "rtps/entities/Reader.h" +#include "rtps/entities/WriterProxy.h" +#include "rtps/storages/MemoryPool.h" + +namespace rtps { +struct SubmessageHeartbeat; + +template class StatefulReaderT final : public Reader { +public: + ~StatefulReaderT() override; + bool init(const TopicData &attributes, NetworkDriver &driver); + void newChange(const ReaderCacheChange &cacheChange) override; + bool addNewMatchedWriter(const WriterProxy &newProxy) override; + bool onNewHeartbeat(const SubmessageHeartbeat &msg, + const GuidPrefix_t &remotePrefix) override; + bool onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) override; + + bool sendPreemptiveAckNack(const WriterProxy &writer) override; + +private: + Ip4Port_t m_srcPort; // TODO intended for reuse but buffer not used as such + NetworkDriver *m_transport; +}; + +using StatefulReader = StatefulReaderT; + +} // namespace rtps + +#include "StatefulReader.tpp" + +#endif // RTPS_STATEFULREADER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp new file mode 100644 index 000000000..117157f28 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -0,0 +1,283 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "lwip/sys.h" +#include "lwip/tcpip.h" +#include "rtps/entities/StatefulReader.h" +#include "rtps/messages/MessageFactory.h" +#include "rtps/utils/Diagnostics.h" +#include "rtps/utils/Lock.h" +#include "rtps/utils/Log.h" + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SFR_LOG(...) \ + if (true) { \ + printf("[StatefulReader %s] ", &m_attributes.topicName[0]); \ + printf(__VA_ARGS__); \ + printf("\r\n"); \ + } +#else +#define SFR_LOG(...) // +#endif + +using rtps::StatefulReaderT; + +template +StatefulReaderT::~StatefulReaderT() {} + +template +bool StatefulReaderT::init(const TopicData &attributes, + NetworkDriver &driver) { + if (!initMutex()) { + return false; + } + + m_proxies.clear(); + m_attributes = attributes; + m_transport = &driver; + m_srcPort = attributes.unicastLocator.port; + m_is_initialized_ = true; + return true; +} + +template +void StatefulReaderT::newChange( + const ReaderCacheChange &cacheChange) { + if (m_callback_count == 0 || !m_is_initialized_) { + return; + } + Lock lock{m_proxies_mutex}; + for (auto &proxy : m_proxies) { + if (proxy.remoteWriterGuid == cacheChange.writerGuid) { + if (proxy.expectedSN == cacheChange.sn) { + SFR_LOG("Delivering SN %u.%u | ! GUID %u %u %u %u \r\n", + (int)cacheChange.sn.high, (int)cacheChange.sn.low, + cacheChange.writerGuid.prefix.id[0], + cacheChange.writerGuid.prefix.id[1], + cacheChange.writerGuid.prefix.id[2], + cacheChange.writerGuid.prefix.id[3]); + executeCallbacks(cacheChange); + ++proxy.expectedSN; + SFR_LOG("Done processing SN %u.%u\r\n", (int)cacheChange.sn.high, + (int)cacheChange.sn.low); + return; + } else { + Diagnostics::StatefulReader::sfr_unexpected_sn++; + SFR_LOG( + "Unexpected SN %u.%u != %u.%u, dropping! GUID %u %u %u %u | \r\n", + (int)proxy.expectedSN.high, (int)proxy.expectedSN.low, + (int)cacheChange.sn.high, (int)cacheChange.sn.low, + cacheChange.writerGuid.prefix.id[0], + cacheChange.writerGuid.prefix.id[1], + cacheChange.writerGuid.prefix.id[2], + cacheChange.writerGuid.prefix.id[3]); + } + } + } +} + +template +bool StatefulReaderT::addNewMatchedWriter( + const WriterProxy &newProxy) { +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("New writer added with id: "); + printGuid(newProxy.remoteWriterGuid); + SFR_LOG("\n"); +#endif + return m_proxies.add(newProxy); +} + +template +bool StatefulReaderT::onNewGapMessage( + const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) { + Lock lock{m_proxies_mutex}; + if (!m_is_initialized_) { + return false; + } + SFR_LOG("Processing gap message %u %u", msg.gapStart, msg.gapList.base); + + Guid_t writerProxyGuid; + writerProxyGuid.prefix = remotePrefix; + writerProxyGuid.entityId = msg.writerId; + WriterProxy *writer = getProxy(writerProxyGuid); + + if (writer == nullptr) { + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("Ignore GAP. Couldn't find a matching " + "writer with id:"); + printEntityId(msg.writerId); + SFR_LOG("\n"); +#endif + return false; + } + + // Case 1: We are still waiting for messages before gapStart + if (writer->expectedSN < msg.gapStart) { + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = writer->remoteLocator.getIp4Address(); + info.destPort = writer->remoteLocator.port; + rtps::MessageFactory::addHeader(info.buffer, + m_attributes.endpointGuid.prefix); + SequenceNumber_t last_valid = msg.gapStart; + --last_valid; + auto missing_sns = writer->getMissing(writer->expectedSN, last_valid); + rtps::MessageFactory::addAckNack(info.buffer, msg.writerId, msg.readerId, + missing_sns, writer->getNextAckNackCount(), + false); + m_transport->sendPacket(info); + return true; + } + + // Case 2: We are expecting a message between [gapStart; gapList.base -1] + // Advance expectedSN beyond gapList.base + if (writer->expectedSN < msg.gapList.base) { + auto before = writer->expectedSN; + writer->expectedSN = msg.gapList.base; + + // writer->expectedSN++; + + // Advance expectedSN to first unset bit + for (uint32_t bit = 0; bit < SNS_MAX_NUM_BITS; + writer->expectedSN++, bit++) { + if (!msg.gapList.isSet(bit)) { + break; + } + } + + return true; + + }else{ + + // Case 3: We are expecting a sequence number beyond gap list base, + // check if we need to update expectedSN + auto i = msg.gapList.base; + for(uint32_t bit = 0; bit < SNS_MAX_NUM_BITS; i++, bit++){ + if(i < writer->expectedSN){ + continue; + } + + if(msg.gapList.isSet(bit)){ + writer->expectedSN++; + }else{ + PacketInfo info; + info.srcPort = m_srcPort; + info.destAddr = writer->remoteLocator.getIp4Address(); + info.destPort = writer->remoteLocator.port; + rtps::MessageFactory::addHeader(info.buffer, + m_attributes.endpointGuid.prefix); + SequenceNumberSet set; + set.base = writer->expectedSN; + set.numBits = 1; + set.bitMap[0] = set.bitMap[0] |= uint32_t{1} << 31; + rtps::MessageFactory::addAckNack(info.buffer, msg.writerId, msg.readerId, + set, writer->getNextAckNackCount(), + false); + m_transport->sendPacket(info); + + return true; + } + } + + + return false; + } +} + +template +bool StatefulReaderT::onNewHeartbeat( + const SubmessageHeartbeat &msg, const GuidPrefix_t &sourceGuidPrefix) { + Lock lock{m_proxies_mutex}; + if (!m_is_initialized_) { + return false; + } + PacketInfo info; + info.srcPort = m_srcPort; + + Guid_t writerProxyGuid; + writerProxyGuid.prefix = sourceGuidPrefix; + writerProxyGuid.entityId = msg.writerId; + WriterProxy *writer = getProxy(writerProxyGuid); + + if (writer == nullptr) { + +#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE + SFR_LOG("Ignore heartbeat. Couldn't find a matching " + "writer with id:"); + printEntityId(msg.writerId); + SFR_LOG("\n"); +#endif + return false; + } + + if (writer->expectedSN < msg.firstSN) { + SFR_LOG("expectedSN < firstSN, advancing expectedSN"); + writer->expectedSN = msg.firstSN; + } + + writer->hbCount.value = msg.count.value; + info.destAddr = writer->remoteLocator.getIp4Address(); + info.destPort = writer->remoteLocator.port; + rtps::MessageFactory::addHeader(info.buffer, + m_attributes.endpointGuid.prefix); + auto missing_sns = writer->getMissing(msg.firstSN, msg.lastSN); + bool final_flag = (missing_sns.numBits == 0); + rtps::MessageFactory::addAckNack(info.buffer, msg.writerId, msg.readerId, + missing_sns, writer->getNextAckNackCount(), + final_flag); + + SFR_LOG("Sending acknack base %u bits %u .\n", (int)missing_sns.base.low, + (int)missing_sns.numBits); + m_transport->sendPacket(info); + return true; +} + +template +bool StatefulReaderT::sendPreemptiveAckNack( + const WriterProxy &writer) { + Lock lock{m_proxies_mutex}; + if (!m_is_initialized_) { + return false; + } + + PacketInfo info; + info.srcPort = m_attributes.unicastLocator.port; + info.destAddr = writer.remoteLocator.getIp4Address(); + info.destPort = writer.remoteLocator.port; + rtps::MessageFactory::addHeader(info.buffer, + m_attributes.endpointGuid.prefix); + SequenceNumberSet number_set; + number_set.base.high = 0; + number_set.base.low = 0; + number_set.numBits = 0; + rtps::MessageFactory::addAckNack( + info.buffer, writer.remoteWriterGuid.entityId, + m_attributes.endpointGuid.entityId, number_set, Count_t{1}, false); + + SFR_LOG("Sending preemptive acknack.\n"); + m_transport->sendPacket(info); + return true; +} diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h new file mode 100644 index 000000000..60d71489c --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h @@ -0,0 +1,91 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATEFULWRITER_H +#define RTPS_STATEFULWRITER_H + +#include "rtps/entities/ReaderProxy.h" +#include "rtps/entities/Writer.h" +#include "rtps/storages/HistoryCacheWithDeletion.h" +#include "rtps/storages/MemoryPool.h" + +namespace rtps { + +template class StatefulWriterT final : public Writer { +public: + ~StatefulWriterT() override; + bool init(TopicData attributes, TopicKind_t topicKind, ThreadPool *threadPool, + NetworkDriver &driver, bool enfUnicast = false); + + //! Executes required steps like sending packets. Intended to be called by + //! worker threads + void progress() override; + const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size, bool inLineQoS = false, + bool markDisposedAfterWrite = false) override; + + bool removeFromHistory(const SequenceNumber_t &s); + void setAllChangesToUnsent() override; + void onNewAckNack(const SubmessageAckNack &msg, + const GuidPrefix_t &sourceGuidPrefix) override; + void reset() override; + void updateChangeKind(SequenceNumber_t &sequence_number); + +private: + NetworkDriver *m_transport; + + HistoryCacheWithDeletion m_history; + + /* + * Cache changes marked as disposeAfterWrite are retained for a short amount + * in case of retransmission The whole 'disposeAfterWrite' mechanisms only + * exists to allow for repeated creation and deletion of endpoints during + * operation. Otherwise the history will quickly reach its limits. Will be + * replaced with something more elegant in the future. + */ + ThreadSafeCircularBuffer m_disposeWithDelay; + void dropDisposeAfterWriteChanges(); + + sys_thread_t m_heartbeatThread; + + Count_t m_hbCount{1}; + + bool m_running = true; + bool m_thread_running = false; + + bool sendData(const ReaderProxy &reader, const CacheChange *next); + bool sendDataWRMulticast(const ReaderProxy &reader, const CacheChange *next); + static void hbFunctionJumppad(void *thisPointer); + void sendHeartBeatLoop(); + void sendHeartBeat(); + void sendGap(const ReaderProxy &reader, const SequenceNumber_t &firstMissing, + const SequenceNumber_t &nextValid); +}; + +using StatefulWriter = StatefulWriterT; +} // namespace rtps + +#include "StatefulWriter.tpp" + +#endif // RTPS_STATEFULWRITER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp new file mode 100644 index 000000000..33bb4d7df --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -0,0 +1,559 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "lwip/sys.h" +#include "rtps/entities/StatefulWriter.h" +#include "rtps/messages/MessageFactory.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/utils/Log.h" +#include +#include + +using rtps::StatefulWriterT; + +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SFW_LOG(...) \ + if (true) { \ + printf("[Stateful Writer %s] ", this->m_attributes.topicName); \ + printf(__VA_ARGS__); \ + printf("\r\n"); \ + } +#else +#define SFW_LOG(...) // +#endif + +template +StatefulWriterT::~StatefulWriterT() { + m_running = false; + while (m_thread_running) { + sys_msleep(500); // Required for tests/ Join currently not available / + // increased because Segfault in Tests if(sys_mutex_valid(&m_mutex)){ + // sys_mutex_free(&m_mutex); + // } + } +} + +template +bool StatefulWriterT::init(TopicData attributes, + TopicKind_t topicKind, + ThreadPool *threadPool, + NetworkDriver &driver, + bool enfUnicast) { + + if (m_mutex == nullptr) { + if (!createMutex(&m_mutex)) { + + SFW_LOG("Failed to create mutex.\n"); + + return false; + } + } + + m_attributes = attributes; + + mp_threadPool = threadPool; + m_srcPort = attributes.unicastLocator.port; + m_enforceUnicast = enfUnicast; + m_topicKind = topicKind; + + m_nextSequenceNumberToSend = {0, 1}; + m_proxies.clear(); + + m_transport = &driver; + m_history.clear(); + m_hbCount = {1}; + + if (!m_disposeWithDelay.init()) { + SFW_LOG("Failed to initialize delayed dispose buffer mutex.\n"); + return false; + } + + // Thread already exists, do not create new one (reusing slot case) + m_is_initialized_ = true; + + if (!m_thread_running) { + + m_running = true; + m_thread_running = false; + + if (m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) { + m_heartbeatThread = sys_thread_new("HBThreadPub", hbFunctionJumppad, this, + Config::HEARTBEAT_STACKSIZE, + Config::THREAD_POOL_WRITER_PRIO); + } else if (m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) { + m_heartbeatThread = sys_thread_new("HBThreadSub", hbFunctionJumppad, this, + Config::HEARTBEAT_STACKSIZE, + Config::THREAD_POOL_WRITER_PRIO); + } else { + m_heartbeatThread = sys_thread_new("HBThread", hbFunctionJumppad, this, + Config::HEARTBEAT_STACKSIZE, + Config::THREAD_POOL_WRITER_PRIO); + } + } + + return true; +} + +template void StatefulWriterT::reset() { + m_is_initialized_ = false; + // TODO +} + +template +const rtps::CacheChange *StatefulWriterT::newChange( + ChangeKind_t kind, const uint8_t *data, DataSize_t size, bool inLineQoS, + bool markDisposedAfterWrite) { + INIT_GUARD() + if (isIrrelevant(kind)) { + return nullptr; + } + + Lock lock{m_mutex}; + if (!m_is_initialized_) { + return nullptr; + } + + if (m_history.isFull()) { + // Right now we drop elements anyway because we cannot detect non-responding + // readers yet. return nullptr; + SequenceNumber_t newMin = + ++SequenceNumber_t(m_history.getCurrentSeqNumMin()); + if (m_nextSequenceNumberToSend < newMin) { + m_nextSequenceNumberToSend = + newMin; // Make sure we have the correct sn to send + } + SFW_LOG("History full! Dropping changes %s.\r\n", this->m_attributes.topicName); + } + + auto *result = + m_history.addChange(data, size, inLineQoS, markDisposedAfterWrite); + if (mp_threadPool != nullptr) { + mp_threadPool->addWorkload(this); + } + + SFW_LOG("Adding new data.\n"); + + return result; +} + +template void StatefulWriterT::progress() { + INIT_GUARD() + Lock lock{m_mutex}; + CacheChange *next = m_history.getChangeBySN(m_nextSequenceNumberToSend); + if (next != nullptr) { + uint32_t i = 0; + for (const auto &proxy : m_proxies) { + if (!m_enforceUnicast) { + sendDataWRMulticast(proxy, next); + } else { + i++; + sendData(proxy, next); + } + } + + SFW_LOG("Sending data with SN %u.%u", (int)m_nextSequenceNumberToSend.low, + (int)m_nextSequenceNumberToSend.high); + + if (next->disposeAfterWrite) { + SFW_LOG("Dispose after write msg sent to %u proxies\r\n", (int)i); + } + + /* + * Use case: deletion of local endpoints + * -> send Data Message with Disposed Flag set + * -> Set respective SEDP CacheChange as NOT_ALIVE_DISPOSED after + * transmission to proxies + * -> onAckNack will send Gap Messages to skip deleted local endpoints + * during SEDP + */ + if (next->disposeAfterWrite) { + next->sentTickCount = xTaskGetTickCount(); + if (!m_disposeWithDelay.copyElementIntoBuffer(next->sequenceNumber)) { + SFW_LOG("Failed to enqueue dispose after write!"); + m_history.dropChange(next->sequenceNumber); + } else { + SFW_LOG("Delayed dispose scheduled for sn %u %u\r\n", + (int)next->sequenceNumber.high, (int)next->sequenceNumber.low); + } + } + + ++m_nextSequenceNumberToSend; + sendHeartBeat(); + + } else { + SFW_LOG("Couldn't get a CacheChange with SN (%li,%lu)\n", + m_nextSequenceNumberToSend.high, m_nextSequenceNumberToSend.low); + } +} + +template +void StatefulWriterT::setAllChangesToUnsent() { + INIT_GUARD() + Lock lock{m_mutex}; + + m_nextSequenceNumberToSend = m_history.getCurrentSeqNumMin(); + + if (mp_threadPool != nullptr) { + mp_threadPool->addWorkload(this); + } +} + +template +void StatefulWriterT::onNewAckNack( + const SubmessageAckNack &msg, const GuidPrefix_t &sourceGuidPrefix) { + INIT_GUARD() + Lock lock{m_mutex}; + if (!m_is_initialized_) { + return; + } + + ReaderProxy *reader = nullptr; + for (auto &proxy : m_proxies) { + if (proxy.remoteReaderGuid.prefix == sourceGuidPrefix && + proxy.remoteReaderGuid.entityId == msg.readerId) { + reader = &proxy; + break; + } + } + + if (reader == nullptr) { +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE + SFW_LOG("No proxy found with id: "); + printEntityId(msg.readerId); + SFW_LOG(" Dropping acknack.\n"); +#endif + return; + } + + reader->ackNackCount = msg.count; + reader->finalFlag = msg.header.finalFlag(); + reader->lastAckNackSequenceNumber = msg.readerSNState.base; + + rtps::SequenceNumber_t nextSN = msg.readerSNState.base; + + // Preemptive ack nack + if (nextSN.low == 0 && nextSN.high == 0) { + sendHeartBeat(); + return; + } + + if (m_history.isEmpty()) { + // We have never sent anything -> heartbeat + if (m_history.getLastUsedSequenceNumber() == rtps::SequenceNumber_t{0, 0}) { + sendHeartBeat(); + } else { + // No data but we have sent something in the past -> GapStart = + // readerSNState.base, NextValid = lastUsedSequenceNumber+1 + rtps::SequenceNumber_t nextValid = m_history.getLastUsedSequenceNumber(); + ++nextValid; + sendGap(*reader, msg.readerSNState.base, nextValid); + } + + return; + } + + // Requesting smaller SN than minimum sequence number -> sendGap + if (msg.readerSNState.base < m_history.getCurrentSeqNumMin()) { + sendGap(*reader, msg.readerSNState.base, m_history.getCurrentSeqNumMin()); + return; + } + + SFW_LOG("Received non-preemptive acknack with %lu bits set.\r\n", + msg.readerSNState.numBits); + for (uint32_t i = 0; i < msg.readerSNState.numBits && + nextSN <= m_history.getLastUsedSequenceNumber(); + ++i, ++nextSN) { + + if (msg.readerSNState.isSet(i)) { + + SFW_LOG("Looking for change %lu | Bit %lu", nextSN.low, i); + const rtps::CacheChange *cache = m_history.getChangeBySN(nextSN); + + // We still have the cache, send DATA + if (cache != nullptr) { + if (cache->disposeAfterWrite) { + SFW_LOG("SERVING FROM DISPOSE AFTER WRITE CACHE\r\n"); + } + sendData(*reader, cache); + } else { + SFW_LOG("> Change not found, search for next valid SN %lu \r\n", + nextSN.low); + // Cache not found, look for next valid SN + rtps::SequenceNumber_t gapBegin = nextSN; + rtps::CacheChange *nextValidChange = nullptr; + uint32_t j = i + 1; + for (++nextSN; nextSN <= m_history.getLastUsedSequenceNumber(); + ++nextSN, ++j) { + nextValidChange = m_history.getChangeBySN(nextSN); + if (nextValidChange != nullptr) { + break; + } + } + if (nextValidChange == nullptr) { + sendGap(*reader, gapBegin, nextSN); + return; + } else { + sendGap(*reader, gapBegin, nextValidChange->sequenceNumber); + } + // sendData(nullptr, nextValidChange); + nextSN = nextValidChange->sequenceNumber; + --nextSN; + i = --j; + } + } + } +} + +template +bool rtps::StatefulWriterT::removeFromHistory( + const SequenceNumber_t &s) { + Lock lock{m_mutex}; + return m_history.dropChange(s); +} + +template +bool StatefulWriterT::sendData(const ReaderProxy &reader, + const CacheChange *next) { + INIT_GUARD() + // TODO smarter packaging e.g. by creating MessageStruct and serialize after + // adjusting values Reusing the pbuf is not possible. See + // https://www.nongnu.org/lwip/2_0_x/raw_api.html (Zero-Copy MACs) + + PacketInfo info; + info.srcPort = m_srcPort; + + MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(info.buffer); + + // Just usable for IPv4 + const LocatorIPv4 &locator = reader.remoteLocator; + + info.destAddr = locator.getIp4Address(); + info.destPort = (Ip4Port_t)locator.port; + + MessageFactory::addSubMessageData( + info.buffer, next->data, next->inLineQoS, next->sequenceNumber, + m_attributes.endpointGuid.entityId, reader.remoteReaderGuid.entityId); + m_transport->sendPacket(info); + + return true; +} + +template +void StatefulWriterT::sendGap( + const ReaderProxy &reader, const SequenceNumber_t &firstMissing, + const SequenceNumber_t &nextValid) { + INIT_GUARD() + // TODO smarter packaging e.g. by creating MessageStruct and serialize after + // adjusting values Reusing the pbuf is not possible. See + // https://www.nongnu.org/lwip/2_0_x/raw_api.html (Zero-Copy MACs) + + PacketInfo info; + info.srcPort = m_srcPort; + + MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(info.buffer); + + // Just usable for IPv4 + const LocatorIPv4 &locator = reader.remoteLocator; + + info.destAddr = locator.getIp4Address(); + info.destPort = (Ip4Port_t)locator.port; + + MessageFactory::addSubmessageGap( + info.buffer, m_attributes.endpointGuid.entityId, + reader.remoteReaderGuid.entityId, firstMissing, nextValid); + m_transport->sendPacket(info); +} + +template +bool StatefulWriterT::sendDataWRMulticast( + const ReaderProxy &reader, const CacheChange *next) { + INIT_GUARD() + + if (reader.useMulticast || reader.suppressUnicast == false) { + PacketInfo info; + info.srcPort = m_srcPort; + + MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(info.buffer); + + // Deceide whether multicast or not + if (reader.useMulticast) { + const LocatorIPv4 &locator = reader.remoteMulticastLocator; + info.destAddr = locator.getIp4Address(); + info.destPort = (Ip4Port_t)locator.port; + } else { + const LocatorIPv4 &locator = reader.remoteLocator; + info.destAddr = locator.getIp4Address(); + info.destPort = (Ip4Port_t)locator.port; + } + + EntityId_t reid; + if (reader.useMulticast) { + reid = ENTITYID_UNKNOWN; + } else { + reid = reader.remoteReaderGuid.entityId; + } + + MessageFactory::addSubMessageData(info.buffer, next->data, next->inLineQoS, + next->sequenceNumber, + m_attributes.endpointGuid.entityId, reid); + + m_transport->sendPacket(info); + } + return true; +} + +template +void StatefulWriterT::hbFunctionJumppad(void *thisPointer) { + auto *writer = static_cast *>(thisPointer); + writer->sendHeartBeatLoop(); +} + +template +void StatefulWriterT::sendHeartBeatLoop() { + m_thread_running = true; + while (m_running) { + sendHeartBeat(); + dropDisposeAfterWriteChanges(); + bool unconfirmed_changes = false; + for (auto it : m_proxies) { + if (it.lastAckNackSequenceNumber < m_nextSequenceNumberToSend) { + unconfirmed_changes = true; + break; + } + } + + // Temporarily increase HB frequency if there are unconfirmed remote changes + if (unconfirmed_changes) { + SFW_LOG("HB SPEEDUP!\r\n"); +#ifdef OS_IS_FREERTOS + vTaskDelay(pdMS_TO_TICKS(Config::SF_WRITER_HB_PERIOD_MS / 4)); + } else { + vTaskDelay(pdMS_TO_TICKS(Config::SF_WRITER_HB_PERIOD_MS)); + } +#else + sys_msleep(Config::SF_WRITER_HB_PERIOD_MS / 4); + } else { + sys_msleep(Config::SF_WRITER_HB_PERIOD_MS); + } +#endif + } + m_thread_running = false; +} + +template +void StatefulWriterT::dropDisposeAfterWriteChanges() { + SequenceNumber_t oldest_retained; + while (m_disposeWithDelay.peakFirst(oldest_retained)) { + + CacheChange *change = m_history.getChangeBySN(oldest_retained); + if (change == nullptr || !change->disposeAfterWrite) { + // Not in history anymore, drop + m_disposeWithDelay.moveFirstInto(oldest_retained); + return; + } + + auto age = (xTaskGetTickCount() - change->sentTickCount); + if (age > pdMS_TO_TICKS(4000)) { + m_history.dropChange(change->sequenceNumber); + SFW_LOG("Removing SN %u %u for good\r\n", + static_cast(oldest_retained.low), + static_cast(oldest_retained.high)); + SequenceNumber_t tmp; + m_disposeWithDelay.moveFirstInto(tmp); + + continue; + } else { + return; + } + } +} + +template +void StatefulWriterT::sendHeartBeat() { + INIT_GUARD() + if (m_proxies.isEmpty() || !m_is_initialized_) { + + SFW_LOG("Skipping heartbeat. No proxies.\n"); + return; + } + + for (auto &proxy : m_proxies) { + + PacketInfo info; + info.srcPort = m_srcPort; + + SequenceNumber_t firstSN; + SequenceNumber_t lastSN; + + MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); + + { + Lock lock{m_mutex}; + + if (!m_history.isEmpty()) { + firstSN = m_history.getCurrentSeqNumMin(); + lastSN = m_history.getCurrentSeqNumMax(); + + // Otherwise we may announce changes that have not been sent at least + // once! + if (lastSN > m_nextSequenceNumberToSend || + lastSN == m_nextSequenceNumberToSend) { + lastSN = m_nextSequenceNumberToSend; + --lastSN; + } + + // Proxy has confirmed all sequence numbers and set final flag + if ((proxy.lastAckNackSequenceNumber > lastSN) && proxy.finalFlag && + proxy.ackNackCount.value > 0) { + continue; + } + } else if (m_history.getLastUsedSequenceNumber() == + SequenceNumber_t{0, 0}) { + firstSN = SequenceNumber_t{0, 1}; + lastSN = SequenceNumber_t{0, 0}; + } else { + firstSN = SequenceNumber_t{0, 1}; + lastSN = m_history.getLastUsedSequenceNumber(); + } + } + + SFW_LOG("Sending HB with SN range [%lu.%lu;%lu.%lu]", firstSN.low, firstSN.high, + lastSN.low, lastSN.high); + + MessageFactory::addHeartbeat( + info.buffer, m_attributes.endpointGuid.entityId, + proxy.remoteReaderGuid.entityId, firstSN, lastSN, m_hbCount); + + info.destAddr = proxy.remoteLocator.getIp4Address(); + info.destPort = proxy.remoteLocator.port; + m_transport->sendPacket(info); + } + m_hbCount.value++; +} diff --git a/components/rtps_embedded/include/rtps/entities/StatelessReader.h b/components/rtps_embedded/include/rtps/entities/StatelessReader.h new file mode 100644 index 000000000..cb2ad7a40 --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessReader.h @@ -0,0 +1,44 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_STATELESSREADER_H +#define RTPS_STATELESSREADER_H + +#include "rtps/entities/Reader.h" + +namespace rtps { +class StatelessReader final : public Reader { +public: + bool init(const TopicData &attributes); + void newChange(const ReaderCacheChange &cacheChange) override; + bool onNewHeartbeat(const SubmessageHeartbeat &msg, + const GuidPrefix_t &remotePrefix) override; + bool addNewMatchedWriter(const WriterProxy &newProxy) override; + bool onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) override; +}; + +} // namespace rtps + +#endif // RTPS_STATELESSREADER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h new file mode 100644 index 000000000..e2a05083c --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h @@ -0,0 +1,68 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_RTPSWRITER_H +#define RTPS_RTPSWRITER_H + +#include "lwip/sys.h" +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/entities/Writer.h" +#include "rtps/storages/MemoryPool.h" +#include "rtps/storages/SimpleHistoryCache.h" + +namespace rtps { + +struct PBufWrapper; + +template class StatelessWriterT : public Writer { +public: + ~StatelessWriterT() override; + bool init(TopicData attributes, TopicKind_t topicKind, ThreadPool *threadPool, + NetworkDriver &driver, bool enfUnicast = false); + + void progress() override; + const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size, bool inLineQoS = false, + bool markDisposedAfterWrite = false) override; + bool removeFromHistory(const SequenceNumber_t &s); + + void setAllChangesToUnsent() override; + void onNewAckNack(const SubmessageAckNack &msg, + const GuidPrefix_t &sourceGuidPrefix) override; + void reset() override; + +private: + NetworkDriver *m_transport; + + SimpleHistoryCache m_history; +}; + +using StatelessWriter = StatelessWriterT; + +} // namespace rtps + +#include "StatelessWriter.tpp" + +#endif // RTPS_RTPSWRITER_H diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp new file mode 100644 index 000000000..5a1aa0aaf --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp @@ -0,0 +1,228 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include +#include + +#include "lwip/sys.h" +#include "lwip/tcpip.h" +#include "rtps/ThreadPool.h" +#include "rtps/communication/UdpDriver.h" +#include "rtps/messages/MessageFactory.h" +#include "rtps/storages/PBufWrapper.h" +#include "rtps/utils/Log.h" +#include "rtps/utils/udpUtils.h" + +using rtps::CacheChange; +using rtps::SequenceNumber_t; +using rtps::StatelessWriterT; + +#if SLW_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SLW_LOG(...) \ + if (true) { \ + printf("[StatelessWriter %s] ", &this->m_attributes.topicName[0]); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define SLW_LOG(...) // +#endif + +template +StatelessWriterT::~StatelessWriterT() { + // if(sys_mutex_valid(&m_mutex)){ + // sys_mutex_free(&m_mutex); + // } +} + +template +bool StatelessWriterT::init(TopicData attributes, + TopicKind_t topicKind, + ThreadPool *threadPool, + NetworkDriver &driver, + bool enfUnicast) { + + m_attributes = attributes; + + if (m_mutex == nullptr) { + if (!createMutex(&m_mutex)) { +#if SLW_VERBOSE + SLW_LOG("Failed to create mutex \n"); +#endif + return false; + } + } + + mp_threadPool = threadPool; + m_srcPort = attributes.unicastLocator.port; + m_enforceUnicast = enfUnicast; + + m_topicKind = topicKind; + m_nextSequenceNumberToSend = {0, 1}; + m_is_initialized_ = true; + + m_proxies.clear(); + m_history.clear(); + + m_transport = &driver; + + return true; +} + +template +void StatelessWriterT::reset() { + m_is_initialized_ = false; +} + +template +const CacheChange *StatelessWriterT::newChange( + rtps::ChangeKind_t kind, const uint8_t *data, DataSize_t size, + bool inLineQoS, bool markDisposedAfterWrite) { + INIT_GUARD(); + if (isIrrelevant(kind)) { + return nullptr; + } + Lock lock(m_mutex); + if (!m_is_initialized_) { + return nullptr; + } + + if (m_history.isFull()) { + SequenceNumber_t newMin = ++SequenceNumber_t(m_history.getSeqNumMin()); + if (m_nextSequenceNumberToSend < newMin) { + m_nextSequenceNumberToSend = + newMin; // Make sure we have the correct sn to send + } + SLW_LOG("History is full, dropping oldest %s\r\n", this->m_attributes.topicName); + } + + auto *result = m_history.addChange(data, size); + if (mp_threadPool != nullptr) { + mp_threadPool->addWorkload(this); + } + + SLW_LOG("Adding new data.\n"); + return result; +} + +template +bool StatelessWriterT::removeFromHistory( + const SequenceNumber_t &s) { + return false; // Stateless Writers currently do not support deletion from + // history +} + +template +void StatelessWriterT::setAllChangesToUnsent() { + INIT_GUARD(); + Lock lock(m_mutex); + + m_nextSequenceNumberToSend = m_history.getSeqNumMin(); + + if (mp_threadPool != nullptr) { + mp_threadPool->addWorkload(this); + } +} + +template +void StatelessWriterT::onNewAckNack( + const SubmessageAckNack & /*msg*/, const GuidPrefix_t &sourceGuidPrefix) { + INIT_GUARD(); + // Too lazy to respond +} + +template +void StatelessWriterT::progress() { + INIT_GUARD(); + // TODO smarter packaging e.g. by creating MessageStruct and serialize after + // adjusting values Reusing the pbuf is not possible. See + // https://www.nongnu.org/lwip/2_1_x/raw_api.html (Zero-Copy MACs) + + if (m_proxies.getNumElements() == 0) { + SLW_LOG("No Proxy!\n"); + } + + for (const auto &proxy : m_proxies) { + + SLW_LOG("Progess.\n"); + // Do nothing, if someone else sends for me... (Multicast) + if (proxy.useMulticast || !proxy.suppressUnicast || m_enforceUnicast) { + PacketInfo info; + info.srcPort = m_srcPort; + + MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(info.buffer); + + { + Lock lock(m_mutex); + const CacheChange *next = + m_history.getChangeBySN(m_nextSequenceNumberToSend); + if (next == nullptr) { + SLW_LOG("Couldn't get a new CacheChange with SN " + "(%li,%li)\n", + m_nextSequenceNumberToSend.high, + m_nextSequenceNumberToSend.low); + return; + } else { + SLW_LOG("Sending change with SN (%li,%li)\n", + m_nextSequenceNumberToSend.high, + m_nextSequenceNumberToSend.low); + } + + // Set EntityId to UNKNOWN if using multicast, because there might be + // different ones... + // TODO: mybe enhance by using UNKNOWN only if ids are really different + EntityId_t reid; + if (proxy.useMulticast && !m_enforceUnicast && proxy.unknown_eid) { + reid = ENTITYID_UNKNOWN; + } else { + reid = proxy.remoteReaderGuid.entityId; + } + MessageFactory::addSubMessageData(info.buffer, next->data, false, + next->sequenceNumber, + m_attributes.endpointGuid.entityId, + reid); // TODO + } + + // Just usable for IPv4 + // Decide which locator to be used unicast/multicast + + if (proxy.useMulticast && !m_enforceUnicast) { + info.destAddr = proxy.remoteMulticastLocator.getIp4Address(); + info.destPort = (Ip4Port_t)proxy.remoteMulticastLocator.port; + } else { + info.destAddr = proxy.remoteLocator.getIp4Address(); + info.destPort = (Ip4Port_t)proxy.remoteLocator.port; + } + SLW_LOG("Sending to %ld.%ld.%ld.%ld:%d\n", (info.destAddr.addr & 0xFF), + (info.destAddr.addr >> 8) & 0xFF, (info.destAddr.addr >> 16) & 0xFF, + (info.destAddr.addr >> 24) & 0xFF, info.destPort); + m_transport->sendPacket(info); + } + } + + m_history.removeUntilIncl(m_nextSequenceNumberToSend); + ++m_nextSequenceNumberToSend; +} diff --git a/components/rtps_embedded/include/rtps/entities/Writer.h b/components/rtps_embedded/include/rtps/entities/Writer.h new file mode 100644 index 000000000..d1693b7bb --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/Writer.h @@ -0,0 +1,113 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_WRITER_H +#define RTPS_WRITER_H + +#include "rtps/ThreadPool.h" +#include "rtps/discovery/TopicData.h" +#include "rtps/entities/ReaderProxy.h" +#include "rtps/storages/CacheChange.h" +#include "rtps/storages/MemoryPool.h" +#include "rtps/storages/PBufWrapper.h" + +#ifdef DEBUG_BUILD +#define COMPILE_INIT_GUARD +#endif + +#ifdef COMPILE_INIT_GUARD +#define INIT_GUARD() \ + if (!m_is_initialized_) { \ + while (1) { \ + printf("using uninitalized endpoint\n;"); \ + } \ + } +#else +#define INIT_GUARD() // +#endif + +namespace rtps { + +class Writer { +public: + TopicData m_attributes; + virtual bool addNewMatchedReader(const ReaderProxy &newProxy); + virtual bool removeProxy(const Guid_t &guid); + virtual void removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix); + virtual void reset() = 0; + virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size); + + //! Executes required steps like sending packets. Intended to be called by + //! worker threads + virtual void progress() = 0; + + virtual bool removeFromHistory(const SequenceNumber_t &s) = 0; + virtual void setAllChangesToUnsent() = 0; + virtual void onNewAckNack(const SubmessageAckNack &msg, + const GuidPrefix_t &sourceGuidPrefix) = 0; + + using dumpProxyCallback = void (*)(const Writer *writer, const ReaderProxy &, + void *arg); + + int dumpAllProxies(dumpProxyCallback target, void *arg); + + bool isInitialized(); + uint32_t getProxiesCount(); + + void setSEDPSequenceNumber(const SequenceNumber_t &sn); + const SequenceNumber_t &getSEDPSequenceNumber(); + + bool isBuiltinEndpoint(); + +protected: + SequenceNumber_t m_sedp_sequence_number; + + SemaphoreHandle_t m_mutex = nullptr; + ThreadPool *mp_threadPool = nullptr; + + Ip4Port_t m_srcPort; + + bool m_enforceUnicast; + + TopicKind_t m_topicKind = TopicKind_t::NO_KEY; + SequenceNumber_t m_nextSequenceNumberToSend; + + friend class SEDPAgent; + virtual const CacheChange *newChange(ChangeKind_t kind, const uint8_t *data, + DataSize_t size, bool inLineQoS, + bool markDisposedAfterWrite) = 0; + + friend class SizeInspector; + bool m_is_initialized_ = false; + virtual ~Writer() = default; + MemoryPool m_proxies; + + void resetSendOptions(); + void manageSendOptions(); + bool isIrrelevant(ChangeKind_t kind) const; +}; +} // namespace rtps + +#endif // RTPS_WRITER_H diff --git a/components/rtps_embedded/include/rtps/entities/WriterProxy.h b/components/rtps_embedded/include/rtps/entities/WriterProxy.h new file mode 100644 index 000000000..4db9e529d --- /dev/null +++ b/components/rtps_embedded/include/rtps/entities/WriterProxy.h @@ -0,0 +1,77 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_WRITERPROXY_H +#define RTPS_WRITERPROXY_H + +#include "rtps/common/types.h" +#include + +namespace rtps { +struct WriterProxy { + Guid_t remoteWriterGuid; + SequenceNumber_t expectedSN; + Count_t ackNackCount; + Count_t hbCount; + bool is_reliable; + LocatorIPv4 remoteLocator; + + WriterProxy() = default; + + WriterProxy(const Guid_t &guid, const LocatorIPv4 &loc, bool reliable) + : remoteWriterGuid(guid), + expectedSN(SequenceNumber_t{0, 1}), ackNackCount{1}, hbCount{0}, + is_reliable(reliable), remoteLocator(loc) {} + + // For now, we don't store any packets, so we just request all starting from + // the next expected + SequenceNumberSet getMissing(const SequenceNumber_t &firstAvail, + const SequenceNumber_t &lastAvail) { + SequenceNumberSet set; + if (lastAvail < expectedSN) { + set.base = expectedSN; + set.numBits = 0; + } else { + set.base = expectedSN; + SequenceNumber_t i; + uint32_t bit; + for (bit = 0, i = expectedSN; i <= lastAvail && bit < SNS_MAX_NUM_BITS; + i++, bit++) { + set.bitMap[0] |= uint32_t{1} << (31 - bit); + set.numBits++; + } + } + + return set; + } + + Count_t getNextAckNackCount() { + const Count_t tmp = ackNackCount; + ++ackNackCount.value; + return tmp; + } +}; +} // namespace rtps + +#endif // RTPS_WRITERPROXY_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageFactory.h b/components/rtps_embedded/include/rtps/messages/MessageFactory.h new file mode 100644 index 000000000..fae2f73f0 --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageFactory.h @@ -0,0 +1,219 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#ifndef RTPS_MESSAGEFACTORY_H +#define RTPS_MESSAGEFACTORY_H + +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/utils/sysFunctions.h" + +#include +#include + +namespace rtps { +namespace MessageFactory { +const std::array PROTOCOL_TYPE{'R', 'T', 'P', 'S'}; +const uint8_t numBytesUntilEndOfLength = + 4; // The first bytes incl. submessagelength don't count + +template +void addHeader(Buffer &buffer, const GuidPrefix_t &guidPrefix) { + + Header header; + header.protocolName = PROTOCOL_TYPE; + header.protocolVersion = PROTOCOLVERSION; + header.vendorId = Config::VENDOR_ID; + header.guidPrefix = guidPrefix; + + serializeMessage(buffer, header); +} + +template +bool addSubMessageInfoDST(Buffer &buffer, GuidPrefix_t &dst) { + SubmessageInfoDST msg; + msg.header.submessageId = SubmessageKind::INFO_DST; + +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + + msg.header.octetsToNextHeader = sizeof(GuidPrefix_t); + msg.guidPrefix = dst; + + return serializeMessage(buffer, msg); +} + +template +void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) { + SubmessageHeader header; + header.submessageId = SubmessageKind::INFO_TS; + +#if IS_LITTLE_ENDIAN + header.flags = FLAG_LITTLE_ENDIAN; +#else + header.flags = FLAG_BIG_ENDIAN; +#endif + + if (setInvalid) { + header.flags |= FLAG_INVALIDATE; + header.octetsToNextHeader = 0; + } else { + header.octetsToNextHeader = sizeof(Time_t); + } + + serializeMessage(buffer, header); + + if (!setInvalid) { + buffer.reserve(header.octetsToNextHeader); + Time_t now = getCurrentTimeStamp(); + buffer.append(reinterpret_cast(&now.seconds), + sizeof(Time_t::seconds)); + buffer.append(reinterpret_cast(&now.fraction), + sizeof(Time_t::fraction)); + } +} + +template +void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, + bool containsInlineQos, const SequenceNumber_t &SN, + const EntityId_t &writerID, const EntityId_t &readerID) { + SubmessageData msg; + msg.header.submessageId = SubmessageKind::DATA; +#if IS_LITTLE_ENDIAN + msg.header.flags = FLAG_LITTLE_ENDIAN; +#else + msg.header.flags = FLAG_BIG_ENDIAN; +#endif + + msg.header.octetsToNextHeader = SubmessageData::getRawSize() + + filledPayload.spaceUsed() - + numBytesUntilEndOfLength; + + if (containsInlineQos) { + msg.header.flags |= FLAG_INLINE_QOS; + } + if (filledPayload.isValid()) { + msg.header.flags |= FLAG_DATA_PAYLOAD; + } + + msg.writerSN = SN; + msg.extraFlags = 0; + msg.readerId = readerID; + msg.writerId = writerID; + + constexpr uint16_t octetsToInlineQoS = + 4 + 4 + 8; // EntityIds + SequenceNumber + msg.octetsToInlineQos = octetsToInlineQoS; + + serializeMessage(buffer, msg); + + if (filledPayload.isValid()) { + buffer.append(filledPayload); + } +} + +template +void addHeartbeat(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumber_t firstSN, SequenceNumber_t lastSN, + Count_t count) { + SubmessageHeartbeat subMsg; + subMsg.header.submessageId = SubmessageKind::HEARTBEAT; + subMsg.header.octetsToNextHeader = + SubmessageHeartbeat::getRawSize() - numBytesUntilEndOfLength; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + // Force response by not setting final flag. + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.firstSN = firstSN; + subMsg.lastSN = lastSN; + subMsg.count = count; + + serializeMessage(buffer, subMsg); +} + +template +void addAckNack(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + SequenceNumberSet readerSNState, Count_t count, + bool final_flag) { + SubmessageAckNack subMsg; + subMsg.header.submessageId = SubmessageKind::ACKNACK; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + if (final_flag) { + subMsg.header.flags |= FLAG_FINAL; // For now, we don't want any response + } else { + subMsg.header.flags &= + ~FLAG_FINAL; // Send future heartbeats, even if no change occured + } + subMsg.header.octetsToNextHeader = + SubmessageAckNack::getRawSize(readerSNState) - numBytesUntilEndOfLength; + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.readerSNState = readerSNState; + subMsg.count = count; + + serializeMessage(buffer, subMsg); +} + +template +void addSubmessageGap(Buffer &buffer, EntityId_t writerId, EntityId_t readerId, + const SequenceNumber_t &firstMissing, + const SequenceNumber_t &nextValid) { + SubmessageGap subMsg; + subMsg.header.submessageId = SubmessageKind::GAP; +#if IS_LITTLE_ENDIAN + subMsg.header.flags = FLAG_LITTLE_ENDIAN; +#else + subMsg.header.flags = FLAG_BIG_ENDIAN; +#endif + subMsg.header.octetsToNextHeader = 32; + + subMsg.writerId = writerId; + subMsg.readerId = readerId; + subMsg.gapStart = firstMissing; + subMsg.gapList.base = nextValid; + subMsg.gapList.numBits = 0; + + serializeMessage(buffer, subMsg); +} +} // namespace MessageFactory +} // namespace rtps + +#endif // RTPS_MESSAGEFACTORY_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageReceiver.h b/components/rtps_embedded/include/rtps/messages/MessageReceiver.h new file mode 100644 index 000000000..a4a62109f --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageReceiver.h @@ -0,0 +1,74 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MESSAGERECEIVER_H +#define RTPS_MESSAGERECEIVER_H + +#include "rtps/common/types.h" +#include "rtps/config.h" +#include "rtps/discovery/BuiltInEndpoints.h" +#include + +namespace rtps { +class Reader; +class Writer; +class Participant; +class MessageProcessingInfo; + +class MessageReceiver { +public: + GuidPrefix_t sourceGuidPrefix = GUIDPREFIX_UNKNOWN; + ProtocolVersion_t sourceVersion = PROTOCOLVERSION; + VendorId_t sourceVendor = VENDOR_UNKNOWN; + bool haveTimeStamp = false; + + explicit MessageReceiver(Participant *part); + + bool processMessage(const uint8_t *data, DataSize_t size); + +private: + Participant *mp_part; + + void resetState(); + + // TODO make msgInfo a member + // This probably make processing faster, as no parameter needs to be passed + // around However, we need to make sure data is set to nullptr after + // processMsg to make sure we don't access it again afterwards. + /** + * Check header for validity, modifies the state of the receiver and + * adjusts the position of msgInfo accordingly + */ + bool processGapSubmessage(MessageProcessingInfo &msgInfo); + bool processHeader(MessageProcessingInfo &msgInfo); + bool processSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader); + bool processDataSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader); + bool processHeartbeatSubmessage(MessageProcessingInfo &msgInfo); + bool processAckNackSubmessage(MessageProcessingInfo &msgInfo); +}; +} // namespace rtps + +#endif // RTPS_MESSAGERECEIVER_H diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.h b/components/rtps_embedded/include/rtps/messages/MessageTypes.h new file mode 100644 index 000000000..0d62cbbda --- /dev/null +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.h @@ -0,0 +1,444 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MESSAGES_H +#define RTPS_MESSAGES_H + +#include "rtps/common/types.h" + +#include + +namespace rtps { + +namespace SMElement { +// TODO endianess +enum ParameterId : uint16_t { + PID_PAD = 0x0000, + PID_SENTINEL = 0x0001, + PID_USER_DATA = 0x002c, + PID_TOPIC_NAME = 0x0005, + PID_TYPE_NAME = 0x0007, + PID_GROUP_DATA = 0x002d, + PID_TOPIC_DATA = 0x002e, + PID_DURABILITY = 0x001d, + PID_DURABILITY_SERVICE = 0x001e, + PID_DEADLINE = 0x0023, + PID_LATENCY_BUDGET = 0x0027, + PID_LIVELINESS = 0x001b, + PID_RELIABILITY = 0x001a, + PID_LIFESPAN = 0x002b, + PID_DESTINATION_ORDER = 0x0025, + PID_HISTORY = 0x0040, + PID_RESOURCE_LIMITS = 0x0041, + PID_OWNERSHIP = 0x001f, + PID_OWNERSHIP_STRENGTH = 0x0006, + PID_PRESENTATION = 0x0021, + PID_PARTITION = 0x0029, + PID_TIME_BASED_FILTER = 0x0004, + PID_TRANSPORT_PRIORITY = 0x0049, + PID_PROTOCOL_VERSION = 0x0015, + PID_VENDORID = 0x0016, + PID_UNICAST_LOCATOR = 0x002f, + PID_MULTICAST_LOCATOR = 0x0030, + PID_MULTICAST_IPADDRESS = 0x0011, + PID_DEFAULT_UNICAST_LOCATOR = 0x0031, + PID_DEFAULT_MULTICAST_LOCATOR = 0x0048, + PID_METATRAFFIC_UNICAST_LOCATOR = 0x0032, + PID_METATRAFFIC_MULTICAST_LOCATOR = 0x0033, + PID_DEFAULT_UNICAST_IPADDRESS = 0x000c, + PID_DEFAULT_UNICAST_PORT = 0x000e, + PID_METATRAFFIC_UNICAST_IPADDRESS = 0x0045, + PID_METATRAFFIC_UNICAST_PORT = 0x000d, + PID_METATRAFFIC_MULTICAST_IPADDRESS = 0x000b, + PID_METATRAFFIC_MULTICAST_PORT = 0x0046, + PID_EXPECTS_INLINE_QOS = 0x0043, + PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT = 0x0034, + PID_PARTICIPANT_BUILTIN_ENDPOINTS = 0x0044, + PID_PARTICIPANT_LEASE_DURATION = 0x0002, + PID_CONTENT_FILTER_PROPERTY = 0x0035, + PID_PARTICIPANT_GUID = 0x0050, + PID_PARTICIPANT_ENTITYID = 0x0051, + PID_GROUP_GUID = 0x0052, + PID_GROUP_ENTITYID = 0x0053, + PID_BUILTIN_ENDPOINT_SET = 0x0058, + PID_PROPERTY_LIST = 0x0059, + PID_ENDPOINT_GUID = 0x005a, + PID_TYPE_MAX_SIZE_SERIALIZED = 0x0060, + PID_ENTITY_NAME = 0x0062, + PID_KEY_HASH = 0x0070, + PID_STATUS_INFO = 0x0071 +}; + +enum BuildInEndpointSet : uint32_t { + DISC_BIE_PARTICIPANT_ANNOUNCER = 1 << 0, + DISC_BIE_PARTICIPANT_DETECTOR = 1 << 1, + DISC_BIE_PUBLICATION_ANNOUNCER = 1 << 2, + DISC_BIE_PUBLICATION_DETECTOR = 1 << 3, + DISC_BIE_SUBSCRIPTION_ANNOUNCER = 1 << 4, + DISC_BIE_SUBSCRIPTION_DETECTOR = 1 << 5, + + DISC_BIE_PARTICIPANT_PROXY_ANNOUNCER = 1 << 6, + DISC_BIE_PARTICIPANT_PROXY_DETECTOR = 1 << 7, + DISC_BIE_PARTICIPANT_STATE_ANNOUNCER = 1 << 8, + DISC_BIE_PARTICIPANT_STATE_DETECTOR = 1 << 9, + + BIE_PARTICIPANT_MESSAGE_DATA_WRITER = 1 << 10, + BIE_PARTICIPANT_MESSAGE_DATA_READER = 1 << 11, +}; + +// TODO endianess + +const std::array SCHEME_CDR_LE{0x00, 0x01}; +const std::array SCHEME_PL_CDR_LE{0x00, 0x03}; + +struct ParameterList_t { + ParameterId pid; + uint16_t length; + // Values follow +} __attribute__((packed)); +} // namespace SMElement + +enum class SubmessageKind : uint8_t { + PAD = 0x01, /* Pad */ + ACKNACK = 0x06, /* AckNack */ + HEARTBEAT = 0x07, /* Heartbeat */ + GAP = 0x08, /* Gap */ + INFO_TS = 0x09, /* InfoTimestamp */ + INFO_SRC = 0x0c, /* InfoSource */ + INFO_REPLY_IP4 = 0x0d, /* InfoReplyIp4 */ + INFO_DST = 0x0e, /* InfoDestination */ + INFO_REPLY = 0x0f, /* InfoReply */ + NACK_FRAG = 0x12, /* NackFrag */ + HEARTBEAT_FRAG = 0x13, /* HeartbeatFrag */ + DATA = 0x15, /* Data */ + DATA_FRAG = 0x16 /* DataFrag */ +}; + +enum SubMessageFlag : uint8_t { + FLAG_ENDIANESS = (1 << 0), + FLAG_BIG_ENDIAN = (0 << 0), + FLAG_LITTLE_ENDIAN = (1 << 0), + FLAG_INVALIDATE = (1 << 1), + FLAG_INLINE_QOS = (1 << 1), + FLAG_NO_PAYLOAD = (0 << 3 | 0 << 2), + FLAG_DATA_PAYLOAD = (0 << 3 | 1 << 2), + FLAG_FINAL = (1 << 1), + FLAG_HB_LIVELINESS = (1 << 2) +}; + +const std::array RTPS_PROTOCOL_NAME = {'R', 'T', 'P', 'S'}; +struct Header { + std::array protocolName; + ProtocolVersion_t protocolVersion; + VendorId_t vendorId; + GuidPrefix_t guidPrefix; + static constexpr uint16_t getRawSize() { + return sizeof(std::array) + sizeof(ProtocolVersion_t) + + sizeof(VendorId_t) + sizeof(GuidPrefix_t); + } +}; + +struct SubmessageHeader { + SubmessageKind submessageId; + uint8_t flags; + uint16_t octetsToNextHeader; + static constexpr uint16_t getRawSize() { + return sizeof(SubmessageKind) + sizeof(uint8_t) + sizeof(uint16_t); + } + + bool finalFlag() const { return (flags & (SubMessageFlag::FLAG_FINAL)); } +}; + +struct SubmessageData { + SubmessageHeader header; + uint16_t extraFlags; + uint16_t octetsToInlineQos; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t writerSN; + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + sizeof(uint16_t) + + sizeof(uint16_t) + (2 * 3 + 2 * 1) // EntityID + + sizeof(SequenceNumber_t); + } +}; + +struct SubmessageHeartbeat { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t firstSN; + SequenceNumber_t lastSN; + Count_t count; + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + (2 * 3 + 2 * 1) // EntityID + + 2 * sizeof(SequenceNumber_t) + sizeof(Count_t); + } +}; + +struct SubmessageInfoDST { + SubmessageHeader header; + GuidPrefix_t guidPrefix; + + static constexpr uint16_t getRawSize() { + return SubmessageHeader::getRawSize() + (sizeof(guidPrefix)); + } +}; + +struct SubmessageGap { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumber_t gapStart; + SequenceNumberSet gapList; + + static uint16_t getRawSizeWithoutSNSet() { + return SubmessageHeader::getRawSize() + + (2 * (3 + 1) + 8); // 2*EntityID + GapStart + } + + static uint16_t getRawSizeWithSingleElementSNSet() { + return SubmessageHeader::getRawSize() + + (2 * (3 + 1) + 8 + 8 + + 4); // 2*EntityID + GapStart + bitmapBase + numBits + } +}; + +struct SubmessageAckNack { + SubmessageHeader header; + EntityId_t readerId; + EntityId_t writerId; + SequenceNumberSet readerSNState; + Count_t count; + static uint16_t getRawSize(const SequenceNumberSet &set) { + uint16_t bitMapSize = 0; + if (set.numBits != 0) { + bitMapSize = SNS_NUM_BYTES; + } + return getRawSizeWithoutSNSet() + sizeof(SequenceNumber_t) + + sizeof(uint32_t) + bitMapSize; // SequenceNumberSet + } + static uint16_t getRawSizeWithoutSNSet() { + return SubmessageHeader::getRawSize() + (2 * (3 + 1)) + sizeof(Count_t); + } +}; + +template +bool serializeMessage(Buffer &buffer, Header &header) { + if (!buffer.reserve(Header::getRawSize())) { + return false; + } + + buffer.append(header.protocolName.data(), sizeof(std::array)); + buffer.append(reinterpret_cast(&header.protocolVersion), + sizeof(ProtocolVersion_t)); + buffer.append(header.vendorId.vendorId.data(), sizeof(VendorId_t)); + buffer.append(header.guidPrefix.id.data(), sizeof(GuidPrefix_t)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageHeader &header) { + if (!buffer.reserve(Header::getRawSize())) { + return false; + } + buffer.reserve(SubmessageHeader::getRawSize()); + + buffer.append(reinterpret_cast(&header.submessageId), + sizeof(SubmessageKind)); + buffer.append(&header.flags, sizeof(uint8_t)); + buffer.append(reinterpret_cast(&header.octetsToNextHeader), + sizeof(uint16_t)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageInfoDST &msg) { + if (!buffer.reserve(SubmessageInfoDST::getRawSize())) { + return false; + } + + bool ret = serializeMessage(buffer, msg.header); + if (!ret) { + return false; + } + + return buffer.append(msg.guidPrefix.id.data(), sizeof(GuidPrefix_t)); +} + +template +bool serializeMessage(Buffer &buffer, SubmessageData &msg) { + if (!buffer.reserve(SubmessageData::getRawSize())) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(reinterpret_cast(&msg.extraFlags), sizeof(uint16_t)); + buffer.append(reinterpret_cast(&msg.octetsToInlineQos), + sizeof(uint16_t)); + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.writerSN.high), + sizeof(msg.writerSN.high)); + buffer.append(reinterpret_cast(&msg.writerSN.low), + sizeof(msg.writerSN.low)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageHeartbeat &msg) { + if (!buffer.reserve(SubmessageHeartbeat::getRawSize())) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.firstSN.high), + sizeof(msg.firstSN.high)); + buffer.append(reinterpret_cast(&msg.firstSN.low), + sizeof(msg.firstSN.low)); + buffer.append(reinterpret_cast(&msg.lastSN.high), + sizeof(msg.lastSN.high)); + buffer.append(reinterpret_cast(&msg.lastSN.low), + sizeof(msg.lastSN.low)); + buffer.append(reinterpret_cast(&msg.count.value), + sizeof(msg.count.value)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageAckNack &msg) { + if (!buffer.reserve(SubmessageAckNack::getRawSize(msg.readerSNState))) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(reinterpret_cast(&msg.readerSNState.base.high), + sizeof(msg.readerSNState.base.high)); + buffer.append(reinterpret_cast(&msg.readerSNState.base.low), + sizeof(msg.readerSNState.base.low)); + buffer.append(reinterpret_cast(&msg.readerSNState.numBits), + sizeof(uint32_t)); + if (msg.readerSNState.numBits != 0) { + buffer.append(reinterpret_cast(msg.readerSNState.bitMap.data()), + 4 * ((msg.readerSNState.numBits / 32) + 1)); + } + buffer.append(reinterpret_cast(&msg.count.value), + sizeof(msg.count.value)); + return true; +} + +template +bool serializeMessage(Buffer &buffer, SubmessageGap &msg) { + if (msg.gapList.numBits != 0) { + return false; + } + if (!buffer.reserve(36)) { + return false; + } + + serializeMessage(buffer, msg.header); + + buffer.append(msg.readerId.entityKey.data(), msg.readerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.readerId.entityKind), + sizeof(EntityKind_t)); + buffer.append(msg.writerId.entityKey.data(), msg.writerId.entityKey.size()); + buffer.append(reinterpret_cast(&msg.writerId.entityKind), + sizeof(EntityKind_t)); + + buffer.append(reinterpret_cast(&msg.gapStart.high), + sizeof(msg.gapStart.high)); + buffer.append(reinterpret_cast(&msg.gapStart.low), + sizeof(msg.gapStart.low)); + + buffer.append(reinterpret_cast(&msg.gapList.base.high), + sizeof(msg.gapList.base.high)); + buffer.append(reinterpret_cast(&msg.gapList.base.low), + sizeof(msg.gapList.base.low)); + + buffer.append(reinterpret_cast(&msg.gapList.numBits), + sizeof(uint32_t)); + + buffer.append(reinterpret_cast(&msg.gapList.numBits), + sizeof(uint32_t)); + + return true; +} + +struct MessageProcessingInfo { + MessageProcessingInfo(const uint8_t *data, DataSize_t size) + : data(data), size(size) {} + const uint8_t *data; + const DataSize_t size; + + //! Offset to the next unprocessed byte + DataSize_t nextPos = 0; + + inline const uint8_t *getPointerToCurrentPos() const { + return &data[nextPos]; + } + + //! Returns the size of data which isn't processed yet + inline DataSize_t getRemainingSize() const { return size - nextPos; } +}; + +bool deserializeMessage(const MessageProcessingInfo &info, Header &header); + +bool deserializeMessage(const MessageProcessingInfo &info, + SubmessageHeader &header); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageData &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, + SubmessageHeartbeat &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, + SubmessageAckNack &msg); + +bool deserializeMessage(const MessageProcessingInfo &info, SubmessageGap &msg); + +void deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, + size_t num_bitfields); + +} // namespace rtps + +#endif // RTPS_MESSAGES_H diff --git a/components/rtps_embedded/include/rtps/rtps.h b/components/rtps_embedded/include/rtps/rtps.h new file mode 100644 index 000000000..4995e0162 --- /dev/null +++ b/components/rtps_embedded/include/rtps/rtps.h @@ -0,0 +1,39 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_RTPS_H +#define RTPS_RTPS_H + +#include "rtps/entities/Domain.h" + +namespace rtps { + +#if defined(unix) || defined(__unix__) || defined(WIN32) || defined(_WIN32) || \ + defined(__WIN32) && !defined(__CYGWIN__) +void init(); +#endif + +} // namespace rtps + +#endif // RTPS_RTPS_H diff --git a/components/rtps_embedded/include/rtps/storages/CacheChange.h b/components/rtps_embedded/include/rtps/storages/CacheChange.h new file mode 100644 index 000000000..dc76bb974 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/CacheChange.h @@ -0,0 +1,68 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_CACHECHANGE_H +#define PROJECT_CACHECHANGE_H + +#include "rtps/common/types.h" +#include "rtps/storages/PBufWrapper.h" + +namespace rtps { +struct CacheChange { + ChangeKind_t kind = ChangeKind_t::INVALID; + bool inLineQoS = false; + bool disposeAfterWrite = false; + TickType_t sentTickCount = 0; + SequenceNumber_t sequenceNumber = SEQUENCENUMBER_UNKNOWN; + PBufWrapper data; + + CacheChange &operator=(const CacheChange &other) = delete; + + CacheChange &operator=(CacheChange &&other) noexcept { + kind = other.kind; + inLineQoS = other.inLineQoS; + disposeAfterWrite = other.disposeAfterWrite; + sentTickCount = other.sentTickCount; + sequenceNumber = other.sequenceNumber; + data = std::move(other.data); + return *this; + } + + CacheChange() = default; + CacheChange(ChangeKind_t kind, SequenceNumber_t sequenceNumber) + : kind(kind), sequenceNumber(sequenceNumber){}; + + void reset() { + kind = ChangeKind_t::INVALID; + sequenceNumber = SEQUENCENUMBER_UNKNOWN; + inLineQoS = false; + disposeAfterWrite = false; + sentTickCount = 0; + } + + bool isInitialized() { return (kind != ChangeKind_t::INVALID); } +}; +} // namespace rtps + +#endif // PROJECT_CACHECHANGE_H diff --git a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h new file mode 100644 index 000000000..cb1c382f4 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h @@ -0,0 +1,282 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef HISTORYCACHEWITHDELETION_H +#define HISTORYCACHEWITHDELETION_H + +#include +#include + +namespace rtps { + +/** + * Extension of the SimpleHistoryCache that allows for deletion operation at the + * cost of efficieny + * TODO: Replace with something better in the future! + * Likely only used for SEDP + */ +template class HistoryCacheWithDeletion { +public: + HistoryCacheWithDeletion() = default; + + uint32_t m_dispose_after_write_cnt = 0; + + bool isFull() const { + uint16_t it = m_head; + incrementIterator(it); + return it == m_tail; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size, + bool inLineQoS, bool disposeAfterWrite) { + CacheChange change; + change.kind = ChangeKind_t::ALIVE; + change.inLineQoS = inLineQoS; + change.disposeAfterWrite = disposeAfterWrite; + change.data.reserve(size); + change.data.append(data, size); + change.sequenceNumber = ++m_lastUsedSequenceNumber; + + if (disposeAfterWrite) { + m_dispose_after_write_cnt++; + } + + CacheChange *place = &m_buffer[m_head]; + incrementHead(); + + *place = std::move(change); + return place; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size) { + return addChange(data, size, 0, false); + } + + void removeUntilIncl(SequenceNumber_t sn) { + if (m_head == m_tail) { + return; + } + + if (getCurrentSeqNumMax() <= sn) { // We won't overrun head + m_head = m_tail; + return; + } + + while (m_buffer[m_tail].sequenceNumber <= sn) { + incrementTail(); + } + } + + void dropOldest() { removeUntilIncl(getCurrentSeqNumMin()); } + + bool dropChange(const SequenceNumber_t &sn) { + uint16_t idx_to_clear; + CacheChange *change; + if (!getChangeBySN(sn, &change, idx_to_clear)) { + return false; // sn does not exist, nothing to do + } + + if (idx_to_clear == m_tail) { + m_buffer[m_tail].reset(); + incrementTail(); + return true; + } + + uint16_t prev = idx_to_clear; + do { + prev = idx_to_clear - 1; + if (prev >= m_buffer.size()) { + prev = m_buffer.size() - 1; + } + + m_buffer[idx_to_clear] = std::move(m_buffer[prev]); + idx_to_clear = prev; + + } while (prev != m_tail); + + incrementTail(); + + return true; + } + + bool setCacheChangeKind(const SequenceNumber_t &sn, ChangeKind_t kind) { + CacheChange *change = getChangeBySN(sn); + if (change == nullptr) { + return false; + } + + change->kind = kind; + return true; + } + + CacheChange *getChangeBySN(SequenceNumber_t sn) { + CacheChange *change; + uint16_t position; + if (getChangeBySN(sn, &change, position)) { + return change; + } else { + return nullptr; + } + } + + bool isEmpty() { return (m_head == m_tail); } + + const SequenceNumber_t &getCurrentSeqNumMin() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_buffer[m_tail].sequenceNumber; + } + } + + const SequenceNumber_t &getCurrentSeqNumMax() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_lastUsedSequenceNumber; + } + } + + const SequenceNumber_t &getLastUsedSequenceNumber() { + return m_lastUsedSequenceNumber; + } + + void clear() { + m_head = 0; + m_tail = 0; + m_lastUsedSequenceNumber = {0, 0}; + } +#ifdef DEBUG_HISTORY_CACHE_WITH_DELETION + void print() { + for (unsigned int i = 0; i < m_buffer.size(); i++) { + std::cout << "[" << i << "] " + << " SN = " << m_buffer[i].sequenceNumber.low; + switch (m_buffer[i].kind) { + case ChangeKind_t::ALIVE: + std::cout << " Type = ALIVE"; + break; + case ChangeKind_t::INVALID: + std::cout << " Type = INVALID"; + break; + case ChangeKind_t::NOT_ALIVE_DISPOSED: + std::cout << " Type = DISPOSED"; + break; + } + if (m_head == i) { + std::cout << " <- HEAD"; + } + if (m_tail == i) { + std::cout << " <- TAIL"; + } + std::cout << std::endl; + } + } +#endif + bool isSNInRange(const SequenceNumber_t &sn) { + if (isEmpty()) { + return false; + } + SequenceNumber_t minSN = getCurrentSeqNumMin(); + if (sn < minSN || getCurrentSeqNumMax() < sn) { + return false; + } + return true; + } + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + static_assert(sizeof(SIZE) <= sizeof(m_head), + "Iterator is large enough for given size"); + + SequenceNumber_t m_lastUsedSequenceNumber{0, 0}; + + bool getChangeBySN(const SequenceNumber_t &sn, CacheChange **out_change, + uint16_t &out_buffer_position) { + if (!isSNInRange(sn)) { + return false; + } + static_assert(std::is_unsigned::value, + "Underflow well defined"); + static_assert(sizeof(m_tail) <= sizeof(uint16_t), "Cast ist well defined"); + + unsigned int cur_idx = m_tail; + while (cur_idx != m_head) { + if (m_buffer[cur_idx].sequenceNumber == sn) { + *out_change = &m_buffer[cur_idx]; + out_buffer_position = cur_idx; + return true; + } + // Sequence numbers are consecutive + if (m_buffer[cur_idx].sequenceNumber > sn) { + *out_change = nullptr; + return false; + } + + cur_idx++; + if (cur_idx >= m_buffer.size()) { + cur_idx -= m_buffer.size(); + } + } + + *out_change = nullptr; + return false; + } + + inline void incrementHead() { + incrementIterator(m_head); + if (m_head == m_tail) { + // Move without check + incrementIterator(m_tail); // drop one + } + } + + inline void incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } + } + + inline void incrementTail() { + if (m_buffer[m_tail].disposeAfterWrite) { + m_dispose_after_write_cnt--; + } + if (m_head != m_tail) { + m_buffer[m_tail].reset(); + incrementIterator(m_tail); + } + } + +protected: + // This constructor was created for unit testing + explicit HistoryCacheWithDeletion(SequenceNumber_t lastUsed) + : HistoryCacheWithDeletion() { + m_lastUsedSequenceNumber = lastUsed; + } +}; +} // namespace rtps + +#endif // HISTORYCACHEWITHDELETION_H diff --git a/components/rtps_embedded/include/rtps/storages/MemoryPool.h b/components/rtps_embedded/include/rtps/storages/MemoryPool.h new file mode 100644 index 000000000..c4bbc2c23 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/MemoryPool.h @@ -0,0 +1,192 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_MEMORYPOOL_H +#define RTPS_MEMORYPOOL_H + +#include +#include +#include + +namespace rtps { + +template class MemoryPool { +public: + template class MemoryPoolIterator { + public: + using iterator_category = std::input_iterator_tag; + using value_type = IT_TYPE; + using difference_type = uint8_t; + using pointer = IT_TYPE *; + using reference = IT_TYPE &; + + explicit MemoryPoolIterator(MemoryPool &pool) : m_pool(pool) { + memcpy(m_bitMap, m_pool.m_bitMap, sizeof(m_bitMap)); + } + + // bool operator==(const MemoryPoolIterator& other) const{ + // return bit == other.bit; + //} + + bool operator!=(const MemoryPoolIterator &other) const { + return m_bit != other.m_bit; + } + + reference operator*() const { return m_pool.m_data[m_bit]; } + + reference operator->() const { return m_pool.m_data[m_bit]; } + + // Pre-increment + MemoryPoolIterator &operator++() { + if (m_pool.m_numElements == 0) { + m_bit = SIZE; + return *this; + } + uint32_t bucket; + do { + ++m_bit; + bucket = m_bit / static_cast(8); + } while (!(m_bitMap[bucket] & (1 << (m_bit % 8))) && m_bit < SIZE); + + return *this; + } + + // Post-increment + MemoryPoolIterator operator++(int) { + MemoryPoolIterator tmp(*this); + ++(*this); + return tmp; + } + + private: + friend class MemoryPool; + MemoryPool &m_pool; + uint8_t m_bitMap[SIZE / 8 + 1]; + uint32_t m_bit = 0; + }; + + typedef MemoryPoolIterator MemPoolIter; + typedef MemoryPoolIterator const_MemPoolIter; + + typedef bool (*condition_fp)(TYPE); + + uint32_t getSize() { return SIZE; } + + bool isFull() { return m_numElements == SIZE; } + + bool isEmpty() { return m_numElements == 0; } + + uint32_t getNumElements() { return m_numElements; } + + bool add(const TYPE &data) { + if (isFull()) { + printf("[MemoryPool] RESSOURCE LIMIT EXCEEDED \n"); + return false; + } + for (uint32_t bucket = 0; bucket < sizeof(m_bitMap); ++bucket) { + if (m_bitMap[bucket] != 0xFF) { + uint8_t byte = m_bitMap[bucket]; + for (uint8_t bit = 0; bit < 8; ++bit) { + if (!(byte & 1)) { + m_bitMap[bucket] |= 1 << bit; + m_data[bucket * 8 + bit] = data; + ++m_numElements; + return true; + } + byte = byte >> 1; + } + } + } + return false; + } + + /** + * Parameters are used in that way to allow lambdas with captures. Use this by + * creating two: E.g.: auto callback=[data](TYPE& value){return value == + * data;}; auto thunk=[](void* arg, TYPE& value){return + * (*static_cast(arg))(value);}; + * + * and then simply call: + * remove(thunk, &callback) + * + * NOTE: You have to make sure that the callback did not run out of scope. + */ + bool remove(bool (*jumppad)(void *, const TYPE &data), + void *isCorrectElement) { + bool retcode = false; + for (auto it = begin(); it != end(); ++it) { + if (jumppad(isCorrectElement, *it)) { + const uint32_t bucket = it.m_bit / uint32_t{8}; + const uint32_t pos = + it.m_bit & + uint32_t{ + 7}; // 7 sets all bits above and including the one for 8 to 0 + m_bitMap[bucket] &= ~(static_cast(1) << pos); + --m_numElements; + retcode = true; + } + } + return retcode; + } + + void clear() { + for (unsigned int i = 0; i < (SIZE / 8 + 1); i++) { + m_bitMap[i] = 0; + } + m_numElements = 0; + } + + TYPE *find(bool (*jumppad)(void *, const TYPE &data), + void *isCorrectElement) { + for (auto it = begin(); it != end(); ++it) { + if (jumppad(isCorrectElement, *it)) { + return &(*it); + } + } + return nullptr; + } + + MemPoolIter begin() { + MemPoolIter it(*this); + if (!(m_bitMap[0] & 1)) { + ++it; + } + return it; + } + + MemPoolIter end() { + MemPoolIter endIt(*this); + endIt.m_bit = SIZE; + return endIt; + } + +private: + uint8_t m_bitMap[SIZE / 8 + 1]{}; + uint32_t m_numElements = 0; + TYPE m_data[SIZE]; +}; + +} // namespace rtps + +#endif // RTPS_MEMORYPOOL_H diff --git a/components/rtps_embedded/include/rtps/storages/PBufWrapper.h b/components/rtps_embedded/include/rtps/storages/PBufWrapper.h new file mode 100644 index 000000000..7a5ff2a70 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/PBufWrapper.h @@ -0,0 +1,84 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#ifndef RTPS_PBUFWRAPPER_H +#define RTPS_PBUFWRAPPER_H + +#include "lwip/pbuf.h" +#include "rtps/common/types.h" + +namespace rtps { + +struct PBufWrapper { + + pbuf *firstElement = nullptr; + + PBufWrapper() = default; + explicit PBufWrapper(pbuf *bufferToWrap); + explicit PBufWrapper(DataSize_t length); + + PBufWrapper(const PBufWrapper &other) = delete; + PBufWrapper &operator=(const PBufWrapper &other) = delete; + + PBufWrapper(PBufWrapper &&other) noexcept; + PBufWrapper &operator=(PBufWrapper &&other) noexcept; + + ~PBufWrapper(); + + bool isValid() const; + + bool append(const uint8_t *data, DataSize_t length); + + /// Note that unused reserved memory is now part of the wrapper. New calls to + /// append(uint8_t*[...]) will continue behind the appended wrapper + void append(const PBufWrapper &other); + + bool reserve(DataSize_t length); + + void destroy(); + + /// After calling this function, data is added starting from the beginning + /// again. It does not revert reserve. + void reset(); + + DataSize_t spaceLeft() const; + DataSize_t spaceUsed() const; + +private: + constexpr static pbuf_layer m_layer = PBUF_TRANSPORT; + constexpr static pbuf_type m_type = PBUF_POOL; + + DataSize_t m_freeSpace = 0; + + bool increaseSizeBy(uint16_t length); + + void copySimpleMembersAndResetBuffer(const PBufWrapper &other); +}; + +} // namespace rtps + +#endif // RTPS_PBUFWRAPPER_H diff --git a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h new file mode 100644 index 000000000..ba6715eb3 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h @@ -0,0 +1,178 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_SIMPLEHISTORYCACHE_H +#define PROJECT_SIMPLEHISTORYCACHE_H + +#include "rtps/config.h" +#include "rtps/storages/CacheChange.h" + +namespace rtps { + +/** + * Simple version of a history cache. It sets consecutive sequence numbers + * automatically which allows an easy and fast approach of dropping acknowledged + * changes. Furthermore, disposing of arbitrary changes is not possible. + * However, this is in principle easy to add by changing the ChangeKind and + * dropping it when passing it during deleting of other sequence numbers + */ +template class SimpleHistoryCache { +public: + SimpleHistoryCache() = default; + + bool isFull() const { + uint16_t it = m_head; + incrementIterator(it); + return it == m_tail; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size, + bool inLineQoS, bool disposeAfterWrite) { + CacheChange change; + change.kind = ChangeKind_t::ALIVE; + change.inLineQoS = inLineQoS; + change.disposeAfterWrite = disposeAfterWrite; + change.data.reserve(size); + change.data.append(data, size); + change.sequenceNumber = ++m_lastUsedSequenceNumber; + + CacheChange *place = &m_buffer[m_head]; + incrementHead(); + + *place = std::move(change); + return place; + } + + const CacheChange *addChange(const uint8_t *data, DataSize_t size) { + return addChange(data, size, 0, false); + } + + void removeUntilIncl(SequenceNumber_t sn) { + if (m_head == m_tail) { + return; + } + + if (getSeqNumMax() <= sn) { // We won't overrun head + m_head = m_tail; + return; + } + + while (m_buffer[m_tail].sequenceNumber <= sn && (m_head != m_tail)) { + incrementTail(); + } + } + + void dropOldest() { removeUntilIncl(getSeqNumMin()); } + + bool setCacheChangeKind(const SequenceNumber_t &sn, ChangeKind_t kind) { + CacheChange *change = getChangeBySN(sn); + if (change == nullptr) { + return false; + } + + change->kind = kind; + return true; + } + + CacheChange *getChangeBySN(SequenceNumber_t sn) { + SequenceNumber_t minSN = getSeqNumMin(); + if (sn < minSN || getSeqNumMax() < sn) { + return nullptr; + } + static_assert(std::is_unsigned::value, + "Underflow well defined"); + static_assert(sizeof(m_tail) <= sizeof(uint16_t), "Cast ist well defined"); + // We don't overtake head, therefore difference of sn is within same range + // as iterators + uint16_t pos = m_tail + static_cast(sn.low - minSN.low); + + // Diff is smaller than the size of the array -> max one overflow + if (pos >= m_buffer.size()) { + pos -= m_buffer.size(); + } + return &m_buffer[pos]; + } + + const SequenceNumber_t &getSeqNumMin() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_buffer[m_tail].sequenceNumber; + } + } + + const SequenceNumber_t &getSeqNumMax() const { + if (m_head == m_tail) { + return SEQUENCENUMBER_UNKNOWN; + } else { + return m_lastUsedSequenceNumber; + } + } + + void clear() { + m_head = 0; + m_tail = 0; + m_lastUsedSequenceNumber = {0, 0}; + } + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + static_assert(sizeof(SIZE) <= sizeof(m_head), + "Iterator is large enough for given size"); + + SequenceNumber_t m_lastUsedSequenceNumber{0, 0}; + + inline void incrementHead() { + incrementIterator(m_head); + if (m_head == m_tail) { + // Move without check + incrementIterator(m_tail); // drop one + } + } + + inline void incrementIterator(uint16_t &iterator) const { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } + } + + inline void incrementTail() { + if (m_head != m_tail) { + incrementIterator(m_tail); + } + } + +protected: + // This constructor was created for unit testing + explicit SimpleHistoryCache(SequenceNumber_t lastUsed) + : SimpleHistoryCache() { + m_lastUsedSequenceNumber = lastUsed; + } +}; +} // namespace rtps + +#endif // PROJECT_SIMPLEHISTORYCACHE_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h new file mode 100644 index 000000000..4f4f7661e --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h @@ -0,0 +1,83 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_THREADSAFEQUEUE_H +#define RTPS_THREADSAFEQUEUE_H + +#include "lwip/sys.h" +#if defined(ESP_PLATFORM) +#include "freertos/semphr.h" +#else +#include "semphr.h" +#endif + +#include +#include + +namespace rtps { + +template class ThreadSafeCircularBuffer { + +public: + bool init(); + + bool moveElementIntoBuffer(T &&elem); + bool copyElementIntoBuffer(const T &elem); + + /** + * Removes the first into the given hull. Also moves responsibility for + * resources. + * @return true if element was injected. False if no element was present. + */ + bool moveFirstInto(T &hull); + bool peakFirst(T &hull); + + uint32_t numElements(); + uint32_t insertionFailures(); + + void clear(); + +private: + std::array m_buffer{}; + uint16_t m_head = 0; + uint16_t m_tail = 0; + uint32_t m_num_elements = 0; + uint32_t m_insertion_failures = 0; + static_assert(SIZE + 1 < std::numeric_limits::max(), + "Iterator is large enough for given size"); + + SemaphoreHandle_t m_mutex = nullptr; + bool m_initialized = false; + + inline bool isFull(); + inline void incrementIterator(uint16_t &iterator); + inline void incrementTail(); + inline void incrementHead(); +}; + +} // namespace rtps + +#include "ThreadSafeCircularBuffer.tpp" + +#endif // RTPS_THREADSAFEQUEUE_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp new file mode 100644 index 000000000..be1d7e843 --- /dev/null +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp @@ -0,0 +1,130 @@ + +#ifndef RTPS_THREADSAFECIRCULARBUFFER_TPP +#define RTPS_THREADSAFECIRCULARBUFFER_TPP + +#include "rtps/utils/Lock.h" +#include "rtps/utils/Log.h" + +#if TSCB_VERBOSE && RTPS_GLOBAL_VERBOSE +#define TSCB_LOG(...) \ + if (true) { \ + printf("[TSCircularBuffer] "); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define TSCB_LOG(...) // +#endif + +namespace rtps { + +template +bool ThreadSafeCircularBuffer::init() { + if (m_initialized) { + return true; + } + if (!createMutex(&m_mutex)) { + TSCB_LOG("Failed to create mutex \n"); + return false; + } else { + TSCB_LOG("Successfully created mutex at %p\n", + static_cast(&m_mutex)); + m_initialized = true; + return true; + } +} + +template +bool ThreadSafeCircularBuffer::moveElementIntoBuffer(T &&elem) { + Lock lock(m_mutex); + if (!isFull()) { + m_buffer[m_head] = std::move(elem); + incrementHead(); + return true; + } else { + m_insertion_failures++; + return false; + } +} + +template +bool ThreadSafeCircularBuffer::copyElementIntoBuffer(const T &elem) { + Lock lock(m_mutex); + if (!isFull()) { + m_buffer[m_head] = elem; + incrementHead(); + return true; + } else { + m_insertion_failures++; + return false; + } +} + +template +bool ThreadSafeCircularBuffer::moveFirstInto(T &hull) { + Lock lock(m_mutex); + if (m_head != m_tail) { + hull = std::move(m_buffer[m_tail]); + incrementTail(); + return true; + } else { + return false; + } +} + +template +bool ThreadSafeCircularBuffer::peakFirst(T &hull) { + Lock lock(m_mutex); + if (m_head != m_tail) { + hull = m_buffer[m_tail]; + return true; + } else { + return false; + } +} + +template +uint32_t ThreadSafeCircularBuffer::numElements() { + return m_num_elements; +} + +template +void ThreadSafeCircularBuffer::clear() { + Lock lock(m_mutex); + m_head = m_tail; + m_num_elements = 0; +} + +template +bool ThreadSafeCircularBuffer::isFull() { + auto it = m_head; + incrementIterator(it); + return it == m_tail; +} + +template +inline void +ThreadSafeCircularBuffer::incrementIterator(uint16_t &iterator) { + ++iterator; + if (iterator >= m_buffer.size()) { + iterator = 0; + } +} + +template +inline void ThreadSafeCircularBuffer::incrementTail() { + incrementIterator(m_tail); + m_num_elements--; +} + +template +inline void ThreadSafeCircularBuffer::incrementHead() { + incrementIterator(m_head); + m_num_elements++; + if (m_head == m_tail) { + incrementTail(); + } +} +} // namespace rtps + +#endif // RTPS_THREADSAFECIRCULARBUFFER_TPP diff --git a/components/rtps_embedded/include/rtps/utils/Diagnostics.h b/components/rtps_embedded/include/rtps/utils/Diagnostics.h new file mode 100644 index 000000000..fbc4aad95 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/Diagnostics.h @@ -0,0 +1,85 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_DIAGNOSTICS_H +#define RTPS_DIAGNOSTICS_H + +#include + +namespace rtps { +namespace Diagnostics { + +namespace ThreadPool { +extern uint32_t dropped_incoming_packets_usertraffic; +extern uint32_t dropped_incoming_packets_metatraffic; + +extern uint32_t dropped_outgoing_packets_usertraffic; +extern uint32_t dropped_outgoing_packets_metatraffic; + +extern uint32_t processed_incoming_metatraffic; +extern uint32_t processed_outgoing_metatraffic; +extern uint32_t processed_incoming_usertraffic; +extern uint32_t processed_outgoing_usertraffic; + +extern uint32_t max_ever_elements_outgoing_usertraffic_queue; +extern uint32_t max_ever_elements_incoming_usertraffic_queue; + +extern uint32_t max_ever_elements_outgoing_metatraffic_queue; +extern uint32_t max_ever_elements_incoming_metatraffic_queue; +} // namespace ThreadPool + +namespace StatefulReader { +extern uint32_t sfr_unexpected_sn; +extern uint32_t sfr_retransmit_requests; +} // namespace StatefulReader + +namespace Network { +extern uint32_t lwip_allocation_failures; +} + +namespace OS { +extern uint32_t current_free_heap_size; +} + +namespace SEDP { +extern uint32_t max_ever_remote_participants; +extern uint32_t current_remote_participants; + +extern uint32_t max_ever_matched_reader_proxies; +extern uint32_t current_max_matched_reader_proxies; + +extern uint32_t max_ever_matched_writer_proxies; +extern uint32_t current_max_matched_writer_proxies; + +extern uint32_t max_ever_unmatched_reader_proxies; +extern uint32_t current_max_unmatched_reader_proxies; + +extern uint32_t max_ever_unmatched_writer_proxies; +extern uint32_t current_max_unmatched_writer_proxies; +} // namespace SEDP + +} // namespace Diagnostics +} // namespace rtps + +#endif diff --git a/components/rtps_embedded/include/rtps/utils/Lock.h b/components/rtps_embedded/include/rtps/utils/Lock.h new file mode 100644 index 000000000..7a3739213 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/Lock.h @@ -0,0 +1,61 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_LOCK_H +#define RTPS_LOCK_H + +#if defined(ESP_PLATFORM) +#include "freertos/FreeRTOS.h" +#include "freertos/semphr.h" +#else +#include "FreeRTOS.h" +#include "semphr.h" +#endif +#include "lwip/sys.h" + +namespace rtps { + +class Lock { +public: + explicit Lock(SemaphoreHandle_t &mutex) : m_mutex(mutex) { + if (m_mutex != nullptr) { + m_locked = (xSemaphoreTakeRecursive(m_mutex, portMAX_DELAY) == pdTRUE); + } + }; + + ~Lock() { + if (m_locked && m_mutex != nullptr) { + xSemaphoreGiveRecursive(m_mutex); + } + }; + +private: + SemaphoreHandle_t m_mutex = nullptr; + bool m_locked = false; +}; + +bool createMutex(SemaphoreHandle_t *mutex); + +} // namespace rtps +#endif // RTPS_LOCK_H diff --git a/components/rtps_embedded/include/rtps/utils/Log.h b/components/rtps_embedded/include/rtps/utils/Log.h new file mode 100644 index 000000000..8c195c2cf --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/Log.h @@ -0,0 +1,47 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_LOG_H +#define RTPS_LOG_H + +#include +#include + +#define RTPS_GLOBAL_VERBOSE 1 + +#define SFW_VERBOSE 0 +#define SPDP_VERBOSE 1 +#define PBUF_WRAP_VERBOSE 0 +#define SEDP_VERBOSE 1 +#define RECV_VERBOSE 0 +#define PARTICIPANT_VERBOSE 0 +#define DOMAIN_VERBOSE 0 +#define UDP_DRIVER_VERBOSE 1 +#define TSCB_VERBOSE 0 +#define SLW_VERBOSE 1 +#define SFR_VERBOSE 0 +#define SLR_VERBOSE 1 +#define THREAD_POOL_VERBOSE 0 + +#endif // RTPS_LOG_H diff --git a/components/rtps_embedded/include/rtps/utils/constants.h b/components/rtps_embedded/include/rtps/utils/constants.h new file mode 100644 index 000000000..2f557a880 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/constants.h @@ -0,0 +1,28 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_CONSTANTS_H +#define RTPS_CONSTANTS_H + +#endif // RTPS_CONSTANTS_H diff --git a/components/rtps_embedded/include/rtps/utils/hash.h b/components/rtps_embedded/include/rtps/utils/hash.h new file mode 100644 index 000000000..3c868e527 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/hash.h @@ -0,0 +1,39 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_HASH_H +#define RTPS_HASH_H + +namespace rtps { +inline size_t hashCharArray(const char *p, size_t s) { + size_t result = 0; + const size_t prime = 31; + for (size_t i = 0; i < s; ++i) { + result = p[i] + (result * prime); + } + return result; +} +} // namespace rtps + +#endif diff --git a/components/rtps_embedded/include/rtps/utils/printutils.h b/components/rtps_embedded/include/rtps/utils/printutils.h new file mode 100644 index 000000000..4d4d1bfa7 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/printutils.h @@ -0,0 +1,29 @@ +// +// Created by andreas on 13.01.19. +// + +#ifndef RTPS_PRINTUTILS_H +#define RTPS_PRINTUTILS_H + +#include "rtps/common/types.h" + +inline void printEntityId(rtps::EntityId_t id) { + for (const auto byte : id.entityKey) { + printf("%x", (int)byte); + } + printf("%x", static_cast(id.entityKind)); +} + +inline void printGuidPrefix(rtps::GuidPrefix_t prefix) { + for (const auto byte : prefix.id) { + printf("%x", (int)byte); + } +} + +inline void printGuid(rtps::Guid_t guid) { + printGuidPrefix(guid.prefix); + printf(":"); + printEntityId(guid.entityId); +} + +#endif // RTPS_PRINTUTILS_H diff --git a/components/rtps_embedded/include/rtps/utils/sysFunctions.h b/components/rtps_embedded/include/rtps/utils/sysFunctions.h new file mode 100644 index 000000000..df6841262 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/sysFunctions.h @@ -0,0 +1,42 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef PROJECT_SYSFUNCTIONS_H +#define PROJECT_SYSFUNCTIONS_H + +#include "lwip/sys.h" +#include "rtps/common/types.h" + +namespace rtps { +inline Time_t getCurrentTimeStamp() { + Time_t now; + // TODO FIX + uint32_t nowMs = sys_now(); + now.seconds = (int32_t)nowMs / 1000; + now.fraction = ((nowMs % 1000) / 1000); + return now; +} +} // namespace rtps + +#endif // PROJECT_SYSFUNCTIONS_H diff --git a/components/rtps_embedded/include/rtps/utils/udpUtils.h b/components/rtps_embedded/include/rtps/utils/udpUtils.h new file mode 100644 index 000000000..5500e2025 --- /dev/null +++ b/components/rtps_embedded/include/rtps/utils/udpUtils.h @@ -0,0 +1,111 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_UDP_UTILS_H +#define RTPS_UDP_UTILS_H + +#include "rtps/config.h" + +namespace rtps { +namespace { +const uint16_t PB = 7400; // Port Base Number +const uint16_t DG = 250; // DomainId Gain +const uint16_t PG = 2; // ParticipantId Gain +// Additional Offsets +const uint16_t D0 = 0; // Builtin multicast +const uint16_t D1 = 10; // Builtin unicast +const uint16_t D2 = 1; // User multicast +const uint16_t D3 = 11; // User unicast +} // namespace + +constexpr ip4_addr transformIP4ToU32(uint8_t MSB, uint8_t p2, uint8_t p1, + uint8_t LSB) { + return ip4_addr{PP_HTONL(LWIP_MAKEU32(MSB, p2, p1, LSB))}; +} + +constexpr Ip4Port_t getBuiltInUnicastPort(ParticipantId_t participantId) { + return PB + DG * Config::DOMAIN_ID + D1 + PG * participantId; +} + +constexpr Ip4Port_t getBuiltInMulticastPort() { + return PB + DG * Config::DOMAIN_ID + D0; +} + +constexpr Ip4Port_t getUserUnicastPort(ParticipantId_t participantId) { + return PB + DG * Config::DOMAIN_ID + D3 + PG * participantId; +} + +constexpr Ip4Port_t getUserMulticastPort() { + return PB + DG * Config::DOMAIN_ID + D2; +} + +constexpr bool isUserPort(Ip4Port_t port) { + return (port & 1) == 1; +} // really useful? There may be other user ports than just odd ones? + +inline bool isMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return idWithoutBase == D0 || + (idWithoutBase >= D2 && + idWithoutBase < D3); // There are several UserMulticastPorts! +} + +inline bool isMetaMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return idWithoutBase == D0; +} + +inline bool isUserMultiCastPort(Ip4Port_t port) { + const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID; + return (idWithoutBase >= D2 && idWithoutBase < D1); +} + +inline bool isZeroAddress(ip4_addr_t address) { return address.addr == 0; } + +inline ParticipantId_t getParticipantIdFromUnicastPort(Ip4Port_t port, + bool isUserPort) { + + const auto basePart = PB + DG * Config::DOMAIN_ID; + ParticipantId_t participantPart = port - basePart; + + uint16_t offset = 0; + if (isUserPort) { + offset = D3; + } else { + offset = D1; + } + + participantPart -= offset; + + auto id = static_cast(participantPart / PG); + if (id * PG + basePart + offset == port) { + return id; + } else { + return PARTICIPANT_ID_INVALID; + } +} + +} // namespace rtps + +#endif // RTPS_UDP_H diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp new file mode 100644 index 000000000..aca048b9f --- /dev/null +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -0,0 +1,325 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/ThreadPool.h" + +#include "lwip/tcpip.h" +#include "rtps/entities/Domain.h" +#include "rtps/entities/Writer.h" +#include "rtps/utils/Diagnostics.h" +#include "rtps/utils/Log.h" +#include "rtps/utils/udpUtils.h" + +using rtps::ThreadPool; + +#if THREAD_POOL_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define THREAD_POOL_LOG(...) \ + if (true) { \ + printf("[ThreadPool] "); \ + printf(__VA_ARGS__); \ + printf("\r\n"); \ + } +#else +#define THREAD_POOL_LOG(...) // +#endif + +ThreadPool::ThreadPool(receiveJumppad_fp receiveCallback, void *callee) + : m_receiveJumppad(receiveCallback), m_callee(callee) { + + if (!m_outgoingMetaTraffic.init() || !m_outgoingUserTraffic.init() || + !m_incomingMetaTraffic.init() || !m_incomingUserTraffic.init()) { + return; + } + err_t inputErr = sys_sem_new(&m_readerNotificationSem, 0); + err_t outputErr = sys_sem_new(&m_writerNotificationSem, 0); + + if (inputErr != ERR_OK || outputErr != ERR_OK) { + THREAD_POOL_LOG("ThreadPool: Failed to create Semaphores.\n"); + } +} + +ThreadPool::~ThreadPool() { + if (m_running) { + stopThreads(); + sys_msleep(500); + } + + if (sys_sem_valid(&m_readerNotificationSem)) { + sys_sem_free(&m_readerNotificationSem); + } + if (sys_sem_valid(&m_writerNotificationSem)) { + sys_sem_free(&m_writerNotificationSem); + } +} + +void ThreadPool::updateDiagnostics() { + + rtps::Diagnostics::ThreadPool::max_ever_elements_incoming_usertraffic_queue = + std::max(rtps::Diagnostics::ThreadPool:: + max_ever_elements_incoming_usertraffic_queue, + m_incomingUserTraffic.numElements()); + + rtps::Diagnostics::ThreadPool::max_ever_elements_outgoing_usertraffic_queue = + std::max(rtps::Diagnostics::ThreadPool:: + max_ever_elements_outgoing_usertraffic_queue, + m_outgoingUserTraffic.numElements()); + + rtps::Diagnostics::ThreadPool::max_ever_elements_incoming_metatraffic_queue = + std::max(rtps::Diagnostics::ThreadPool:: + max_ever_elements_incoming_metatraffic_queue, + m_incomingMetaTraffic.numElements()); + + rtps::Diagnostics::ThreadPool::max_ever_elements_outgoing_metatraffic_queue = + std::max(rtps::Diagnostics::ThreadPool:: + max_ever_elements_outgoing_metatraffic_queue, + m_outgoingMetaTraffic.numElements()); +} + +bool ThreadPool::startThreads() { + if (m_running) { + return true; + } + if (!sys_sem_valid(&m_readerNotificationSem) || + !sys_sem_valid(&m_writerNotificationSem)) { + return false; + } + + m_running = true; + for (auto &thread : m_writers) { + // TODO ID, err check, waitOnStop + thread = sys_thread_new("WriterThread", writerThreadFunction, this, + Config::THREAD_POOL_WRITER_STACKSIZE, + Config::THREAD_POOL_WRITER_PRIO); + } + + for (auto &thread : m_readers) { + // TODO ID, err check, waitOnStop + thread = sys_thread_new("ReaderThread", readerThreadFunction, this, + Config::THREAD_POOL_READER_STACKSIZE, + Config::THREAD_POOL_READER_PRIO); + } + return true; +} + +void ThreadPool::stopThreads() { + m_running = false; + // This should call all the semaphores for each thread once, so they don't + // stuck before ended. + for (auto &thread : m_writers) { + (void)thread; + sys_sem_signal(&m_writerNotificationSem); + sys_msleep(10); + } + for (auto &thread : m_readers) { + (void)thread; + sys_sem_signal(&m_readerNotificationSem); + sys_msleep(10); + } + // TODO make sure they have finished. Seems to be sufficient for tests. + // Not sufficient if threads shall actually be stopped during runtime. + sys_msleep(10); +} + +void ThreadPool::clearQueues() { + m_outgoingMetaTraffic.clear(); + m_outgoingUserTraffic.clear(); + m_incomingMetaTraffic.clear(); + m_incomingUserTraffic.clear(); +} + +bool ThreadPool::addWorkload(Writer *workload) { + bool res = false; + if (workload->isBuiltinEndpoint()) { + res = m_outgoingMetaTraffic.moveElementIntoBuffer(std::move(workload)); + } else { + res = m_outgoingUserTraffic.moveElementIntoBuffer(std::move(workload)); + } + if (res) { + sys_sem_signal(&m_writerNotificationSem); + } else { + if(workload->isBuiltinEndpoint()){ + rtps::Diagnostics::ThreadPool::dropped_outgoing_packets_metatraffic++; + }else{ + rtps::Diagnostics::ThreadPool::dropped_outgoing_packets_usertraffic++; + } + THREAD_POOL_LOG("Failed to enqueue outgoing packet."); + } + + return res; +} + +bool ThreadPool::addBuiltinPort(const Ip4Port_t &port) { + if (m_builtinPortsIdx == m_builtinPorts.size()) { + return false; + } + + // TODO: Does not allow for participant deletion! + m_builtinPorts[m_builtinPortsIdx] = port; + m_builtinPortsIdx++; + + return true; +} + +bool ThreadPool::isBuiltinPort(const Ip4Port_t &port) { + if (getBuiltInMulticastLocator().port == port) { + return true; + } + + for (unsigned int i = 0; i < m_builtinPortsIdx; i++) { + if (m_builtinPorts[i] == port) { + return true; + } + } + + return false; +} + +bool ThreadPool::addNewPacket(PacketInfo &&packet) { + bool res = false; + if (isBuiltinPort(packet.destPort)) { + res = m_incomingMetaTraffic.moveElementIntoBuffer(std::move(packet)); + } else { + res = m_incomingUserTraffic.moveElementIntoBuffer(std::move(packet)); + } + if (res) { + sys_sem_signal(&m_readerNotificationSem); + } else { + THREAD_POOL_LOG("failed to enqueue packet for port %u", + static_cast(packet.destPort)); + } + return res; +} + +void ThreadPool::writerThreadFunction(void *arg) { + auto pool = static_cast(arg); + if (pool == nullptr) { + + THREAD_POOL_LOG("nullptr passed to writer function\n"); + + return; + } + + pool->doWriterWork(); +} + +void ThreadPool::doWriterWork() { + while (m_running) { + Writer *workload_usertraffic = nullptr; + bool workload_usertraffic_available = m_outgoingUserTraffic.moveFirstInto(workload_usertraffic); + if (workload_usertraffic_available) { + workload_usertraffic->progress(); + Diagnostics::ThreadPool::processed_outgoing_usertraffic++; + } + + Writer *workload_metatraffic = nullptr; + bool workload_metatraffic_available = m_outgoingMetaTraffic.moveFirstInto(workload_metatraffic); + if (workload_metatraffic_available) { + workload_metatraffic->progress(); + Diagnostics::ThreadPool::processed_outgoing_metatraffic++; + } + + if (workload_usertraffic_available || workload_metatraffic_available) { + continue; + } else { + THREAD_POOL_LOG("WriterWorker | User = %u, Meta = %u\r\n", + static_cast(Diagnostics::ThreadPool::processed_outgoing_usertraffic), + static_cast(Diagnostics::ThreadPool::processed_outgoing_metatraffic)); + updateDiagnostics(); + sys_sem_wait(&m_writerNotificationSem); + } + } +} + +void ThreadPool::readCallback(void *args, udp_pcb *target, pbuf *pbuf, + const ip_addr_t *addr, Ip4Port_t port) { + auto &pool = *static_cast(args); + + PacketInfo packet; + + // TODO This is a workaround for chained pbufs caused by hardware limitations, + // not a general fix + if (pbuf->next != nullptr) { + struct pbuf *test = pbuf_alloc(PBUF_RAW, pbuf->tot_len, PBUF_POOL); + pbuf_copy(test, pbuf); + pbuf_free(pbuf); + pbuf = test; + } + + packet.destAddr = {0}; // not relevant + packet.destPort = target->local_port; + packet.srcPort = port; + packet.buffer = PBufWrapper{pbuf}; + + if (!pool.addNewPacket(std::move(packet))) { + THREAD_POOL_LOG("ThreadPool: dropped packet\n"); + if (pool.isBuiltinPort(port)) { + rtps::Diagnostics::ThreadPool::dropped_incoming_packets_metatraffic++; + } else { + rtps::Diagnostics::ThreadPool::dropped_incoming_packets_usertraffic++; + } + } +} + +void ThreadPool::readerThreadFunction(void *arg) { + auto pool = static_cast(arg); + if (pool == nullptr) { + + THREAD_POOL_LOG("nullptr passed to reader function\n"); + + return; + } + pool->doReaderWork(); +} + +void ThreadPool::doReaderWork() { + uint32_t metatraffic = 0; + uint32_t usertraffic = 0; + while (m_running) { + PacketInfo packet_user; + auto isUserWorkToDo = m_incomingUserTraffic.moveFirstInto(packet_user); + if (isUserWorkToDo) { + Diagnostics::ThreadPool::processed_incoming_usertraffic++; + m_receiveJumppad(m_callee, const_cast(packet_user)); + } + + PacketInfo packet_meta; + auto isMetaWorkToDo = m_incomingMetaTraffic.moveFirstInto(packet_meta); + if (isMetaWorkToDo) { + Diagnostics::ThreadPool::processed_incoming_metatraffic++; + m_receiveJumppad(m_callee, const_cast(packet_meta)); + } + + if (isUserWorkToDo || isMetaWorkToDo) { + continue; + } + THREAD_POOL_LOG("ReaderWorker | User = %u, Meta = %u\r\n", + static_cast(usertraffic), + static_cast(metatraffic)); + updateDiagnostics(); + sys_sem_wait(&m_readerNotificationSem); + } +} + +#undef THREAD_POOL_VERBOSE diff --git a/components/rtps_embedded/src/communication/UdpDriver.cpp b/components/rtps_embedded/src/communication/UdpDriver.cpp new file mode 100644 index 000000000..3b14da023 --- /dev/null +++ b/components/rtps_embedded/src/communication/UdpDriver.cpp @@ -0,0 +1,153 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/communication/UdpDriver.h" +#include "rtps/communication/TcpipCoreLock.h" +#include "rtps/utils/Lock.h" +#include "rtps/utils/Log.h" + +#include +#include + +using rtps::UdpDriver; + +#if UDP_DRIVER_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define UDP_DRIVER_LOG(...) \ + if (true) { \ + printf("[UDP Driver] "); \ + printf(__VA_ARGS__); \ + printf("\r\n"); \ + } +#else +#define UDP_DRIVER_LOG(...) // +#endif + +UdpDriver::UdpDriver(rtps::UdpDriver::udpRxFunc_fp callback, void *args) + : m_rxCallback(callback), m_callbackArgs(args) {} + +const rtps::UdpConnection * +UdpDriver::createUdpConnection(Ip4Port_t receivePort) { + for (uint8_t i = 0; i < m_numConns; ++i) { + if (m_conns[i].port == receivePort) { + return &m_conns[i]; + } + } + + if (m_numConns == m_conns.size()) { + return nullptr; + } + + UdpConnection udp_conn(receivePort); + + { + TcpipCoreLock lock; + err_t err = udp_bind(udp_conn.pcb, IP_ADDR_ANY, + receivePort); // to receive multicast + + if (err != ERR_OK && err != ERR_USE) { + return nullptr; + } + + udp_recv(udp_conn.pcb, m_rxCallback, m_callbackArgs); + } + + m_conns[m_numConns] = std::move(udp_conn); + m_numConns++; + + UDP_DRIVER_LOG("Successfully created UDP connection on port %u \n", + receivePort); + + return &m_conns[m_numConns - 1]; +} + +bool UdpDriver::isSameSubnet(ip4_addr_t addr) { + return (ip4_addr_netcmp(&addr, netif_ip4_addr(netif_default), + netif_ip4_netmask(netif_default)) != 0); +} + +bool UdpDriver::isMulticastAddress(ip4_addr_t addr) { +#if IS_LITTLE_ENDIAN + return (addr.addr & 0xF0) == 0xE0; +#else + return (addr.addr >> 28) == 14; +#endif +} + +bool UdpDriver::joinMultiCastGroup(ip4_addr_t addr) const { + err_t iret; + + { + TcpipCoreLock lock; + iret = igmp_joingroup(ip_2_ip4(IP_ADDR_ANY), (&addr)); + } + + if (iret != ERR_OK) { + + UDP_DRIVER_LOG("Failed to join IGMP multicast group %s\n", + ip4addr_ntoa(&addr)); + + return false; + } else { + + UDP_DRIVER_LOG("Succesfully joined IGMP multicast group %s\n", + ip4addr_ntoa(&addr)); + } + return true; +} + +bool UdpDriver::sendPacket(const UdpConnection &conn, ip4_addr_t &destAddr, + Ip4Port_t destPort, pbuf &buffer) { + err_t err; + { + TcpipCoreLock lock; + ip_addr_t dest; + ip_addr_copy_from_ip4(dest, destAddr); + err = udp_sendto(conn.pcb, &buffer, &dest, destPort); + } + + if (err != ERR_OK) { + ; + + UDP_DRIVER_LOG("UDP TRANSMIT NOT SUCCESSFUL %s:%u size: %u err: %i\n", + ip4addr_ntoa(&destAddr), destPort, buffer.tot_len, err); + + return false; + } + return true; +} + +void UdpDriver::sendPacket(PacketInfo &packet) { + auto p_conn = createUdpConnection(packet.srcPort); + if (p_conn == nullptr) { + ; + + UDP_DRIVER_LOG("Failed to create connection on port %u \n", packet.srcPort); + + return; + } + + sendPacket(*p_conn, packet.destAddr, packet.destPort, + *packet.buffer.firstElement); +} diff --git a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp new file mode 100644 index 000000000..7a95938c5 --- /dev/null +++ b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp @@ -0,0 +1,216 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/SPDPAgent.h" +#include "rtps/entities/Participant.h" +#include "rtps/utils/Log.h" + +using rtps::ParticipantProxyData; + +ParticipantProxyData::ParticipantProxyData(Guid_t guid) : m_guid(guid) {} + +void ParticipantProxyData::reset() { + m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; + m_manualLivelinessCount = Count_t{1}; + m_expectsInlineQos = false; + onAliveSignal(); + for (int i = 0; i < Config::SPDP_MAX_NUM_LOCATORS; ++i) { + m_metatrafficUnicastLocatorList[i].setInvalid(); + m_metatrafficMulticastLocatorList[i].setInvalid(); + m_defaultUnicastLocatorList[i].setInvalid(); + m_defaultMulticastLocatorList[i].setInvalid(); + } +} + +bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, + Participant *participant) { + reset(); + SMElement::ParameterId pid; + uint16_t length; + SPDP_LOG("Start deserializing ParticipantProxyData\n"); + SPDP_LOG("Buffer has %u bytes remaining\n", ucdr_buffer_remaining(&buffer)); + while (ucdr_buffer_remaining(&buffer) >= 4) { + ucdr_deserialize_uint16_t(&buffer, reinterpret_cast(&pid)); + + ucdr_deserialize_uint16_t(&buffer, &length); + SPDP_LOG("Deserializing parameter with id %u and length %u\n", + static_cast(pid), length); + if (ucdr_buffer_remaining(&buffer) < length) { + SPDP_LOG("Not enough data left in buffer to read parameter with id %u and length %u\n", + static_cast(pid), length); + return false; + } + + switch (pid) { + case ParameterId::PID_KEY_HASH: { + // TODO + break; + } + + case ParameterId::PID_PROTOCOL_VERSION: { + ucdr_deserialize_uint8_t(&buffer, &m_protocolVersion.major); + if (m_protocolVersion.major < PROTOCOLVERSION.major) { + SPDP_LOG("Unsupported protocol version: %u.%u\n", m_protocolVersion.major, + m_protocolVersion.minor); + return false; + } else { + ucdr_deserialize_uint8_t(&buffer, &m_protocolVersion.minor); + } + break; + } + case ParameterId::PID_VENDORID: { + ucdr_deserialize_array_uint8_t(&buffer, m_vendorId.vendorId.data(), + m_vendorId.vendorId.size()); + break; + } + + case ParameterId::PID_EXPECTS_INLINE_QOS: { + ucdr_deserialize_bool(&buffer, &m_expectsInlineQos); + break; + } + case ParameterId::PID_PARTICIPANT_GUID: { + ucdr_deserialize_array_uint8_t(&buffer, m_guid.prefix.id.data(), + m_guid.prefix.id.size()); + ucdr_deserialize_array_uint8_t(&buffer, m_guid.entityId.entityKey.data(), + m_guid.entityId.entityKey.size()); + ucdr_deserialize_uint8_t( + &buffer, reinterpret_cast(&m_guid.entityId.entityKind)); + if (participant->findRemoteParticipant(m_guid.prefix)) { + SPDP_LOG("stopping deserialization early, participant is known\n"); + return true; + } + break; + } + case ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_metatrafficMulticastLocatorList)) { + SPDP_LOG("Failed to read metatraffic multicast locator\n"); + return false; + } + break; + } + case ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_metatrafficUnicastLocatorList)) { + SPDP_LOG("Failed to read metatraffic unicast locator\n"); + return false; + } + break; + } + case ParameterId::PID_DEFAULT_UNICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_defaultUnicastLocatorList)) { + SPDP_LOG("Failed to read default unicast locator\n"); + return false; + } + break; + } + case ParameterId::PID_DEFAULT_MULTICAST_LOCATOR: { + if (!readLocatorIntoList(buffer, m_defaultMulticastLocatorList)) { + SPDP_LOG("Failed to read default multicast locator\n"); + return false; + } + break; + } + case ParameterId::PID_PARTICIPANT_LEASE_DURATION: { + ucdr_deserialize_int32_t(&buffer, &m_leaseDuration.seconds); + ucdr_deserialize_uint32_t(&buffer, &m_leaseDuration.fraction); + break; + } + case ParameterId::PID_BUILTIN_ENDPOINT_SET: { + ucdr_deserialize_uint32_t(&buffer, &m_availableBuiltInEndpoints); + break; + } + case ParameterId::PID_ENTITY_NAME: { + // TODO + buffer.iterator += length; + buffer.last_data_size = 1; + break; + } + case ParameterId::PID_PROPERTY_LIST: { + // TODO + buffer.iterator += length; + buffer.last_data_size = 1; + break; + } + case ParameterId::PID_USER_DATA: { + // TODO + buffer.iterator += length; + buffer.last_data_size = 1; + break; + } + case ParameterId::PID_PAD: { + buffer.iterator += length; + buffer.last_data_size = 1; + break; + } + case ParameterId::PID_SENTINEL: { + return true; + } + default: { + // SPDP_LOG("unknow ID. could be vender defined.\n"); + // should not return false for unknown parameter, just skip it, otherwise we might miss some important information if the remote participant is using some vendor specific parameters that we do not know about. + // TODO: GUO: need read out the data for the length of the parameter, otherwise the buffer will be in wrong state and the following parameters cannot be read correctly. For now just skip the data by moving the iterator forward, but we might want to actually read out the data and store it for future use if needed, especially for some vendor specific parameters that we do not know about. + buffer.iterator += length; + break; } + } + // Parameter lists are 4-byte aligned + uint32_t alignment = ucdr_buffer_alignment(&buffer, 4); + buffer.iterator += alignment; + buffer.last_data_size = 4; + } + return true; +} + +bool ParticipantProxyData::readLocatorIntoList( + ucdrBuffer &buffer, + std::array &list) { + int valid_locators = 0; + FullLengthLocator full_length_locator; + for (auto &proxy_locator : list) { + if (!proxy_locator.isValid()) { + bool ret = full_length_locator.readFromUcdrBuffer(buffer); + if (ret && full_length_locator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + proxy_locator = LocatorIPv4(full_length_locator); + SPDP_LOG("Adding locator: %u %u %u %u", + (int)proxy_locator.address[0], (int)proxy_locator.address[1], + (int)proxy_locator.address[2], (int)proxy_locator.address[3]); + return true; + } else { + SPDP_LOG("Ignoring locator: %u %u %u %u", + (int)full_length_locator.address[12], + (int)full_length_locator.address[13], + (int)full_length_locator.address[14], + (int)full_length_locator.address[15]); + return true; + } + } else { + valid_locators++; + if (valid_locators == Config::SPDP_MAX_NUM_LOCATORS) { + buffer.iterator += sizeof(FullLengthLocator); + SPDP_LOG("Max number of valid locators exceed, ignoring this locator " + "as we have at least one valid locator\n"); + return true; + } + } + } + return false; +} diff --git a/components/rtps_embedded/src/discovery/SEDPAgent.cpp b/components/rtps_embedded/src/discovery/SEDPAgent.cpp new file mode 100644 index 000000000..000da1012 --- /dev/null +++ b/components/rtps_embedded/src/discovery/SEDPAgent.cpp @@ -0,0 +1,516 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/SEDPAgent.h" +#include "rtps/discovery/TopicData.h" +#include "rtps/entities/Participant.h" +#include "rtps/entities/Reader.h" +#include "rtps/entities/Writer.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/utils/Log.h" +#include "ucdr/microcdr.h" + +using rtps::SEDPAgent; + +#if SEDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#define SEDP_LOG(...) \ + if (true) { \ + printf("[SEDP] "); \ + printf(__VA_ARGS__); \ + printf("\r\n"); \ + } +#else +#define SEDP_LOG(...) // +#endif + +void SEDPAgent::init(Participant &part, const BuiltInEndpoints &endpoints) { + // TODO move + if (!createMutex(&m_mutex)) { + SEDP_LOG("SEDPAgent failed to create mutex\n"); + return; + } + + m_part = ∂ + m_endpoints = endpoints; + if (m_endpoints.sedpPubReader != nullptr) { + m_endpoints.sedpPubReader->registerCallback(jumppadPublisherReader, this); + } + if (m_endpoints.sedpSubReader != nullptr) { + m_endpoints.sedpSubReader->registerCallback(jumppadSubscriptionReader, + this); + } +} + +void SEDPAgent::registerOnNewPublisherMatchedCallback( + void (*callback)(void *arg), void *args) { + mfp_onNewPublisherCallback = callback; + m_onNewPublisherArgs = args; +} + +void SEDPAgent::registerOnNewSubscriberMatchedCallback( + void (*callback)(void *arg), void *args) { + mfp_onNewSubscriberCallback = callback; + m_onNewSubscriberArgs = args; +} + +void SEDPAgent::jumppadPublisherReader(void *callee, + const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->handlePublisherReaderMessage(cacheChange); +} + +void SEDPAgent::jumppadSubscriptionReader( + void *callee, const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->handleSubscriptionReaderMessage(cacheChange); +} + +void SEDPAgent::handlePublisherReaderMessage(const ReaderCacheChange &change) { + Lock lock{m_mutex}; +#if SEDP_VERBOSE + SEDP_LOG("New publisher\n"); +#endif + + if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { +#if SEDP_VERBOSE + SEDP_LOG("EDPAgent: Buffer too small.\n"); +#endif + return; + } + ucdrBuffer cdrBuffer; + ucdr_init_buffer(&cdrBuffer, m_buffer, sizeof(m_buffer)); + + TopicData topicData; + if (topicData.readFromUcdrBuffer(cdrBuffer)) { + handlePublisherReaderMessage(topicData, change); + } +} + +void SEDPAgent::addUnmatchedRemoteWriter(const TopicData &writerData) { + addUnmatchedRemoteWriter(TopicDataCompressed(writerData)); +} + +void SEDPAgent::addUnmatchedRemoteReader(const TopicData &readerData) { + addUnmatchedRemoteReader(TopicDataCompressed(readerData)); +} + +void SEDPAgent::addUnmatchedRemoteWriter( + const TopicDataCompressed &writerData) { + if (m_unmatchedRemoteWriters.isFull()) { +#if SEDP_VERBOSE + SEDP_LOG("List of unmatched remote writers is full.\n"); +#endif + return; + } + SEDP_LOG("Adding unmatched remote writer %zx %zx.\n", writerData.topicHash, + writerData.typeHash); + m_unmatchedRemoteWriters.add(writerData); +} + +void SEDPAgent::addUnmatchedRemoteReader( + const TopicDataCompressed &readerData) { + if (m_unmatchedRemoteReaders.isFull()) { +#if SEDP_VERBOSE + SEDP_LOG("List of unmatched remote readers is full.\n"); +#endif + return; + } + SEDP_LOG("Adding unmatched remote reader %zx %zx.\n", readerData.topicHash, + readerData.typeHash); + m_unmatchedRemoteReaders.add(readerData); +} + +void SEDPAgent::removeUnmatchedEntity(const Guid_t &guid) { + auto isElementToRemove = [&](const TopicDataCompressed &topicData) { + return topicData.endpointGuid == guid; + }; + + auto thunk = [](void *arg, const TopicDataCompressed &value) { + return (*static_cast(arg))(value); + }; + + m_unmatchedRemoteReaders.remove(thunk, &isElementToRemove); + m_unmatchedRemoteWriters.remove(thunk, &isElementToRemove); +} + +void SEDPAgent::removeUnmatchedEntitiesOfParticipant( + const GuidPrefix_t &guidPrefix) { + Lock lock{m_mutex}; + auto isElementToRemove = [&](const TopicDataCompressed &topicData) { + return topicData.endpointGuid.prefix == guidPrefix; + }; + + auto thunk = [](void *arg, const TopicDataCompressed &value) { + return (*static_cast(arg))(value); + }; + + m_unmatchedRemoteReaders.remove(thunk, &isElementToRemove); + m_unmatchedRemoteWriters.remove(thunk, &isElementToRemove); +} + +uint32_t SEDPAgent::getNumRemoteUnmatchedReaders() { + return m_unmatchedRemoteReaders.getNumElements(); +} + +uint32_t SEDPAgent::getNumRemoteUnmatchedWriters() { + return m_unmatchedRemoteWriters.getNumElements(); +} + +void SEDPAgent::handlePublisherReaderMessage(const TopicData &writerData, + const ReaderCacheChange &change) { + // TODO Is it okay to add Endpoint if the respective participant is unknown + // participant? + if (!m_part->findRemoteParticipant(writerData.endpointGuid.prefix)) { + return; + } + + if (writerData.isDisposedFlagSet() || writerData.isUnregisteredFlagSet()) { + handleRemoteEndpointDeletion(writerData, change); + return; + } + +#if SEDP_VERBOSE + SEDP_LOG("PUB T/D %s/%s", writerData.topicName, writerData.typeName); +#endif + Reader *reader = m_part->getMatchingReader(writerData); + if (reader == nullptr) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Couldn't find reader for new Publisher[%s, %s] \n", + writerData.topicName, writerData.typeName); +#endif + addUnmatchedRemoteWriter(writerData); + return; + } + // TODO check policies +#if SEDP_VERBOSE + SEDP_LOG("Found a new "); + if (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { + SEDP_LOG("reliable "); + } else { + SEDP_LOG("best-effort "); + } + SEDP_LOG("publisher\n"); +#endif + reader->addNewMatchedWriter( + WriterProxy{writerData.endpointGuid, writerData.unicastLocator, + (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + if (mfp_onNewPublisherCallback != nullptr) { + mfp_onNewPublisherCallback(m_onNewPublisherArgs); + } +} + +void SEDPAgent::handleSubscriptionReaderMessage( + const ReaderCacheChange &change) { + Lock lock{m_mutex}; +#if SEDP_VERBOSE + SEDP_LOG("New subscriber\n"); +#endif + + if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Buffer too small."); +#endif + return; + } + ucdrBuffer cdrBuffer; + ucdr_init_buffer(&cdrBuffer, m_buffer, sizeof(m_buffer)); + + TopicData topicData; + if (topicData.readFromUcdrBuffer(cdrBuffer)) { + handleSubscriptionReaderMessage(topicData, change); + } +} + +void SEDPAgent::handleRemoteEndpointDeletion(const TopicData &topic, + const ReaderCacheChange &change) { + SEDP_LOG("Endpoint deletion message SN %u.%u GUID %u %u %u %u \r\n", + (int)change.sn.high, (int)change.sn.low, + change.writerGuid.prefix.id[0], change.writerGuid.prefix.id[1], + change.writerGuid.prefix.id[2], change.writerGuid.prefix.id[3]); + if (!topic.entityIdFromKeyHashValid) { + return; + } + + Guid_t guid; + guid.prefix = topic.endpointGuid.prefix; + guid.entityId = topic.entityIdFromKeyHash; + + // Remove entity ID from all proxies of local endpoints + m_part->removeProxyFromAllEndpoints(guid); + + // Remove entity ID from unmatched endpoints + removeUnmatchedEntity(guid); +} + +void SEDPAgent::handleSubscriptionReaderMessage( + const TopicData &readerData, const ReaderCacheChange &change) { + if (!m_part->findRemoteParticipant(readerData.endpointGuid.prefix)) { + return; + } + + if (readerData.isDisposedFlagSet() || readerData.isUnregisteredFlagSet()) { + handleRemoteEndpointDeletion(readerData, change); + return; + } + + Writer *writer = m_part->getMatchingWriter(readerData); +#if SEDP_VERBOSE + SEDP_LOG("SUB T/D %s/%s", readerData.topicName, readerData.typeName); +#endif + if (writer == nullptr) { +#if SEDP_VERBOSE + SEDP_LOG("SEDPAgent: Couldn't find writer for new subscriber[%s, %s]\n", + readerData.topicName, readerData.typeName); +#endif + addUnmatchedRemoteReader(readerData); + return; + } + + // TODO check policies +#if SEDP_VERBOSE + SEDP_LOG("Found a new "); + if (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { + SEDP_LOG("reliable "); + } else { + SEDP_LOG("best-effort "); + } + SEDP_LOG("Subscriber\n"); +#endif + if (readerData.multicastLocator.kind == + rtps::LocatorKind_t::LOCATOR_KIND_UDPv4) { + writer->addNewMatchedReader(ReaderProxy{ + readerData.endpointGuid, readerData.unicastLocator, + readerData.multicastLocator, + (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + } else { + writer->addNewMatchedReader(ReaderProxy{ + readerData.endpointGuid, readerData.unicastLocator, + (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE)}); + } + + if (mfp_onNewSubscriberCallback != nullptr) { + mfp_onNewSubscriberCallback(m_onNewSubscriberArgs); + } +} + +void SEDPAgent::tryMatchUnmatchedEndpoints() { + // Try to match remote readers with local writers + for (auto &proxy : m_unmatchedRemoteReaders) { + auto writer = m_part->getMatchingWriter(proxy); + if (writer != nullptr) { + writer->addNewMatchedReader( + ReaderProxy{proxy.endpointGuid, proxy.unicastLocator, + proxy.multicastLocator, proxy.is_reliable}); + removeUnmatchedEntity(proxy.endpointGuid); + } + } + + // Try to match remote writers with local readers + for (auto &proxy : m_unmatchedRemoteWriters) { + auto reader = m_part->getMatchingReader(proxy); + if (reader != nullptr) { + reader->addNewMatchedWriter(WriterProxy{ + proxy.endpointGuid, proxy.unicastLocator, proxy.is_reliable}); + removeUnmatchedEntity(proxy.endpointGuid); + } + } +} + +bool SEDPAgent::addWriter(Writer &writer) { + if (m_endpoints.sedpPubWriter == nullptr) { + return true; + } + EntityKind_t writerKind = + writer.m_attributes.endpointGuid.entityId.entityKind; + if (writerKind == EntityKind_t::BUILD_IN_WRITER_WITH_KEY || + writerKind == EntityKind_t::BUILD_IN_WRITER_WITHOUT_KEY) { + return true; // No need to announce builtin endpoints + } + + Lock lock{m_mutex}; + + // Check unmatched writers for this new reader + tryMatchUnmatchedEndpoints(); + + ucdrBuffer microbuffer; + ucdr_init_buffer(µbuffer, m_buffer, + sizeof(m_buffer) / sizeof(m_buffer[0])); + const uint16_t zero_options = 0; + + ucdr_serialize_array_uint8_t(µbuffer, + rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + ucdr_serialize_uint16_t(µbuffer, zero_options); + writer.m_attributes.serializeIntoUcdrBuffer(microbuffer); + auto change = m_endpoints.sedpPubWriter->newChange( + ChangeKind_t::ALIVE, m_buffer, ucdr_buffer_length(µbuffer)); + writer.setSEDPSequenceNumber(change->sequenceNumber); + return (change != nullptr); +#if SEDP_VERBOSE + SEDP_LOG("Added new change to sedpPubWriter.\n"); +#endif +} + +template +bool SEDPAgent::disposeEndpointInSEDPHistory(A *local_endpoint, + Writer *sedp_writer) { + return sedp_writer->removeFromHistory( + local_endpoint->getSEDPSequenceNumber()); +} + +template +bool SEDPAgent::announceEndpointDeletion(A *local_endpoint, + Writer *sedp_endpoint) { + ucdrBuffer microbuffer; + ucdr_init_buffer(µbuffer, m_buffer, + sizeof(m_buffer) / sizeof(m_buffer[0])); + + ucdr_serialize_uint16_t(µbuffer, ParameterId::PID_KEY_HASH); + ucdr_serialize_uint16_t(µbuffer, 16); + ucdr_serialize_array_uint8_t( + µbuffer, local_endpoint->m_attributes.endpointGuid.prefix.id.data(), + sizeof(GuidPrefix_t::id)); + ucdr_serialize_array_uint8_t(µbuffer, local_endpoint->m_attributes.endpointGuid.entityId.entityKey.data(), 3); + ucdr_serialize_uint8_t(µbuffer, static_cast(local_endpoint->m_attributes.endpointGuid.entityId.entityKind)); + + ucdr_serialize_uint16_t(µbuffer, ParameterId::PID_STATUS_INFO); + ucdr_serialize_uint16_t(µbuffer, static_cast(4)); + ucdr_serialize_uint8_t(µbuffer, 0); + ucdr_serialize_uint8_t(µbuffer, 0); + ucdr_serialize_uint8_t(µbuffer, 0); + ucdr_serialize_uint8_t(µbuffer, 3); + + // Sentinel to terminate inline qos + ucdr_serialize_uint16_t(µbuffer, ParameterId::PID_SENTINEL); + ucdr_serialize_uint16_t(µbuffer, 0); + + // Sentinel to terminate serialized data + ucdr_serialize_uint16_t(µbuffer, ParameterId::PID_SENTINEL); + ucdr_serialize_uint16_t(µbuffer, 0); + + auto ret = + sedp_endpoint->newChange(ChangeKind_t::ALIVE, m_buffer, + ucdr_buffer_length(µbuffer), true, true); + SEDP_LOG("Annoucing endpoint delete, SN = %u.%u\r\n", + (int)ret->sequenceNumber.low, (int)ret->sequenceNumber.high); + return (ret != nullptr); +} + +void SEDPAgent::jumppadTakeProxyOfDisposedReader(const Reader *reader, + const WriterProxy &proxy, + void *arg) { + auto agent = static_cast(arg); + TopicDataCompressed topic_data(reader->m_attributes); + topic_data.endpointGuid = proxy.remoteWriterGuid; + topic_data.is_reliable = proxy.is_reliable; + topic_data.multicastLocator.kind = LocatorKind_t::LOCATOR_KIND_INVALID; + topic_data.unicastLocator = proxy.remoteLocator; + agent->addUnmatchedRemoteWriter(topic_data); +} + +void SEDPAgent::jumppadTakeProxyOfDisposedWriter(const Writer *writer, + const ReaderProxy &proxy, + void *arg) { + auto agent = static_cast(arg); + TopicDataCompressed topic_data(writer->m_attributes); + topic_data.endpointGuid = proxy.remoteReaderGuid; + topic_data.is_reliable = proxy.is_reliable; + topic_data.multicastLocator.kind = LocatorKind_t::LOCATOR_KIND_INVALID; + topic_data.unicastLocator = proxy.remoteLocator; + agent->addUnmatchedRemoteReader(topic_data); +} + +bool SEDPAgent::deleteReader(Reader *reader) { + Lock lock{m_mutex}; + // Set cache change kind in SEDP endpoint to DISPOSED + if (!disposeEndpointInSEDPHistory(reader, m_endpoints.sedpSubWriter)) { + return false; + } + + // Create Deletion Message [UD] and add to corret builtin endpoint + if (!announceEndpointDeletion(reader, m_endpoints.sedpSubWriter)) { + return false; + } + + // Move all matched proxies of this endpoint to the list of unmatched + // endpoints + reader->dumpAllProxies(SEDPAgent::jumppadTakeProxyOfDisposedReader, this); + + return true; +} + +bool SEDPAgent::deleteWriter(Writer *writer) { + Lock lock{m_mutex}; + // Set cache change kind in SEDP endpoint to DISPOSED + if (!disposeEndpointInSEDPHistory(writer, m_endpoints.sedpPubWriter)) { + return false; + } + + // Create Deletion Mesasge [UD] and add to corret builtin endpoint + if (!announceEndpointDeletion(writer, m_endpoints.sedpPubWriter)) { + return false; + } + + // Move all matched proxies of this endpoint to the list of unmatched + // endpoints + writer->dumpAllProxies(SEDPAgent::jumppadTakeProxyOfDisposedWriter, this); + + return true; +} + +bool SEDPAgent::addReader(Reader &reader) { + if (m_endpoints.sedpSubWriter == nullptr) { + return true; + } + + EntityKind_t readerKind = + reader.m_attributes.endpointGuid.entityId.entityKind; + if (readerKind == EntityKind_t::BUILD_IN_READER_WITH_KEY || + readerKind == EntityKind_t::BUILD_IN_READER_WITHOUT_KEY) { + return true; // No need to announce builtin endpoints + } + + Lock lock{m_mutex}; + + // Check unmatched writers for this new reader + tryMatchUnmatchedEndpoints(); + + ucdrBuffer microbuffer; + ucdr_init_buffer(µbuffer, m_buffer, + sizeof(m_buffer) / sizeof(m_buffer[0])); + const uint16_t zero_options = 0; + + ucdr_serialize_array_uint8_t(µbuffer, + rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + ucdr_serialize_uint16_t(µbuffer, zero_options); + reader.m_attributes.serializeIntoUcdrBuffer(microbuffer); + auto change = m_endpoints.sedpSubWriter->newChange( + ChangeKind_t::ALIVE, m_buffer, ucdr_buffer_length(µbuffer)); + reader.setSEDPSequenceNumber(change->sequenceNumber); + return (change != nullptr); +#if SEDP_VERBOSE + SEDP_LOG("Added new change to sedpSubWriter.\n"); +#endif +} diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp new file mode 100644 index 000000000..9f287065a --- /dev/null +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -0,0 +1,372 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/discovery/SPDPAgent.h" +#include "lwip/sys.h" +#include "rtps/discovery/ParticipantProxyData.h" +#include "rtps/entities/Participant.h" +#include "rtps/entities/Reader.h" +#include "rtps/entities/Writer.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/utils/Log.h" +#include "rtps/utils/udpUtils.h" + +using rtps::SPDPAgent; +using rtps::SMElement::BuildInEndpointSet; +using rtps::SMElement::ParameterId; + +void SPDPAgent::init(Participant &participant, BuiltInEndpoints &endpoints) { + if (!createMutex(&m_mutex)) { + SPDP_LOG("Could not alloc mutex"); + return; + } + mp_participant = &participant; + m_buildInEndpoints = endpoints; + m_buildInEndpoints.spdpReader->registerCallback(receiveCallback, this); + + ucdr_init_buffer(&m_microbuffer, m_outputBuffer.data(), + m_outputBuffer.size()); + // addInlineQos(); + addParticipantParameters(); + initialized = true; +} + +void SPDPAgent::start() { + if (m_running) { + return; + } + m_running = true; + auto t = + sys_thread_new("SPDPThread", runBroadcast, this, + Config::SPDP_WRITER_STACKSIZE, Config::SPDP_WRITER_PRIO); +} + +void SPDPAgent::stop() { m_running = false; } + +void SPDPAgent::runBroadcast(void *args) { + SPDPAgent &agent = *static_cast(args); + const DataSize_t size = ucdr_buffer_length(&agent.m_microbuffer); + const uint8_t *payload = agent.m_microbuffer.init; + agent.m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, payload, + size); + while (agent.m_running) { +#ifdef OS_IS_FREERTOS + vTaskDelay(pdMS_TO_TICKS(Config::SPDP_RESEND_PERIOD_MS)); +#else + sys_msleep(Config::SPDP_RESEND_PERIOD_MS); +#endif + // StatelessWriter drops already-sent history; enqueue a fresh SPDP sample + // for each announce cycle. + agent.m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, + payload, size); + if (agent.m_cycleHB == Config::SPDP_CYCLECOUNT_HEARTBEAT) { + agent.m_cycleHB = 0; + agent.mp_participant->checkAndResetHeartbeats(); + } else { + agent.m_cycleHB++; + } + } +} + +void SPDPAgent::receiveCallback(void *callee, + const ReaderCacheChange &cacheChange) { + auto agent = static_cast(callee); + agent->handleSPDPPackage(cacheChange); +} + +void SPDPAgent::handleSPDPPackage(const ReaderCacheChange &cacheChange) { + if (!initialized) { + SPDP_LOG("Callback called without initialization\n"); + return; + } + + Lock lock{m_mutex}; + if (cacheChange.size > m_inputBuffer.size()) { + SPDP_LOG("Input buffer to small\n"); + return; + } + + // Something went wrong deserializing remote participant + if (!cacheChange.copyInto(m_inputBuffer.data(), m_inputBuffer.size())) { + return; + } + + ucdrBuffer buffer; + ucdr_init_buffer(&buffer, m_inputBuffer.data(), m_inputBuffer.size()); + + if (cacheChange.kind == ChangeKind_t::ALIVE) { + configureEndianessAndOptions(buffer); + volatile bool success = + m_proxyDataBuffer.readFromUcdrBuffer(buffer, mp_participant); + if (success) { + // TODO In case we store the history we can free the history mutex here + processProxyData(); + } else { + SPDP_LOG("ParticipantProxyData deserializtaion failed\n"); + } + } else { + // TODO RemoveParticipant + } +} + +void SPDPAgent::configureEndianessAndOptions(ucdrBuffer &buffer) { + std::array encapsulation{}; + // Endianess doesn't matter for this since those are single bytes + ucdr_deserialize_array_uint8_t(&buffer, encapsulation.data(), + encapsulation.size()); + if (encapsulation == SMElement::SCHEME_PL_CDR_LE) { + buffer.endianness = UCDR_LITTLE_ENDIANNESS; + } else { + buffer.endianness = UCDR_BIG_ENDIANNESS; + } + // Reuse encapsulation buffer to skip options + ucdr_deserialize_array_uint8_t(&buffer, encapsulation.data(), + encapsulation.size()); +} + +void SPDPAgent::processProxyData() { + if (m_proxyDataBuffer.m_guid.prefix.id == mp_participant->m_guidPrefix.id) { + return; // Our own packet + } + + SPDP_LOG("Message from GUID = %u %u %u %u", m_proxyDataBuffer.m_guid.prefix.id[4], m_proxyDataBuffer.m_guid.prefix.id[5], m_proxyDataBuffer.m_guid.prefix.id[6], m_proxyDataBuffer.m_guid.prefix.id[7]); + const rtps::ParticipantProxyData *remote_part; + remote_part = + mp_participant->findRemoteParticipant(m_proxyDataBuffer.m_guid.prefix); + if (remote_part != nullptr) { + SPDP_LOG("Not adding this participant"); + mp_participant->refreshRemoteParticipantLiveliness( + m_proxyDataBuffer.m_guid.prefix); + return; // Already in our list + } + + if (mp_participant->addNewRemoteParticipant(m_proxyDataBuffer)) { + addProxiesForBuiltInEndpoints(); + const DataSize_t size = ucdr_buffer_length(&m_microbuffer); + m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, + m_microbuffer.init, size); +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE + SPDP_LOG("Added new participant with guid: "); + printGuidPrefix(m_proxyDataBuffer.m_guid.prefix); + } else { + SPDP_LOG("Failed to add new participant"); + } +#else + } else { + while (1) { + SPDP_LOG("failed to add remote participant"); + } + } +#endif +} + +bool SPDPAgent::addProxiesForBuiltInEndpoints() { + + LocatorIPv4 *locator = nullptr; + + // Check if the remote participants has a locator in our subnet + for (unsigned int i = 0; + i < m_proxyDataBuffer.m_metatrafficUnicastLocatorList.size(); i++) { + LocatorIPv4 *l = &(m_proxyDataBuffer.m_metatrafficUnicastLocatorList[i]); + if (l->isValid() && l->isSameSubnet()) { + locator = l; + break; + } + } + + // Fallback: if subnet check fails or local netif is not fully configured yet, + // still use any valid unicast locator so SEDP matching can proceed. + if (!locator) { + for (unsigned int i = 0; + i < m_proxyDataBuffer.m_metatrafficUnicastLocatorList.size(); i++) { + LocatorIPv4 *l = &(m_proxyDataBuffer.m_metatrafficUnicastLocatorList[i]); + if (l->isValid()) { + locator = l; + break; + } + } + } + + if (!locator) { + return false; + } + +#if SPDP_VERBOSE + ip4_addr_t ip4addr = locator->getIp4Address(); + const char *addr = ip4addr_ntoa(&ip4addr); +#endif + + if (m_proxyDataBuffer.hasPublicationReader()) { + const ReaderProxy proxy{{m_proxyDataBuffer.m_guid.prefix, + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER}, + *locator, + true}; + m_buildInEndpoints.sedpPubWriter->addNewMatchedReader(proxy); + } + + if (m_proxyDataBuffer.hasSubscriptionReader()) { + const ReaderProxy proxy{{m_proxyDataBuffer.m_guid.prefix, + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER}, + *locator, + true}; + m_buildInEndpoints.sedpSubWriter->addNewMatchedReader(proxy); + } + + if (m_proxyDataBuffer.hasPublicationWriter()) { + const WriterProxy proxy{{m_proxyDataBuffer.m_guid.prefix, + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER}, + *locator, + true}; + m_buildInEndpoints.sedpPubReader->addNewMatchedWriter(proxy); + m_buildInEndpoints.sedpPubReader->sendPreemptiveAckNack(proxy); + } + + if (m_proxyDataBuffer.hasSubscriptionWriter()) { + const WriterProxy proxy{{m_proxyDataBuffer.m_guid.prefix, + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER}, + *locator, + true}; + m_buildInEndpoints.sedpSubReader->addNewMatchedWriter(proxy); + m_buildInEndpoints.sedpPubReader->sendPreemptiveAckNack(proxy); + } + + return true; +} + +void SPDPAgent::addInlineQos() { + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_KEY_HASH); + ucdr_serialize_uint16_t(&m_microbuffer, 16); + ucdr_serialize_array_uint8_t(&m_microbuffer, + mp_participant->m_guidPrefix.id.data(), + sizeof(GuidPrefix_t::id)); + ucdr_serialize_array_uint8_t(&m_microbuffer, + ENTITYID_BUILD_IN_PARTICIPANT.entityKey.data(), + sizeof(EntityId_t::entityKey)); + ucdr_serialize_uint8_t( + &m_microbuffer, + static_cast(ENTITYID_BUILD_IN_PARTICIPANT.entityKind)); + + endCurrentList(); +} + +void SPDPAgent::endCurrentList() { + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_SENTINEL); + ucdr_serialize_uint16_t(&m_microbuffer, 0); +} + +void SPDPAgent::addParticipantParameters() { + const uint16_t zero_options = 0; + const uint16_t protocolVersionSize = + sizeof(PROTOCOLVERSION.major) + sizeof(PROTOCOLVERSION.minor); + const uint16_t vendorIdSize = Config::VENDOR_ID.vendorId.size(); + const uint16_t locatorSize = sizeof(FullLengthLocator); + const uint16_t durationSize = + sizeof(Duration_t::seconds) + sizeof(Duration_t::fraction); + const uint16_t entityKeySize = 3; + const uint16_t entityKindSize = 1; + const uint16_t entityIdSize = entityKeySize + entityKindSize; + const uint16_t guidSize = sizeof(GuidPrefix_t::id) + entityIdSize; + + const FullLengthLocator userUniCastLocator = + getUserUnicastLocator(mp_participant->m_participantId); + const FullLengthLocator builtInUniCastLocator = + getBuiltInUnicastLocator(mp_participant->m_participantId); + const FullLengthLocator builtInMultiCastLocator = + getBuiltInMulticastLocator(); + + ucdr_serialize_array_uint8_t(&m_microbuffer, + rtps::SMElement::SCHEME_PL_CDR_LE.data(), + rtps::SMElement::SCHEME_PL_CDR_LE.size()); + ucdr_serialize_uint16_t(&m_microbuffer, zero_options); + + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_PROTOCOL_VERSION); + ucdr_serialize_uint16_t(&m_microbuffer, protocolVersionSize + 2); + ucdr_serialize_uint8_t(&m_microbuffer, PROTOCOLVERSION.major); + ucdr_serialize_uint8_t(&m_microbuffer, PROTOCOLVERSION.minor); + m_microbuffer.iterator += 2; // padding + m_microbuffer.last_data_size = 4; // to 4 byte + + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_VENDORID); + ucdr_serialize_uint16_t(&m_microbuffer, vendorIdSize + 2); + ucdr_serialize_array_uint8_t(&m_microbuffer, + Config::VENDOR_ID.vendorId.data(), vendorIdSize); + m_microbuffer.iterator += 2; // padding + m_microbuffer.last_data_size = 4; // to 4 byte + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_DEFAULT_UNICAST_LOCATOR); + ucdr_serialize_uint16_t(&m_microbuffer, locatorSize); + ucdr_serialize_array_uint8_t( + &m_microbuffer, reinterpret_cast(&userUniCastLocator), + locatorSize); + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR); + ucdr_serialize_uint16_t(&m_microbuffer, locatorSize); + ucdr_serialize_array_uint8_t( + &m_microbuffer, reinterpret_cast(&builtInUniCastLocator), + locatorSize); + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR); + ucdr_serialize_uint16_t(&m_microbuffer, locatorSize); + ucdr_serialize_array_uint8_t( + &m_microbuffer, + reinterpret_cast(&builtInMultiCastLocator), locatorSize); + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_PARTICIPANT_LEASE_DURATION); + ucdr_serialize_uint16_t(&m_microbuffer, durationSize); + ucdr_serialize_int32_t(&m_microbuffer, + Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION.seconds); + ucdr_serialize_uint32_t(&m_microbuffer, + Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION.fraction); + + ucdr_serialize_uint16_t(&m_microbuffer, ParameterId::PID_PARTICIPANT_GUID); + ucdr_serialize_uint16_t(&m_microbuffer, guidSize); + ucdr_serialize_array_uint8_t(&m_microbuffer, + mp_participant->m_guidPrefix.id.data(), + sizeof(GuidPrefix_t::id)); + ucdr_serialize_array_uint8_t(&m_microbuffer, + ENTITYID_BUILD_IN_PARTICIPANT.entityKey.data(), + entityKeySize); + ucdr_serialize_uint8_t( + &m_microbuffer, + static_cast(ENTITYID_BUILD_IN_PARTICIPANT.entityKind)); + + ucdr_serialize_uint16_t(&m_microbuffer, + ParameterId::PID_BUILTIN_ENDPOINT_SET); + ucdr_serialize_uint16_t(&m_microbuffer, sizeof(BuildInEndpointSet)); + ucdr_serialize_uint32_t( + &m_microbuffer, BuildInEndpointSet::DISC_BIE_PARTICIPANT_ANNOUNCER | + BuildInEndpointSet::DISC_BIE_PARTICIPANT_DETECTOR | + BuildInEndpointSet::DISC_BIE_PUBLICATION_ANNOUNCER | + BuildInEndpointSet::DISC_BIE_PUBLICATION_DETECTOR | + BuildInEndpointSet::DISC_BIE_SUBSCRIPTION_ANNOUNCER | + BuildInEndpointSet::DISC_BIE_SUBSCRIPTION_DETECTOR); + + endCurrentList(); +} + +#undef SPDP_VERBOSE diff --git a/components/rtps_embedded/src/discovery/TopicData.cpp b/components/rtps_embedded/src/discovery/TopicData.cpp new file mode 100644 index 000000000..783d6b83d --- /dev/null +++ b/components/rtps_embedded/src/discovery/TopicData.cpp @@ -0,0 +1,246 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ +#include "rtps/discovery/TopicData.h" +#include "rtps/messages/MessageTypes.h" +#include +#include + +using rtps::TopicData; +using rtps::TopicDataCompressed; +using rtps::SMElement::ParameterId; + +bool TopicData::isDisposedFlagSet() const { + return statusInfoValid && ((statusInfo & 0b1)); +} + +bool TopicData::isUnregisteredFlagSet() const { + return statusInfoValid && ((statusInfo & (0b1 << 1)) != 0); +} + +bool TopicData::matchesTopicOf(const TopicData &other) { + return strcmp(this->topicName, other.topicName) == 0 && + strcmp(this->typeName, other.typeName) == 0; +} + +bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { + + // Reset valid flags, as the respective parameters are optional + statusInfoValid = false; + entityIdFromKeyHashValid = false; + + while (ucdr_buffer_remaining(&buffer) >= 4) { + if (ucdr_buffer_has_error(&buffer)) { + while (1) { + printf("FAILED TO DESERIALIZE TOPIC DATA\n"); + } + } + ParameterId pid; + uint16_t length; + FullLengthLocator uLoc; + ucdr_deserialize_uint16_t(&buffer, reinterpret_cast(&pid)); + ucdr_deserialize_uint16_t(&buffer, &length); + + if (ucdr_buffer_remaining(&buffer) < length) { + return false; + } + + switch (pid) { + case ParameterId::PID_ENDPOINT_GUID: + ucdr_deserialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(), + endpointGuid.prefix.id.size()); + ucdr_deserialize_array_uint8_t(&buffer, + endpointGuid.entityId.entityKey.data(), + endpointGuid.entityId.entityKey.size()); + ucdr_deserialize_uint8_t(&buffer, reinterpret_cast( + &endpointGuid.entityId.entityKind)); + break; + case ParameterId::PID_RELIABILITY: + ucdr_deserialize_uint32_t(&buffer, + reinterpret_cast(&reliabilityKind)); + buffer.iterator += 8; + // TODO Skip 8 bytes. don't know what they are yet + break; + case ParameterId::PID_SENTINEL: + return true; + case ParameterId::PID_TOPIC_NAME: + uint32_t topicNameLength; + ucdr_deserialize_uint32_t(&buffer, &topicNameLength); + ucdr_deserialize_array_char(&buffer, topicName, topicNameLength); + break; + case ParameterId::PID_TYPE_NAME: + uint32_t typeNameLength; + ucdr_deserialize_uint32_t(&buffer, &typeNameLength); + ucdr_deserialize_array_char(&buffer, typeName, typeNameLength); + break; + case ParameterId::PID_UNICAST_LOCATOR: + uLoc.readFromUcdrBuffer(buffer); + // Accept valid UDPv4 locators even if subnet detection is temporarily + // unavailable (e.g. early startup) to avoid keeping placeholder defaults. + if (uLoc.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + unicastLocator = uLoc; + printf ("Received unicast locator: %d.%d.%d.%d:%ld\n", uLoc.address[12], uLoc.address[13], uLoc.address[14], uLoc.address[15], uLoc.port); + } + else { + // print warning and the invalid locator for debugging + printf("Warning: Received invalid unicast locator with kind %d\n", + static_cast(uLoc.kind)); + } + break; + case ParameterId::PID_MULTICAST_LOCATOR: + multicastLocator.readFromUcdrBuffer(buffer); + break; + case ParameterId::PID_STATUS_INFO: { + if (length == 4) { + buffer.iterator += 3; // skip first 3 bytes of status info as they are + // reserved parameters + ucdr_deserialize_uint8_t(&buffer, &statusInfo); + statusInfoValid = true; + } else { // Ignore Status Info + buffer.iterator += length; + } + } break; + case ParameterId::PID_KEY_HASH: // only use case so far is deleting remote + // endpoints + { + if (length == 16) { + ucdr_deserialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(), + endpointGuid.prefix.id.size()); + ucdr_deserialize_array_uint8_t( + &buffer, this->entityIdFromKeyHash.entityKey.data(), + this->entityIdFromKeyHash.entityKey.size()); + ucdr_deserialize_uint8_t(&buffer, + reinterpret_cast( + &(this->entityIdFromKeyHash.entityKind))); + entityIdFromKeyHashValid = true; + } else { // Ignore value + buffer.iterator += length; + } + } break; + default: + buffer.iterator += length; + buffer.last_data_size = 1; + } + + uint32_t alignment = ucdr_buffer_alignment(&buffer, 4); + buffer.iterator += alignment; + buffer.last_data_size = 4; // 4 Byte alignment per element + } + return ucdr_buffer_remaining(&buffer) == 0; +} + +bool TopicData::serializeIntoUcdrBuffer(ucdrBuffer &buffer) const { + // TODO Check if buffer length is sufficient + const uint16_t guidSize = sizeof(GuidPrefix_t::id) + 4; + +#if SUPPRESS_UNICAST + if (multicastLocator.kind != LocatorKind_t::LOCATOR_KIND_UDPv4) { +#endif + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_UNICAST_LOCATOR); + ucdr_serialize_uint16_t(&buffer, sizeof(FullLengthLocator)); + ucdr_serialize_array_uint8_t( + &buffer, reinterpret_cast(&unicastLocator), + sizeof(FullLengthLocator)); +#if SUPPRESS_UNICAST + } +#endif + + if (multicastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_MULTICAST_LOCATOR); + ucdr_serialize_uint16_t(&buffer, sizeof(FullLengthLocator)); + ucdr_serialize_array_uint8_t( + &buffer, reinterpret_cast(&multicastLocator), + sizeof(FullLengthLocator)); + } + + // It's a 32 bit instead of 16 because it seems like the field is padded. + const auto lenTopicName = + static_cast(strlen(topicName) + 1); // + \0 + uint16_t topicAlignment = 0; + if (lenTopicName % 4 != 0) { + topicAlignment = static_cast(4 - (lenTopicName % 4)); + } + const auto totalLengthTopicNameField = static_cast( + sizeof(lenTopicName) + lenTopicName + topicAlignment); + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_TOPIC_NAME); + ucdr_serialize_uint16_t(&buffer, totalLengthTopicNameField); + ucdr_serialize_uint32_t(&buffer, lenTopicName); + ucdr_serialize_array_char(&buffer, topicName, lenTopicName); + ucdr_align_to(&buffer, 4); + + // It's a 32 bit instead of 16 because it seems like the field is padded. + const auto lenTypeName = static_cast(strlen(typeName) + 1); // + \0 + uint16_t typeAlignment = 0; + if (lenTypeName % 4 != 0) { + typeAlignment = static_cast(4 - (lenTypeName % 4)); + } + const auto totalLengthTypeNameField = + static_cast(sizeof(lenTypeName) + lenTypeName + typeAlignment); + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_TYPE_NAME); + ucdr_serialize_uint16_t(&buffer, totalLengthTypeNameField); + ucdr_serialize_uint32_t(&buffer, lenTypeName); + ucdr_serialize_array_char(&buffer, typeName, lenTypeName); + ucdr_align_to(&buffer, 4); + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_KEY_HASH); + ucdr_serialize_uint16_t(&buffer, guidSize); + ucdr_serialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(), + endpointGuid.prefix.id.size()); + ucdr_serialize_array_uint8_t(&buffer, endpointGuid.entityId.entityKey.data(), + endpointGuid.entityId.entityKey.size()); + ucdr_serialize_uint8_t( + &buffer, static_cast(endpointGuid.entityId.entityKind)); + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_ENDPOINT_GUID); + ucdr_serialize_uint16_t(&buffer, guidSize); + ucdr_serialize_array_uint8_t(&buffer, endpointGuid.prefix.id.data(), + endpointGuid.prefix.id.size()); + ucdr_serialize_array_uint8_t(&buffer, endpointGuid.entityId.entityKey.data(), + endpointGuid.entityId.entityKey.size()); + ucdr_serialize_uint8_t( + &buffer, static_cast(endpointGuid.entityId.entityKind)); + + const uint8_t unidentifiedOffset = 8; + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_RELIABILITY); + ucdr_serialize_uint16_t(&buffer, + sizeof(ReliabilityKind_t) + unidentifiedOffset); + ucdr_serialize_uint32_t(&buffer, static_cast(reliabilityKind)); + ucdr_serialize_uint32_t(&buffer, 0); // unidentified additional value + ucdr_serialize_uint32_t(&buffer, 0); // unidentified additional value + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_DURABILITY); + ucdr_serialize_uint16_t(&buffer, sizeof(DurabilityKind_t)); + ucdr_serialize_uint32_t(&buffer, static_cast(durabilityKind)); + + ucdr_serialize_uint16_t(&buffer, ParameterId::PID_SENTINEL); + ucdr_serialize_uint16_t(&buffer, 0); + + return true; +} + +bool TopicDataCompressed::matchesTopicOf(const TopicData &other) const { + return (hashCharArray(other.topicName, sizeof(other.topicName)) == + topicHash && + hashCharArray(other.typeName, sizeof(other.typeName)) == typeHash); +} diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps_embedded/src/entities/Domain.cpp new file mode 100644 index 000000000..ac6563816 --- /dev/null +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -0,0 +1,551 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/Domain.h" +#include "rtps/utils/Log.h" +#include "rtps/utils/udpUtils.h" + +#if defined(ESP_PLATFORM) +#include "esp_mac.h" +#endif + +#if DOMAIN_VERBOSE && RTPS_GLOBAL_VERBOSE +#define DOMAIN_LOG(...) \ + if (true) { \ + printf("[Domain] "); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define DOMAIN_LOG(...) // +#endif + +using rtps::Domain; + +Domain::Domain() + : m_threadPool(receiveJumppad, this), + m_transport(ThreadPool::readCallback, &m_threadPool) { + m_transport.createUdpConnection(getUserMulticastPort()); + m_transport.createUdpConnection(getBuiltInMulticastPort()); + m_transport.joinMultiCastGroup(transformIP4ToU32(239, 255, 0, 1)); + createMutex(&m_mutex); +} + +Domain::~Domain() { stop(); } + +bool Domain::completeInit() { + m_initComplete = m_threadPool.startThreads(); + + if (!m_initComplete) { + DOMAIN_LOG("Failed starting threads\n"); + } + + for (auto i = 0; i < m_nextParticipantId; i++) { + m_participants[i].getSPDPAgent().start(); + } + return m_initComplete; +} + +void Domain::stop() { m_threadPool.stopThreads(); } + +void Domain::receiveJumppad(void *callee, const PacketInfo &packet) { + auto domain = static_cast(callee); + domain->receiveCallback(packet); +} + +void Domain::receiveCallback(const PacketInfo &packet) { + if (packet.buffer.firstElement->next != nullptr) { + + DOMAIN_LOG("Cannot handle multiple elements chained. You might " + "want to increase PBUF_POOL_BUFSIZE\n"); + } + + if (isMetaMultiCastPort(packet.destPort)) { + // Pass to all + DOMAIN_LOG("Domain: Multicast to port %u\n", packet.destPort); + for (auto i = 0; i < m_nextParticipantId - PARTICIPANT_START_ID; ++i) { + m_participants[i].newMessage( + static_cast(packet.buffer.firstElement->payload), + packet.buffer.firstElement->len); + } + // First Check if UserTraffic Multicast + } else if (isUserMultiCastPort(packet.destPort)) { + // Pass to Participant with assigned Multicast Adress (Port ist everytime + // the same) + DOMAIN_LOG("Domain: Got user multicast message on port %u\n", + packet.destPort); + for (auto i = 0; i < m_nextParticipantId - PARTICIPANT_START_ID; ++i) { + if (m_participants[i].hasReaderWithMulticastLocator(packet.destAddr)) { + DOMAIN_LOG("Domain: Forward Multicast only to Participant: %u\n", i); + m_participants[i].newMessage( + static_cast(packet.buffer.firstElement->payload), + packet.buffer.firstElement->len); + } + } + } else { + // Pass to addressed one only (Unicast, by Port) + ParticipantId_t id = getParticipantIdFromUnicastPort( + packet.destPort, isUserPort(packet.destPort)); + if (id != PARTICIPANT_ID_INVALID) { + DOMAIN_LOG("Domain: Got unicast message on port %u\n", packet.destPort); + if (id < m_nextParticipantId && + id >= PARTICIPANT_START_ID) { // added extra check to avoid segfault + // (id below START_ID) + m_participants[id - PARTICIPANT_START_ID].newMessage( + static_cast(packet.buffer.firstElement->payload), + packet.buffer.firstElement->len); + } else { + DOMAIN_LOG("Domain: Participant id too high or unplausible.\n"); + } + } else { + DOMAIN_LOG("Domain: Got message to port %u: no matching participant\n", + packet.destPort); + } + } +} + +rtps::Participant *Domain::createParticipant() { + + DOMAIN_LOG("Domain: Creating new participant.\n"); + + auto nextSlot = + static_cast(m_nextParticipantId - PARTICIPANT_START_ID); + if (m_initComplete || m_participants.size() <= nextSlot) { + return nullptr; + } + + auto &entry = m_participants[nextSlot]; + entry.reuse(generateGuidPrefix(m_nextParticipantId), m_nextParticipantId); + registerPort(entry); + createBuiltinWritersAndReaders(entry); + ++m_nextParticipantId; + return &entry; +} + +void Domain::createBuiltinWritersAndReaders(Participant &part) { + // SPDP + StatelessWriter *spdpWriter = + getNextUnusedEndpoint( + m_statelessWriters); + StatelessReader *spdpReader = + getNextUnusedEndpoint( + m_statelessReaders); + + TopicData spdpWriterAttributes; + spdpWriterAttributes.topicName[0] = '\0'; + spdpWriterAttributes.typeName[0] = '\0'; + spdpWriterAttributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + spdpWriterAttributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + spdpWriterAttributes.endpointGuid.prefix = part.m_guidPrefix; + spdpWriterAttributes.endpointGuid.entityId = + ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER; + spdpWriterAttributes.unicastLocator = getBuiltInMulticastLocator(); + + spdpWriter->init(spdpWriterAttributes, TopicKind_t::WITH_KEY, &m_threadPool, + m_transport); + spdpWriter->addNewMatchedReader( + ReaderProxy{{part.m_guidPrefix, ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER}, + getBuiltInMulticastLocator(), + false}); + + TopicData spdpReaderAttributes; + spdpReaderAttributes.endpointGuid = { + part.m_guidPrefix, ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER}; + spdpReader->init(spdpReaderAttributes); + + // SEDP + + // Prepare attributes + TopicData sedpAttributes; + sedpAttributes.topicName[0] = '\0'; + sedpAttributes.typeName[0] = '\0'; + sedpAttributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + sedpAttributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + sedpAttributes.endpointGuid.prefix = part.m_guidPrefix; + sedpAttributes.unicastLocator = + getBuiltInUnicastLocator(part.m_participantId); + + // READER + StatefulReader *sedpPubReader = + getNextUnusedEndpoint( + m_statefulReaders); + sedpAttributes.endpointGuid.entityId = + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER; + sedpPubReader->init(sedpAttributes, m_transport); + + StatefulReader *sedpSubReader = + getNextUnusedEndpoint( + m_statefulReaders); + sedpAttributes.endpointGuid.entityId = + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER; + sedpSubReader->init(sedpAttributes, m_transport); + + // WRITER + StatefulWriter *sedpPubWriter = + getNextUnusedEndpoint( + m_statefulWriters); + sedpAttributes.endpointGuid.entityId = + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER; + sedpPubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, &m_threadPool, + m_transport); + + StatefulWriter *sedpSubWriter = + getNextUnusedEndpoint( + m_statefulWriters); + sedpAttributes.endpointGuid.entityId = + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER; + sedpSubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, &m_threadPool, + m_transport); + + // COLLECT + BuiltInEndpoints endpoints{}; + endpoints.spdpWriter = spdpWriter; + endpoints.spdpReader = spdpReader; + endpoints.sedpPubReader = sedpPubReader; + endpoints.sedpSubReader = sedpSubReader; + endpoints.sedpPubWriter = sedpPubWriter; + endpoints.sedpSubWriter = sedpSubWriter; + + part.addBuiltInEndpoints(endpoints); +} + +void Domain::registerPort(const Participant &part) { + m_transport.createUdpConnection(getUserUnicastPort(part.m_participantId)); + m_transport.createUdpConnection(getBuiltInUnicastPort(part.m_participantId)); + m_threadPool.addBuiltinPort(getBuiltInUnicastPort(part.m_participantId)); +} + +void Domain::registerMulticastPort(FullLengthLocator mcastLocator) { + if (mcastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { + m_transport.createUdpConnection(mcastLocator.getLocatorPort()); + } +} + +rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, + const char *typeName, bool reliable) { + Lock lock{m_mutex}; + if (reliable) { + for (unsigned int i = 0; i < m_statefulReaders.size(); i++) { + if (m_statefulReaders[i].isInitialized()) { + if (strncmp(m_statefulReaders[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statefulReaders[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatefulReader exists already [%s, %s]\n", topicName, + typeName); + + return &m_statefulReaders[i]; + } + } + } else { + for (unsigned int i = 0; i < m_statelessReaders.size(); i++) { + if (m_statelessReaders[i].isInitialized()) { + if (strncmp(m_statelessReaders[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statelessReaders[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatelessReader exists [%s, %s]\n", topicName, typeName); + + return &m_statelessReaders[i]; + } + } + } + + return nullptr; +} + +rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, + const char *typeName, bool reliable) { + Lock lock{m_mutex}; + if (reliable) { + for (unsigned int i = 0; i < m_statefulWriters.size(); i++) { + if (m_statefulWriters[i].isInitialized()) { + if (strncmp(m_statefulWriters[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statefulWriters[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatefulWriter exists [%s, %s]\n", topicName, typeName); + + return &m_statefulWriters[i]; + } + } + } else { + for (unsigned int i = 0; i < m_statelessWriters.size(); i++) { + if (m_statelessWriters[i].isInitialized()) { + if (strncmp(m_statelessWriters[i].m_attributes.topicName, topicName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + if (strncmp(m_statelessWriters[i].m_attributes.typeName, typeName, + Config::MAX_TYPENAME_LENGTH) != 0) { + continue; + } + + DOMAIN_LOG("StatelessWriter exists [%s, %s]\n", topicName, typeName); + + return &m_statelessWriters[i]; + } + } + } + + return nullptr; +} + +rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, + const char *typeName, bool reliable, + bool enforceUnicast) { + Lock lock{m_mutex}; + StatelessWriter *statelessWriter = + getNextUnusedEndpoint( + m_statelessWriters); + StatefulWriter *statefulWriter = + getNextUnusedEndpoint( + m_statefulWriters); + + // Check if there is enough capacity for more writers + if ((reliable && statefulWriter == nullptr) || + (!reliable && statelessWriter == nullptr) || part.isWritersFull()) { + + DOMAIN_LOG("No Writer created. Max Number of Writers reached.\n"); + + return nullptr; + } + + // TODO Distinguish WithKey and NoKey (Also changes EntityKind) + TopicData attributes; + + if (strlen(topicName) > Config::MAX_TOPICNAME_LENGTH || + strlen(typeName) > Config::MAX_TYPENAME_LENGTH) { + return nullptr; + } + strcpy(attributes.topicName, topicName); + strcpy(attributes.typeName, typeName); + attributes.endpointGuid.prefix = part.m_guidPrefix; + attributes.endpointGuid.entityId = { + part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; + attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); + attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + + DOMAIN_LOG("Creating writer[%s, %s]\n", topicName, typeName); + + if (reliable) { + attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + + if (!statefulWriter->init(attributes, TopicKind_t::NO_KEY, &m_threadPool, + m_transport, enforceUnicast)) { + DOMAIN_LOG("StatefulWriter init failed.\n"); + return nullptr; + } + + if (!part.addWriter(statefulWriter)) { + return nullptr; + } + return statefulWriter; + } else { + attributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + + if (!statelessWriter->init(attributes, TopicKind_t::NO_KEY, &m_threadPool, + m_transport, enforceUnicast)) { + DOMAIN_LOG("StatelessWriter init failed.\n"); + return nullptr; + } + + if (!part.addWriter(statelessWriter)) { + return nullptr; + } + return statelessWriter; + } +} + +rtps::Reader *Domain::createReader(Participant &part, const char *topicName, + const char *typeName, bool reliable, + ip4_addr_t mcastaddress) { + Lock lock{m_mutex}; + StatelessReader *statelessReader = + getNextUnusedEndpoint( + m_statelessReaders); + StatefulReader *statefulReader = + getNextUnusedEndpoint( + m_statefulReaders); + + if ((reliable && statefulReader == nullptr) || + (!reliable && statelessReader == nullptr) || part.isReadersFull()) { + + DOMAIN_LOG("No Reader created. Max Number of Readers reached.\n"); + + return nullptr; + } + + // TODO Distinguish WithKey and NoKey (Also changes EntityKind) + TopicData attributes; + + if (strlen(topicName) > Config::MAX_TOPICNAME_LENGTH || + strlen(typeName) > Config::MAX_TYPENAME_LENGTH) { + return nullptr; + } + strcpy(attributes.topicName, topicName); + strcpy(attributes.typeName, typeName); + attributes.endpointGuid.prefix = part.m_guidPrefix; + attributes.endpointGuid.entityId = { + part.getNextUserEntityKey(), + EntityKind_t::USER_DEFINED_READER_WITHOUT_KEY}; + attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); + if (!isZeroAddress(mcastaddress)) { + if (ip4_addr_ismulticast(&mcastaddress)) { + attributes.multicastLocator = rtps::FullLengthLocator::createUDPv4Locator( + ip4_addr1(&mcastaddress), ip4_addr2(&mcastaddress), + ip4_addr3(&mcastaddress), ip4_addr4(&mcastaddress), + getUserMulticastPort()); + m_transport.joinMultiCastGroup( + attributes.multicastLocator.getIp4Address()); + registerMulticastPort(attributes.multicastLocator); + + DOMAIN_LOG("Multicast enabled!\n"); + + } else { + + DOMAIN_LOG("This is not a Multicastaddress!\n"); + } + } + attributes.durabilityKind = DurabilityKind_t::VOLATILE; + + DOMAIN_LOG("Creating reader[%s, %s]\n", topicName, typeName); + + if (reliable) { + + attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; + + statefulReader->init(attributes, m_transport); + + if (!part.addReader(statefulReader)) { + DOMAIN_LOG("Failed to add reader to participant.\n"); + + return nullptr; + } + return statefulReader; + } else { + + attributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; + + statelessReader->init(attributes); + + if (!part.addReader(statelessReader)) { + return nullptr; + } + return statelessReader; + } +} + +bool rtps::Domain::deleteReader(Participant &part, Reader *reader) { + Lock lock{m_mutex}; + if(reader == nullptr || !reader->isInitialized()){ + return false; + } + if (!part.deleteReader(reader)) { + return false; + } + + reader->reset(); + return true; +} + +bool rtps::Domain::deleteWriter(Participant &part, Writer *writer) { + Lock lock{m_mutex}; + if(writer == nullptr || !writer->isInitialized()){ + return false; + } + if (!part.deleteWriter(writer)) { + return false; + } + + writer->reset(); + return true; +} + +void rtps::Domain::printInfo() { + for (unsigned int i = 0; i < m_participants.size(); i++) { + DOMAIN_LOG("Participant %u\r\n", i); + m_participants[i].printInfo(); + } +} + +rtps::GuidPrefix_t Domain::generateGuidPrefix(ParticipantId_t id) const { + GuidPrefix_t prefix; +#if defined(ESP_PLATFORM) + uint8_t mac[6] = {0}; + esp_err_t mac_err = esp_read_mac(mac, ESP_MAC_ETH); + if (mac_err != ESP_OK) { + mac_err = esp_read_mac(mac, ESP_MAC_WIFI_STA); + } + if (mac_err == ESP_OK) { + // Make participant GUID unique per board while keeping a stable layout. + prefix.id[0] = mac[0]; + prefix.id[1] = mac[1]; + prefix.id[2] = mac[2]; + prefix.id[3] = mac[3]; + prefix.id[4] = mac[4]; + prefix.id[5] = mac[5]; + prefix.id[6] = static_cast(id); + prefix.id[7] = Config::VENDOR_ID.vendorId[0]; + prefix.id[8] = Config::VENDOR_ID.vendorId[1]; + prefix.id[9] = Config::DOMAIN_ID; + prefix.id[10] = 0xA5; + prefix.id[11] = 0x5A; + return prefix; + } +#endif + + if (Config::BASE_GUID_PREFIX == GUID_RANDOM) { + for (unsigned int i = 0; i < rtps::Config::BASE_GUID_PREFIX.id.size(); + i++) { + prefix.id[i] = rand(); + } + } else { + for (unsigned int i = 0; i < rtps::Config::BASE_GUID_PREFIX.id.size(); + i++) { + prefix.id[i] = Config::BASE_GUID_PREFIX.id[i]; + } + } + return prefix; +} diff --git a/components/rtps_embedded/src/entities/Participant.cpp b/components/rtps_embedded/src/entities/Participant.cpp new file mode 100644 index 000000000..57efa137a --- /dev/null +++ b/components/rtps_embedded/src/entities/Participant.cpp @@ -0,0 +1,542 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/Participant.h" +#include "rtps/entities/Reader.h" +#include "rtps/entities/Writer.h" +#include "rtps/messages/MessageReceiver.h" +#include "rtps/utils/Lock.h" +#include "rtps/utils/Log.h" + +#if PARTICIPANT_VERBOSE && RTPS_GLOBAL_VERBOSE +#define PARTICIPANT_LOG(...) \ + if (true) { \ + printf("[Participant] "); \ + printf(__VA_ARGS__); \ + printf("\r\n"); \ + } +#else +#define PARTICIPANT_LOG(...) // +#endif + +using rtps::Participant; + +Participant::Participant() + : m_guidPrefix(GUIDPREFIX_UNKNOWN), m_participantId(PARTICIPANT_ID_INVALID), + m_receiver(this) { + if (!createMutex(&m_mutex)) { + std::terminate(); + } +} +Participant::Participant(const GuidPrefix_t &guidPrefix, + ParticipantId_t participantId) + : m_guidPrefix(guidPrefix), m_participantId(participantId), + m_receiver(this) { + if (!createMutex(&m_mutex)) { + while (1) + ; + } +} + +Participant::~Participant() { m_spdpAgent.stop(); } + +void Participant::reuse(const GuidPrefix_t &guidPrefix, + ParticipantId_t participantId) { + m_guidPrefix = guidPrefix; + m_participantId = participantId; +} + +bool Participant::isValid() { + return m_participantId != PARTICIPANT_ID_INVALID; +} + +std::array Participant::getNextUserEntityKey() { + const auto result = m_nextUserEntityId; + + ++m_nextUserEntityId[2]; + if (m_nextUserEntityId[2] == 0) { + ++m_nextUserEntityId[1]; + if (m_nextUserEntityId[1] == 0) { + ++m_nextUserEntityId[0]; + } + } + return result; +} + +bool Participant::registerOnNewPublisherMatchedCallback( + void (*callback)(void *arg), void *args) { + if (!m_hasBuilInEndpoints) { + return false; + } + + m_sedpAgent.registerOnNewPublisherMatchedCallback(callback, args); + return true; +} + +bool Participant::registerOnNewSubscriberMatchedCallback( + void (*callback)(void *arg), void *args) { + if (!m_hasBuilInEndpoints) { + return false; + } + + m_sedpAgent.registerOnNewSubscriberMatchedCallback(callback, args); + return true; +} + +rtps::Writer *Participant::addWriter(Writer *pWriter) { + Lock lock{m_mutex}; + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + m_writers[i] = pWriter; + if (m_hasBuilInEndpoints) { + m_sedpAgent.addWriter(*pWriter); + } + return pWriter; + } + } + return nullptr; +} + +bool Participant::isWritersFull() { + Lock lock{m_mutex}; + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + return false; + } + } + + return true; +} + +rtps::Reader *Participant::addReader(Reader *pReader) { + Lock lock{m_mutex}; + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + m_readers[i] = pReader; + if (m_hasBuilInEndpoints) { + m_sedpAgent.addReader(*pReader); + } + return pReader; + } + } + + return nullptr; +} + +bool Participant::deleteReader(Reader *reader) { + Lock lock{m_mutex}; + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i]->getSEDPSequenceNumber() == + reader->getSEDPSequenceNumber()) { + if (m_sedpAgent.deleteReader(reader)) { + m_readers[i] = nullptr; + return true; + } + PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + } + } + return false; +} + +bool Participant::deleteWriter(Writer *writer) { + Lock lock{m_mutex}; + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i]->getSEDPSequenceNumber() == + writer->getSEDPSequenceNumber()) { + if (m_sedpAgent.deleteWriter(writer)) { + m_writers[i] = nullptr; + return true; + } + PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + } + } + return false; +} + +bool Participant::isReadersFull() { + Lock lock{m_mutex}; + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + return false; + } + } + + return true; +} + +rtps::Writer *Participant::getWriter(EntityId_t id) { + Lock lock{m_mutex}; + for (uint8_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == id) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getReader(EntityId_t id) { + Lock lock{m_mutex}; + for (uint8_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == id) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getReaderByWriterId(const Guid_t &guid) { + Lock lock{m_mutex}; + for (uint8_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->isProxy(guid)) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Writer *Participant::getMatchingWriter(const TopicData &readerTopicData) { + Lock lock{m_mutex}; + for (uint8_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->m_attributes.matchesTopicOf(readerTopicData) && + (readerTopicData.reliabilityKind == ReliabilityKind_t::BEST_EFFORT || + m_writers[i]->m_attributes.reliabilityKind == + ReliabilityKind_t::RELIABLE)) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader *Participant::getMatchingReader(const TopicData &writerTopicData) { + Lock lock{m_mutex}; + for (uint8_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.matchesTopicOf(writerTopicData) && + (writerTopicData.reliabilityKind == ReliabilityKind_t::RELIABLE || + m_readers[i]->m_attributes.reliabilityKind == + ReliabilityKind_t::BEST_EFFORT)) { + return m_readers[i]; + } + } + return nullptr; +} + +rtps::Writer * +Participant::getMatchingWriter(const TopicDataCompressed &readerTopicData) { + Lock lock{m_mutex}; + for (uint8_t i = 0; i < m_writers.size(); ++i) { + if (m_writers[i] == nullptr) { + continue; + } + if (readerTopicData.matchesTopicOf(m_writers[i]->m_attributes) && + (readerTopicData.is_reliable == false || + m_writers[i]->m_attributes.reliabilityKind == + ReliabilityKind_t::RELIABLE)) { + return m_writers[i]; + } + } + return nullptr; +} + +rtps::Reader * +Participant::getMatchingReader(const TopicDataCompressed &writerTopicData) { + Lock lock{m_mutex}; + for (uint8_t i = 0; i < m_readers.size(); ++i) { + if (m_readers[i] == nullptr) { + continue; + } + if (writerTopicData.matchesTopicOf(m_readers[i]->m_attributes) && + (writerTopicData.is_reliable == true || + m_readers[i]->m_attributes.reliabilityKind == + ReliabilityKind_t::BEST_EFFORT)) { + return m_readers[i]; + } + } + return nullptr; +} + +bool Participant::addNewRemoteParticipant( + const ParticipantProxyData &remotePart) { + Lock lock{m_mutex}; + return m_remoteParticipants.add(remotePart); +} + +bool Participant::removeRemoteParticipant(const GuidPrefix_t &prefix) { + Lock lock{m_mutex}; + auto isElementToRemove = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + removeAllProxiesOfParticipant(prefix); + m_sedpAgent.removeUnmatchedEntitiesOfParticipant(prefix); + return m_remoteParticipants.remove(thunk, &isElementToRemove); +} + +void Participant::removeAllProxiesOfParticipant(const GuidPrefix_t &prefix) { + Lock lock{m_mutex}; + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + m_readers[i]->removeAllProxiesOfParticipant(prefix); + } + + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + continue; + } + m_writers[i]->removeAllProxiesOfParticipant(prefix); + } +} + +void Participant::removeProxyFromAllEndpoints(const Guid_t &guid) { + Lock lock{m_mutex}; + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + continue; + } + if (m_writers[i]->removeProxy(guid)) { + PARTICIPANT_LOG("Removing proxy for writer [%s, %s], proxies left = %u\n", + m_writers[i]->m_attributes.topicName, + m_writers[i]->m_attributes.typeName, + (int)m_writers[i]->getProxiesCount()); + } + } + + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->removeProxy(guid)) { + PARTICIPANT_LOG("Removing proxy for reader [%s, %s], proxies left = %u\n", + m_readers[i]->m_attributes.topicName, + m_readers[i]->m_attributes.typeName, + (int)m_readers[i]->getProxiesCount()); + } + } +} + +const rtps::ParticipantProxyData * +Participant::findRemoteParticipant(const GuidPrefix_t &prefix) { + Lock lock{m_mutex}; + auto isElementToFind = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + return m_remoteParticipants.find(thunk, &isElementToFind); +} + +void Participant::refreshRemoteParticipantLiveliness( + const GuidPrefix_t &prefix) { + Lock lock{m_mutex}; + auto isElementToFind = [&](const ParticipantProxyData &proxy) { + return proxy.m_guid.prefix == prefix; + }; + auto thunk = [](void *arg, const ParticipantProxyData &value) { + return (*static_cast(arg))(value); + }; + + auto remoteParticipant = m_remoteParticipants.find(thunk, &isElementToFind); + if (remoteParticipant != nullptr) { + remoteParticipant->onAliveSignal(); + } +} + +bool Participant::hasReaderWithMulticastLocator(ip4_addr_t address) { + Lock lock{m_mutex}; + for (uint8_t i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + continue; + } + if (m_readers[i]->m_attributes.multicastLocator.isSameAddress(&address)) { + return true; + } + } + return false; +} + +uint32_t Participant::getRemoteParticipantCount() { + Lock lock{m_mutex}; + return m_remoteParticipants.getNumElements(); +} + +rtps::MessageReceiver *Participant::getMessageReceiver() { return &m_receiver; } + +bool Participant::checkAndResetHeartbeats() { + Lock lock1{m_mutex}; + Lock lock2{m_spdpAgent.m_mutex}; + PARTICIPANT_LOG("Have %u remote participants", + (unsigned int)m_remoteParticipants.getNumElements()); + PARTICIPANT_LOG( + "Unmatched remote writers/readers, %u / %u", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters()), + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + for (auto &remote : m_remoteParticipants) { + PARTICIPANT_LOG("Remote GUID = %u %u %u %u | Age = %u [ms]", + remote.m_guid.prefix.id[4], remote.m_guid.prefix.id[5], remote.m_guid.prefix.id[6], remote.m_guid.prefix.id[7], (unsigned int)remote.getAliveSignalAgeInMilliseconds() ); + if (remote.isAlive()) { + continue; + } + PARTICIPANT_LOG("removing remote participant"); + bool success = removeRemoteParticipant(remote.m_guid.prefix); + if (!success) { + return false; + }else{ + return true; + } + } + return true; +} + +void Participant::printInfo() { + + uint32_t max_reader_proxies = 0; + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] != nullptr && m_readers[i]->isInitialized()) { + if (m_hasBuilInEndpoints && i < 3) { +#ifdef PARTICIPANT_PRINTINFO_LONG + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER) { + printf("Reader %u: SPDP BUILTIN READER | Remote Proxies = %u \r\n ", + i, static_cast(m_readers[i]->getProxiesCount())); + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER) { + printf( + "Reader %u: SEDP PUBLICATION READER | Remote Proxies = %u \r\n ", + i, static_cast(m_readers[i]->getProxiesCount())); + } + if (m_readers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER) { + printf( + "Reader %u: SEDP SUBSCRIPTION READER | Remote Proxies = %u \r\n", + i, static_cast(m_readers[i]->getProxiesCount())); + } +#endif + continue; + } + + max_reader_proxies = + std::max(max_reader_proxies, m_readers[i]->getProxiesCount()); +#ifdef PARTICIPANT_PRINTINFO_LONG + printf("Reader %u: Topic = %s | Type = %s | Remote Proxies = %u | SEDP " + "SN = %u \r\n ", + i, m_readers[i]->m_attributes.topicName, + m_readers[i]->m_attributes.typeName, + static_cast(m_readers[i]->getProxiesCount()), + static_cast(m_readers[i]->getSEDPSequenceNumber().low)); +#endif + } + } + + uint32_t max_writer_proxies = 0; + for (unsigned int i = 0; i < m_writers.size(); i++) { + + if (m_hasBuilInEndpoints && i < 3) { +#ifdef PARTICIPANT_PRINTINFO_LONG + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER) { + printf("Writer %u: SPDP WRITER | Remote Proxies = %u \r\n ", i, + static_cast(m_writers[i]->getProxiesCount())); + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) { + printf( + "Writer %u: SEDP PUBLICATION WRITER | Remote Proxies = %u \r\n ", + i, static_cast(m_writers[i]->getProxiesCount())); + } + if (m_writers[i]->m_attributes.endpointGuid.entityId == + ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) { + printf( + "Writer %u: SEDP SUBSCRIPTION WRITER | Remote Proxies = %u \r\n ", + i, static_cast(m_writers[i]->getProxiesCount())); + } +#endif + continue; + } + + if (m_writers[i] != nullptr && m_writers[i]->isInitialized()) { + max_writer_proxies = + std::max(max_writer_proxies, m_writers[i]->getProxiesCount()); +#ifdef PARTICIPANT_PRINTINFO_LONG + printf("Writer %u: Topic = %s | Type = %s | Remote Proxies = %u | SEDP " + "SN = %u \r\n ", + i, m_writers[i]->m_attributes.topicName, + m_writers[i]->m_attributes.typeName, + static_cast(m_writers[i]->getProxiesCount()), + static_cast(m_writers[i]->getSEDPSequenceNumber().low)); +#endif + } + } + + printf("Max Writer Proxies %lu \r\n ", max_writer_proxies); + printf("Max Reader Proxies %lu \r\n ", max_reader_proxies); + printf("Unmatched Remote Readers = %u\r\n", + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + printf("Unmatched Remote Writers = %u \r\n ", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters())); + printf("Remote Participants = %u \r\n ", + static_cast(m_remoteParticipants.getNumElements())); +} + +rtps::SPDPAgent &Participant::getSPDPAgent() { return m_spdpAgent; } + +void Participant::addBuiltInEndpoints(BuiltInEndpoints &endpoints) { + Lock lock{m_mutex}; + m_hasBuilInEndpoints = true; + m_spdpAgent.init(*this, endpoints); + m_sedpAgent.init(*this, endpoints); + + // This needs to be done after initializing the agents + addWriter(endpoints.spdpWriter); + addReader(endpoints.spdpReader); + addWriter(endpoints.sedpPubWriter); + addReader(endpoints.sedpPubReader); + addWriter(endpoints.sedpSubWriter); + addReader(endpoints.sedpSubReader); +} + +void Participant::newMessage(const uint8_t *data, DataSize_t size) { + if (!m_receiver.processMessage(data, size)) { + PARTICIPANT_LOG("MESSAGE PROCESSING FAILE \r\n"); + } +} diff --git a/components/rtps_embedded/src/entities/Reader.cpp b/components/rtps_embedded/src/entities/Reader.cpp new file mode 100644 index 000000000..9d22a8e55 --- /dev/null +++ b/components/rtps_embedded/src/entities/Reader.cpp @@ -0,0 +1,165 @@ +#include +#include +#include +#include +#include +#include + +using namespace rtps; + +Reader::Reader() { m_callbacks.fill({nullptr, nullptr, 0}); } + +void Reader::executeCallbacks(const ReaderCacheChange &cacheChange) { + Lock lock{m_callback_mutex}; + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].function != nullptr) { + m_callbacks[i].function(m_callbacks[i].arg, cacheChange); + } + } +} + +bool Reader::initMutex() { + if (m_proxies_mutex == nullptr) { + if (!createMutex(&m_proxies_mutex)) { + SFR_LOG("StatefulReader: Failed to create mutex.\n"); + return false; + } + } + + if (m_callback_mutex == nullptr) { + if (!createMutex(&m_callback_mutex)) { + SFR_LOG("StatefulReader: Failed to create mutex.\n"); + return false; + } + } + + return true; +} + +void Reader::reset() { + Lock lock1{m_proxies_mutex}; + Lock lock2{m_callback_mutex}; + + m_proxies.clear(); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + m_callbacks[i].function = nullptr; + m_callbacks[i].arg = nullptr; + } + + m_callback_count = 0; + m_is_initialized_ = false; +} + +bool Reader::isProxy(const Guid_t &guid) { + for (const auto &proxy : m_proxies) { + if (proxy.remoteWriterGuid.operator==(guid)) { + return true; + } + } + return false; +} + +WriterProxy *Reader::getProxy(Guid_t guid) { + auto isElementToFind = [&](const WriterProxy &proxy) { + return proxy.remoteWriterGuid == guid; + }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + return m_proxies.find(thunk, &isElementToFind); +} + +Reader::callbackIdentifier_t +Reader::registerCallback(Reader::callbackFunction_t cb, void *arg) { + Lock lock{m_callback_mutex}; + if (m_callback_count == m_callbacks.size() || cb == nullptr) { + return false; + } + + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].function == nullptr) { + m_callbacks[i].function = cb; + m_callbacks[i].arg = arg; + m_callbacks[i].identifier = m_callback_identifier++; + m_callback_count++; + return m_callbacks[i].identifier; + } + } + + return 0; +} + +uint32_t Reader::getProxiesCount() { return m_proxies.getNumElements(); } + +bool Reader::removeCallback(Reader::callbackIdentifier_t identifier) { + Lock lock{m_callback_mutex}; + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].identifier == identifier) { + m_callbacks[i].function = nullptr; + m_callbacks[i].arg = nullptr; + m_callback_count--; + return true; + } + } + + return false; +} + +uint8_t Reader::getNumCallbacks() { return m_callback_count; } + +void Reader::removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix) { + Lock lock{m_proxies_mutex}; + auto isElementToRemove = [&](const WriterProxy &proxy) { + return proxy.remoteWriterGuid.prefix == guidPrefix; + }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + + m_proxies.remove(thunk, &isElementToRemove); +} + +bool Reader::removeProxy(const Guid_t &guid) { + Lock lock{m_proxies_mutex}; + auto isElementToRemove = [&](const WriterProxy &proxy) { + return proxy.remoteWriterGuid == guid; + }; + auto thunk = [](void *arg, const WriterProxy &value) { + return (*static_cast(arg))(value); + }; + + return m_proxies.remove(thunk, &isElementToRemove); +} + +bool Reader::addNewMatchedWriter(const WriterProxy &newProxy) { + Lock lock{m_proxies_mutex}; +#if (SFR_VERBOSE || SLR_VERBOSE) && RTPS_GLOBAL_VERBOSE + SFR_LOG("New writer added with id: "); + printGuid(newProxy.remoteWriterGuid); + SFR_LOG("\n"); +#endif + return m_proxies.add(newProxy); +} + +void rtps::Reader::setSEDPSequenceNumber(const SequenceNumber_t &sn) { + m_sedp_sequence_number = sn; +} +const rtps::SequenceNumber_t &rtps::Reader::getSEDPSequenceNumber() { + return m_sedp_sequence_number; +} + +int rtps::Reader::dumpAllProxies(dumpProxyCallback target, void *arg) { + if (target == nullptr) { + return 0; + } + Lock lock{m_proxies_mutex}; + int dump_count = 0; + for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { + target(this, *it, arg); + } + return dump_count; +} + +bool rtps::Reader::sendPreemptiveAckNack(const WriterProxy &writer) { + return true; +} diff --git a/components/rtps_embedded/src/entities/StatelessReader.cpp b/components/rtps_embedded/src/entities/StatelessReader.cpp new file mode 100644 index 000000000..1fde550d6 --- /dev/null +++ b/components/rtps_embedded/src/entities/StatelessReader.cpp @@ -0,0 +1,81 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/entities/StatelessReader.h" +#include "rtps/utils/Lock.h" +#include "rtps/utils/Log.h" + +using rtps::StatelessReader; + +#if SLR_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define SLR_LOG(...) \ + if (true) { \ + printf("[StatelessReader %s] ", &m_attributes.topicName[0]); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define SLR_LOG(...) // +#endif + +bool StatelessReader::init(const TopicData &attributes) { + if (!initMutex()) { + return false; + } + + m_proxies.clear(); + m_attributes = attributes; + m_is_initialized_ = true; + return true; +} + +void StatelessReader::newChange(const ReaderCacheChange &cacheChange) { + if (!m_is_initialized_) { + return; + } + executeCallbacks(cacheChange); +} + +bool StatelessReader::addNewMatchedWriter(const WriterProxy &newProxy) { +#if (SLR_VERBOSE && RTPS_GLOBAL_VERBOSE) + SLR_LOG("Adding WriterProxy"); + printGuid(newProxy.remoteWriterGuid); + printf("\n"); +#endif + return m_proxies.add(newProxy); +} + +bool StatelessReader::onNewHeartbeat(const SubmessageHeartbeat &, + const GuidPrefix_t &) { + // nothing to do + return true; +} + +bool StatelessReader::onNewGapMessage(const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) { + return true; +} + +#undef SLR_VERBOSE diff --git a/components/rtps_embedded/src/entities/Writer.cpp b/components/rtps_embedded/src/entities/Writer.cpp new file mode 100644 index 000000000..be21ffee0 --- /dev/null +++ b/components/rtps_embedded/src/entities/Writer.cpp @@ -0,0 +1,147 @@ +#include "rtps/utils/Log.h" +#include +#include +#include +#include +#include + +using namespace rtps; + +bool rtps::Writer::addNewMatchedReader(const ReaderProxy &newProxy) { + INIT_GUARD(); +#if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE + SFW_LOG("New reader added with id: "); + printGuid(newProxy.remoteReaderGuid); +#endif + Lock lock{m_mutex}; + bool success = m_proxies.add(newProxy); + if (!m_enforceUnicast) { + manageSendOptions(); + } + return success; +} + +bool rtps::Writer::removeProxy(const Guid_t &guid) { + INIT_GUARD() + Lock lock{m_mutex}; + auto isElementToRemove = [&](const ReaderProxy &proxy) { + return proxy.remoteReaderGuid == guid; + }; + auto thunk = [](void *arg, const ReaderProxy &value) { + return (*static_cast(arg))(value); + }; + + bool ret = m_proxies.remove(thunk, &isElementToRemove); + resetSendOptions(); + return ret; +} + +uint32_t rtps::Writer::getProxiesCount() { + Lock lock{m_mutex}; + return m_proxies.getNumElements(); +} + +void rtps::Writer::resetSendOptions() { + INIT_GUARD() + for (auto &proxy : m_proxies) { + proxy.suppressUnicast = false; + proxy.useMulticast = false; + proxy.unknown_eid = false; + } + manageSendOptions(); +} + +const rtps::CacheChange *rtps::Writer::newChange(ChangeKind_t kind, + const uint8_t *data, + DataSize_t size) { + return newChange(kind, data, size, false, false); +} + +void rtps::Writer::manageSendOptions() { + INIT_GUARD(); + Lock lock{m_mutex}; + for (auto &proxy : m_proxies) { + if (proxy.remoteMulticastLocator.kind == + LocatorKind_t::LOCATOR_KIND_INVALID) { + proxy.suppressUnicast = false; + proxy.useMulticast = false; + } else { + bool found = false; + for (auto &avproxy : m_proxies) { + if (avproxy.remoteMulticastLocator.kind == + LocatorKind_t::LOCATOR_KIND_UDPv4 && + avproxy.remoteMulticastLocator.getIp4Address().addr == + proxy.remoteMulticastLocator.getIp4Address().addr && + avproxy.remoteLocator.getIp4Address().addr != + proxy.remoteLocator.getIp4Address().addr) { + if (avproxy.suppressUnicast == false) { + avproxy.useMulticast = false; + avproxy.suppressUnicast = true; + proxy.useMulticast = true; + proxy.suppressUnicast = true; + if (avproxy.remoteReaderGuid.entityId != + proxy.remoteReaderGuid.entityId) { + proxy.unknown_eid = true; + } + } + found = true; + } + } + if (!found) { + proxy.useMulticast = false; + proxy.suppressUnicast = false; + } + } + } +} + +void rtps::Writer::removeAllProxiesOfParticipant( + const GuidPrefix_t &guidPrefix) { + INIT_GUARD(); + Lock lock{m_mutex}; + auto isElementToRemove = [&](const ReaderProxy &proxy) { + return proxy.remoteReaderGuid.prefix == guidPrefix; + }; + auto thunk = [](void *arg, const ReaderProxy &value) { + return (*static_cast(arg))(value); + }; + + m_proxies.remove(thunk, &isElementToRemove); + resetSendOptions(); +} + +bool rtps::Writer::isBuiltinEndpoint() { + return !(m_attributes.endpointGuid.entityId.entityKind == + EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY || + m_attributes.endpointGuid.entityId.entityKind == + EntityKind_t::USER_DEFINED_WRITER_WITH_KEY); +} + +bool rtps::Writer::isIrrelevant(ChangeKind_t kind) const { + // Right now we only allow alive changes + // return kind == ChangeKind_t::INVALID || (m_topicKind == TopicKind_t::NO_KEY + // && kind != ChangeKind_t::ALIVE); + return kind != ChangeKind_t::ALIVE; +} + +bool rtps::Writer::isInitialized() { return m_is_initialized_; } + +void rtps::Writer::setSEDPSequenceNumber(const SequenceNumber_t &sn) { + m_sedp_sequence_number = sn; +} + +const rtps::SequenceNumber_t &rtps::Writer::getSEDPSequenceNumber() { + return m_sedp_sequence_number; +} + +int rtps::Writer::dumpAllProxies(dumpProxyCallback target, void *arg) { + if (target == nullptr) { + return 0; + } + Lock lock{m_mutex}; + int dump_count = 0; + for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { + target(this, *it, arg); + } + return dump_count; +} diff --git a/components/rtps_embedded/src/messages/MessageReceiver.cpp b/components/rtps_embedded/src/messages/MessageReceiver.cpp new file mode 100644 index 000000000..589f2e7e9 --- /dev/null +++ b/components/rtps_embedded/src/messages/MessageReceiver.cpp @@ -0,0 +1,239 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/messages/MessageReceiver.h" +#include + +#include "rtps/entities/Reader.h" +#include "rtps/entities/Writer.h" +#include "rtps/messages/MessageTypes.h" +#include "rtps/utils/Log.h" + +using rtps::MessageReceiver; + +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define RECV_LOG(...) \ + if (true) { \ + printf("[RECV] "); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define RECV_LOG(...) // +#endif + +MessageReceiver::MessageReceiver(Participant *part) : mp_part(part) {} + +void MessageReceiver::resetState() { + sourceGuidPrefix = GUIDPREFIX_UNKNOWN; + sourceVersion = PROTOCOLVERSION; + sourceVendor = VENDOR_UNKNOWN; + haveTimeStamp = false; +} + +bool MessageReceiver::processMessage(const uint8_t *data, DataSize_t size) { + resetState(); + MessageProcessingInfo msgInfo(data, size); + + if (!processHeader(msgInfo)) { + return false; + } + SubmessageHeader submsgHeader; + while (msgInfo.nextPos < msgInfo.size) { + if (!deserializeMessage(msgInfo, submsgHeader)) { + return false; + } + processSubmessage(msgInfo, submsgHeader); + } + + return true; +} + +bool MessageReceiver::processHeader(MessageProcessingInfo &msgInfo) { + Header header; + if (!deserializeMessage(msgInfo, header)) { + return false; + } + + if (header.guidPrefix.id == mp_part->m_guidPrefix.id) { + RECV_LOG("[MessageReceiver]: Received own message.\n"); + return false; // Don't process our own packet + } + + if (header.protocolName != RTPS_PROTOCOL_NAME || + header.protocolVersion.major != PROTOCOLVERSION.major) { + return false; + } + + sourceGuidPrefix = header.guidPrefix; + sourceVendor = header.vendorId; + sourceVersion = header.protocolVersion; + + msgInfo.nextPos += Header::getRawSize(); + return true; +} + +bool MessageReceiver::processSubmessage(MessageProcessingInfo &msgInfo, + const SubmessageHeader &submsgHeader) { + bool success = false; + + switch (submsgHeader.submessageId) { + case SubmessageKind::ACKNACK: + RECV_LOG("Processing AckNack submessage\n"); + success = processAckNackSubmessage(msgInfo); + break; + case SubmessageKind::DATA: + RECV_LOG("Processing Data submessage\n"); + success = processDataSubmessage(msgInfo, submsgHeader); + break; + case SubmessageKind::HEARTBEAT: + RECV_LOG("Processing Heartbeat submessage\n"); + success = processHeartbeatSubmessage(msgInfo); + break; + case SubmessageKind::INFO_DST: + RECV_LOG("Info_DST submessage not relevant.\n"); + success = true; // Not relevant + break; + case SubmessageKind::GAP: + RECV_LOG("Processing GAP submessage\n"); + success = processGapSubmessage(msgInfo); + break; + case SubmessageKind::INFO_TS: + RECV_LOG("Info_TS submessage not relevant.\n"); + success = true; // Not relevant now + break; + default: + RECV_LOG("Submessage of type %u currently not supported. Skipping..\n", + static_cast(submsgHeader.submessageId)); + success = false; + } + msgInfo.nextPos += + submsgHeader.octetsToNextHeader + SubmessageHeader::getRawSize(); + return success; +} + +bool MessageReceiver::processDataSubmessage( + MessageProcessingInfo &msgInfo, const SubmessageHeader &submsgHeader) { + SubmessageData dataSubmsg; + if (!deserializeMessage(msgInfo, dataSubmsg)) { + return false; + } + + const uint8_t *serializedData = + msgInfo.getPointerToCurrentPos() + SubmessageData::getRawSize(); + + const DataSize_t size = submsgHeader.octetsToNextHeader - + SubmessageData::getRawSize() + + SubmessageHeader::getRawSize(); + + RECV_LOG("Received data message size %u", (int)size); + + Reader *reader; + if (dataSubmsg.readerId == ENTITYID_UNKNOWN) { +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + RECV_LOG("Received ENTITYID_UNKNOWN readerID, searching for writer ID = "); + printGuid(Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); + printf("\n"); +#endif + reader = mp_part->getReaderByWriterId( + Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); + if (reader != nullptr) + RECV_LOG("Found reader!"); + } else { + reader = mp_part->getReader(dataSubmsg.readerId); +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + auto reader_by_writer = mp_part->getReaderByWriterId( + Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); + + if (reader_by_writer == nullptr && reader != nullptr) { + RECV_LOG("FOUND By READER ID, NOT BY WRITER ID ="); + printGuid(Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); + printf("\n"); + } +#endif + } + if (reader != nullptr) { + Guid_t writerGuid{sourceGuidPrefix, dataSubmsg.writerId}; + ReaderCacheChange change{ChangeKind_t::ALIVE, writerGuid, + dataSubmsg.writerSN, serializedData, size}; + reader->newChange(change); + } else { +#if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE + RECV_LOG("Couldn't find a reader with id: "); + printEntityId(dataSubmsg.readerId); + printf("\n"); +#endif + } + + return true; +} + +bool MessageReceiver::processHeartbeatSubmessage( + MessageProcessingInfo &msgInfo) { + SubmessageHeartbeat submsgHB; + if (!deserializeMessage(msgInfo, submsgHB)) { + return false; + } + + Reader *reader = mp_part->getReader(submsgHB.readerId); + if (reader != nullptr) { + reader->onNewHeartbeat(submsgHB, sourceGuidPrefix); + mp_part->refreshRemoteParticipantLiveliness(sourceGuidPrefix); + return true; + } else { + return false; + } +} + +bool MessageReceiver::processAckNackSubmessage(MessageProcessingInfo &msgInfo) { + SubmessageAckNack submsgAckNack; + if (!deserializeMessage(msgInfo, submsgAckNack)) { + return false; + } + + Writer *writer = mp_part->getWriter(submsgAckNack.writerId); + if (writer != nullptr) { + writer->onNewAckNack(submsgAckNack, sourceGuidPrefix); + return true; + } else { + return false; + } +} + +bool MessageReceiver::processGapSubmessage(MessageProcessingInfo &msgInfo) { + SubmessageGap submsgGap; + if (!deserializeMessage(msgInfo, submsgGap)) { + return false; + } + + Reader *reader = mp_part->getReader(submsgGap.readerId); + if (reader != nullptr) { + reader->onNewGapMessage(submsgGap, sourceGuidPrefix); + return true; + } else { + return false; + } +} +#undef RECV_VERBOSE diff --git a/components/rtps_embedded/src/messages/MessageTypes.cpp b/components/rtps_embedded/src/messages/MessageTypes.cpp new file mode 100644 index 000000000..bf56695d5 --- /dev/null +++ b/components/rtps_embedded/src/messages/MessageTypes.cpp @@ -0,0 +1,216 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/messages/MessageTypes.h" +#include + +#include +using namespace rtps; + +void doCopyAndMoveOn(uint8_t *dst, const uint8_t *&src, size_t size) { + memcpy(dst, src, size); + src += size; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + Header &header) { + if (info.getRemainingSize() < Header::getRawSize()) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos(); + doCopyAndMoveOn(header.protocolName.data(), currentPos, + sizeof(std::array)); + doCopyAndMoveOn(reinterpret_cast(&header.protocolVersion), + currentPos, sizeof(ProtocolVersion_t)); + doCopyAndMoveOn(header.vendorId.vendorId.data(), currentPos, + header.vendorId.vendorId.size()); + doCopyAndMoveOn(header.guidPrefix.id.data(), currentPos, + header.guidPrefix.id.size()); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageHeader &header) { + if (info.getRemainingSize() < SubmessageHeader::getRawSize()) { + return false; + } + + const uint8_t *currentPos = info.getPointerToCurrentPos(); + header.submessageId = static_cast(*currentPos++); + header.flags = *(currentPos++); + doCopyAndMoveOn(reinterpret_cast(&header.octetsToNextHeader), + currentPos, sizeof(uint16_t)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageData &msg) { + if (info.getRemainingSize() < SubmessageHeader::getRawSize()) { + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + // Check for length including data + if (info.getRemainingSize() < + SubmessageHeader::getRawSize() + msg.header.octetsToNextHeader) { + return false; + } + + const uint8_t *currentPos = + info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(reinterpret_cast(&msg.extraFlags), currentPos, + sizeof(uint16_t)); + doCopyAndMoveOn(reinterpret_cast(&msg.octetsToInlineQos), + currentPos, sizeof(uint16_t)); + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, + msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, + msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(reinterpret_cast(&msg.writerSN.high), currentPos, + sizeof(msg.writerSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.writerSN.low), currentPos, + sizeof(msg.writerSN.low)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageHeartbeat &msg) { + if (info.getRemainingSize() < SubmessageHeartbeat::getRawSize()) { + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = + info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, + msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, + msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(reinterpret_cast(&msg.firstSN.high), currentPos, + sizeof(msg.firstSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.firstSN.low), currentPos, + sizeof(msg.firstSN.low)); + doCopyAndMoveOn(reinterpret_cast(&msg.lastSN.high), currentPos, + sizeof(msg.lastSN.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.lastSN.low), currentPos, + sizeof(msg.lastSN.low)); + doCopyAndMoveOn(reinterpret_cast(&msg.count.value), currentPos, + sizeof(msg.count.value)); + return true; +} + +void rtps::deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, + size_t num_bitfields) { + + doCopyAndMoveOn(reinterpret_cast(&set.base.high), position, + sizeof(SequenceNumber_t::high)); + doCopyAndMoveOn(reinterpret_cast(&set.base.low), position, + sizeof(SequenceNumber_t::low)); + doCopyAndMoveOn(reinterpret_cast(&set.numBits), position, + sizeof(uint32_t)); + + // Ensure that we copy not more bits than our sequence number set can hold + if (set.numBits != 0) { + // equal to size = std::min(SNS_NUM_BYTES, num_bitfields) + size_t size = num_bitfields > SNS_NUM_BYTES ? SNS_NUM_BYTES : num_bitfields; + doCopyAndMoveOn(reinterpret_cast(set.bitMap.data()), position, + size); + position += (num_bitfields - size); + } +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageAckNack &msg) { + const DataSize_t remainingSizeAtBeginning = info.getRemainingSize(); + if (remainingSizeAtBeginning < + SubmessageAckNack:: + getRawSizeWithoutSNSet()) { // Size of SequenceNumberSet unknown + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = + info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, + msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, + msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + + size_t num_bitfields = msg.header.octetsToNextHeader - 4 - 4 - 8 - 4 - 4; + deserializeSNS(currentPos, msg.readerSNState, num_bitfields); + + doCopyAndMoveOn(reinterpret_cast(&msg.count.value), currentPos, + sizeof(msg.count.value)); + return true; +} + +bool rtps::deserializeMessage(const MessageProcessingInfo &info, + SubmessageGap &msg) { + + const DataSize_t remainingSizeAtBeginning = info.getRemainingSize(); + if (remainingSizeAtBeginning < + SubmessageGap::getRawSizeWithoutSNSet()) { // Size of SequenceNumberSet + // unknown + return false; + } + if (!deserializeMessage(info, msg.header)) { + return false; + } + + const uint8_t *currentPos = + info.getPointerToCurrentPos() + SubmessageHeader::getRawSize(); + + doCopyAndMoveOn(msg.readerId.entityKey.data(), currentPos, + msg.readerId.entityKey.size()); + msg.readerId.entityKind = static_cast(*currentPos++); + doCopyAndMoveOn(msg.writerId.entityKey.data(), currentPos, + msg.writerId.entityKey.size()); + msg.writerId.entityKind = static_cast(*currentPos++); + + doCopyAndMoveOn(reinterpret_cast(&msg.gapStart.high), currentPos, + sizeof(msg.gapStart.high)); + doCopyAndMoveOn(reinterpret_cast(&msg.gapStart.low), currentPos, + sizeof(msg.gapStart.low)); + + size_t num_bitfields = msg.header.octetsToNextHeader - 4 - 4 - 8 - 8 - 4; + deserializeSNS(currentPos, msg.gapList, num_bitfields); + + return true; +} diff --git a/components/rtps_embedded/src/rtps.cpp b/components/rtps_embedded/src/rtps.cpp new file mode 100644 index 000000000..67c14094d --- /dev/null +++ b/components/rtps_embedded/src/rtps.cpp @@ -0,0 +1,119 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/rtps.h" +#include "lwip/sys.h" +#include +#include + +// Currently initialization is expected to be done in project main for e.g. +// Aurix and STM32 +#if defined(unix) || defined(__unix__) || defined(WIN32) || defined(_WIN32) || \ + defined(__WIN32) && !defined(__CYGWIN__) + +#include "lwip/ip4_addr.h" +#include "lwip/netif.h" +#include "lwipcfg.h" +#include + +#if defined(unix) || defined(__unix__) +#include "netif/tapif.h" +#elif defined(WIN32) || defined(_WIN32) || \ + defined(__WIN32) && !defined(__CYGWIN__) +#include "../pcapif.h" +#include "default_netif.h" +#else +#include "ethernetif.h" +#endif + +#define INIT_VERBOSE 0 + +static struct netif netif; + +static void init(void *arg) { + if (arg == nullptr) { +#if INIT_VERBOSE + printf("Failed to init. nullptr passed"); +#endif + return; + } + auto init_sem = static_cast(arg); + + srand((unsigned int)time(nullptr)); + + ip4_addr_t ipaddr; + ip4_addr_t netmask; + ip4_addr_t gw; + LWIP_PORT_INIT_GW(&gw); + LWIP_PORT_INIT_IPADDR(&ipaddr); + LWIP_PORT_INIT_NETMASK(&netmask); +#if INIT_VERBOSE + printf("Starting lwIP, local interface IP is %s\n", ip4addr_ntoa(&ipaddr)); +#endif + +#if defined(unix) || defined(__unix__) + netif_add(&netif, &ipaddr, &netmask, &gw, nullptr, tapif_init, tcpip_input); +#elif defined(WIN32) || defined(_WIN32) || \ + defined(__WIN32) && !defined(__CYGWIN__) + netif_add(&netif, &ipaddr, &netmask, &gw, nullptr, pcapif_init, tcpip_input); +#else + netif_add(&netif, &ipaddr, &netmask, &gw, nullptr, ethernetif_init, + tcpip_input); +#endif + + netif_set_default(&netif); + netif_set_up(netif_default); + + sys_sem_signal(init_sem); +} + +void LwIPInit() { + /* no stdio-buffering, please! */ + setvbuf(stdout, nullptr, _IONBF, 0); + + err_t err; + sys_sem_t init_sem; + + err = sys_sem_new(&init_sem, 0); + LWIP_ASSERT("failed to create init_sem", err == ERR_OK); + LWIP_UNUSED_ARG(err); + tcpip_init(init, &init_sem); + /* we have to wait for initialization to finish before + * calling update_adapter()! */ + sys_sem_wait(&init_sem); + sys_sem_free(&init_sem); +} + +void rtps::init() { + // TODO This is not threadsafe. Might cause problems in tests. For now, it + // seems to work. + static bool initialized = false; + if (!initialized) { + LwIPInit(); + initialized = true; + } +} +#endif + +#undef INIT_VERBOSE diff --git a/components/rtps_embedded/src/storages/PBufWrapper.cpp b/components/rtps_embedded/src/storages/PBufWrapper.cpp new file mode 100644 index 000000000..c183f049f --- /dev/null +++ b/components/rtps_embedded/src/storages/PBufWrapper.cpp @@ -0,0 +1,163 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +// Copyright 2023 Apex.AI, Inc. +// All rights reserved. + +#include "rtps/storages/PBufWrapper.h" +#include "rtps/utils/Log.h" + +using rtps::PBufWrapper; + +#if PBUF_WRAP_VERBOSE && RTPS_GLOBAL_VERBOSE +#include "rtps/utils/printutils.h" +#define PBUF_WRAP_LOG(...) \ + if (true) { \ + printf("[PBUF Wrapper] "); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + } +#else +#define PBUF_WRAP_LOG(...) // +#endif + +PBufWrapper::PBufWrapper(pbuf *bufferToWrap) : firstElement(bufferToWrap) { + m_freeSpace = 0; // Assume it to be full +} + +PBufWrapper::PBufWrapper(DataSize_t length) + : firstElement(pbuf_alloc(m_layer, length, m_type)) { + + if (isValid()) { + m_freeSpace = length; + } +} + +// TODO: Uses move assignment. Improvement possible +PBufWrapper::PBufWrapper(PBufWrapper &&other) noexcept { + *this = std::move(other); +} + +PBufWrapper &PBufWrapper::operator=(PBufWrapper &&other) noexcept { + copySimpleMembersAndResetBuffer(other); + + if (other.firstElement != nullptr) { + firstElement = other.firstElement; + other.firstElement = nullptr; + } + return *this; +} + +void PBufWrapper::copySimpleMembersAndResetBuffer(const PBufWrapper &other) { + m_freeSpace = other.m_freeSpace; + + if (firstElement != nullptr) { + pbuf_free(firstElement); + firstElement = nullptr; + } +} + +void PBufWrapper::destroy() +{ + if (firstElement != nullptr) { + pbuf_free(firstElement); + firstElement = nullptr; + } + m_freeSpace = 0; +} + +PBufWrapper::~PBufWrapper() { + destroy(); +} + +bool PBufWrapper::isValid() const { return firstElement != nullptr; } + +rtps::DataSize_t PBufWrapper::spaceLeft() const { return m_freeSpace; } + +rtps::DataSize_t PBufWrapper::spaceUsed() const { + if (firstElement == nullptr) { + return 0; + } + + return firstElement->tot_len - m_freeSpace; +} + +bool PBufWrapper::append(const uint8_t *data, DataSize_t length) { + if (data == nullptr) { + return false; + } + + err_t err = pbuf_take_at(firstElement, data, length, spaceUsed()); + if (err != ERR_OK) { + return false; + } + m_freeSpace -= length; + return true; +} + +void PBufWrapper::append(const PBufWrapper &other) { + if (this->firstElement == nullptr) { + m_freeSpace = other.m_freeSpace; + this->firstElement = other.firstElement; + pbuf_ref(this->firstElement); + return; + } + + m_freeSpace += other.m_freeSpace; + pbuf_chain(this->firstElement, other.firstElement); +} + +bool PBufWrapper::reserve(DataSize_t length) { + int16_t additionalAllocation = length - m_freeSpace; + if (additionalAllocation <= 0) { + return true; + } + + return increaseSizeBy(additionalAllocation); +} + +void PBufWrapper::reset() { + if (firstElement != nullptr) { + m_freeSpace = firstElement->tot_len; + } +} + +bool PBufWrapper::increaseSizeBy(uint16_t length) { + pbuf *allocation = pbuf_alloc(m_layer, length, m_type); + if (allocation == nullptr) { + return false; + } + + m_freeSpace += length; + + if (firstElement == nullptr) { + firstElement = allocation; + } else { + pbuf_cat(firstElement, allocation); + } + + return true; +} + +#undef PBUF_WRAP_VERBOSE diff --git a/components/rtps_embedded/src/utils/Diagnostics.cpp b/components/rtps_embedded/src/utils/Diagnostics.cpp new file mode 100644 index 000000000..2f50f0c1d --- /dev/null +++ b/components/rtps_embedded/src/utils/Diagnostics.cpp @@ -0,0 +1,53 @@ +#include + +namespace rtps { +namespace Diagnostics { + +namespace ThreadPool { +uint32_t dropped_incoming_packets_usertraffic = 0; +uint32_t dropped_incoming_packets_metatraffic = 0; + +uint32_t dropped_outgoing_packets_usertraffic = 0; +uint32_t dropped_outgoing_packets_metatraffic = 0; + +uint32_t processed_incoming_metatraffic = 0; +uint32_t processed_outgoing_metatraffic = 0; +uint32_t processed_incoming_usertraffic = 0; +uint32_t processed_outgoing_usertraffic = 0; + +uint32_t max_ever_elements_outgoing_usertraffic_queue; +uint32_t max_ever_elements_incoming_usertraffic_queue; + +uint32_t max_ever_elements_outgoing_metatraffic_queue; +uint32_t max_ever_elements_incoming_metatraffic_queue; + +} // namespace ThreadPool + +namespace StatefulReader { +uint32_t sfr_unexpected_sn; +uint32_t sfr_retransmit_requests; +} // namespace StatefulReader + +namespace Network { +uint32_t lwip_allocation_failures; +} + +namespace SEDP { +uint32_t max_ever_remote_participants; +uint32_t current_remote_participants; + +uint32_t max_ever_matched_reader_proxies; +uint32_t current_max_matched_reader_proxies; + +uint32_t max_ever_matched_writer_proxies; +uint32_t current_max_matched_writer_proxies; + +uint32_t max_ever_unmatched_reader_proxies; +uint32_t current_max_unmatched_reader_proxies; + +uint32_t max_ever_unmatched_writer_proxies; +uint32_t current_max_unmatched_writer_proxies; +} // namespace SEDP + +} // namespace Diagnostics +} // namespace rtps diff --git a/components/rtps_embedded/src/utils/Lock.cpp b/components/rtps_embedded/src/utils/Lock.cpp new file mode 100644 index 000000000..d3b48fc68 --- /dev/null +++ b/components/rtps_embedded/src/utils/Lock.cpp @@ -0,0 +1,15 @@ +#include "rtps/utils/Lock.h" + +namespace rtps { + +bool createMutex(SemaphoreHandle_t *mutex) { + *mutex = xSemaphoreCreateRecursiveMutex(); + if (*mutex != NULL) { + return true; + } else { + LWIP_ASSERT("Mutex creation failed", true); + return false; + } +} + +} // namespace rtps From bf22a300ca2e08f54b11dc3eb121c23b0f96b9ea Mon Sep 17 00:00:00 2001 From: Guo Date: Wed, 8 Jul 2026 14:29:23 -0500 Subject: [PATCH 02/17] add submodule --- components/rtps_embedded/.gitmodules | 3 +++ components/rtps_embedded/include/rtps/config.h | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) create mode 100644 components/rtps_embedded/.gitmodules diff --git a/components/rtps_embedded/.gitmodules b/components/rtps_embedded/.gitmodules new file mode 100644 index 000000000..205841b7f --- /dev/null +++ b/components/rtps_embedded/.gitmodules @@ -0,0 +1,3 @@ +[submodule "thirdparty/ucdr"] + path = thirdparty/ucdr + url = git@github.com:eProsima/Micro-CDR.git diff --git a/components/rtps_embedded/include/rtps/config.h b/components/rtps_embedded/include/rtps/config.h index 89b8aac94..43642c4a9 100644 --- a/components/rtps_embedded/include/rtps/config.h +++ b/components/rtps_embedded/include/rtps/config.h @@ -31,7 +31,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #if defined(ESP_PLATFORM) #include "rtps/config_esp32.h" #else -#include "rtps/config_stm.h" +#include "rtps/config_desktop.h" #endif #endif From bc85052d881fe373cd7049c0e09c5238be6c2308 Mon Sep 17 00:00:00 2001 From: Guo Date: Wed, 8 Jul 2026 14:30:23 -0500 Subject: [PATCH 03/17] update submodule --- components/rtps_embedded/.gitmodules | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/components/rtps_embedded/.gitmodules b/components/rtps_embedded/.gitmodules index 205841b7f..12a05f7ee 100644 --- a/components/rtps_embedded/.gitmodules +++ b/components/rtps_embedded/.gitmodules @@ -1,3 +1,3 @@ -[submodule "thirdparty/ucdr"] - path = thirdparty/ucdr +[submodule "thirdparty/Micro-CDR"] + path = thirdparty/Micro-CDR url = git@github.com:eProsima/Micro-CDR.git From 734c114ef46a9e97d9c543c5857c57cae5ff5818 Mon Sep 17 00:00:00 2001 From: Guo Date: Wed, 8 Jul 2026 16:19:19 -0500 Subject: [PATCH 04/17] add example --- .gitignore | 3 + .gitmodules | 3 + components/rtps_embedded/.gitmodules | 3 - components/rtps_embedded/CMakeLists.txt | 3 +- .../rtps_embedded/example/CMakeLists.txt | 21 +++ .../rtps_embedded/example/main/CMakeLists.txt | 3 + .../example/main/Kconfig.projbuild | 109 +++++++++++ .../rtps_embedded/example/main/main.cpp | 175 ++++++++++++++++++ .../rtps_embedded/example/partitions.csv | 5 + .../rtps_embedded/example/sdkconfig.defaults | 13 ++ components/rtps_embedded/thirdparty/Micro-CDR | 1 + 11 files changed, 335 insertions(+), 4 deletions(-) delete mode 100644 components/rtps_embedded/.gitmodules create mode 100644 components/rtps_embedded/example/CMakeLists.txt create mode 100644 components/rtps_embedded/example/main/CMakeLists.txt create mode 100644 components/rtps_embedded/example/main/Kconfig.projbuild create mode 100644 components/rtps_embedded/example/main/main.cpp create mode 100644 components/rtps_embedded/example/partitions.csv create mode 100644 components/rtps_embedded/example/sdkconfig.defaults create mode 160000 components/rtps_embedded/thirdparty/Micro-CDR diff --git a/.gitignore b/.gitignore index e4c7f6946..ab5c012df 100644 --- a/.gitignore +++ b/.gitignore @@ -34,6 +34,9 @@ # build folder for ESP-IDF build/ +# submodule configuration +components/rtps_embedded/thirdparty/Micro-CDR/include/ucdr/config.h + # we only version sdkconfig.defaults sdkconfig sdkconfig.old diff --git a/.gitmodules b/.gitmodules index 7817faf37..c4061a1ab 100644 --- a/.gitmodules +++ b/.gitmodules @@ -37,3 +37,6 @@ [submodule "components/gfps_service/detail/nearby"] path = components/gfps_service/detail/nearby url = https://github.com/esp-cpp/nearby +[submodule "components/rtps_embedded/thirdparty/Micro-CDR"] + path = components/rtps_embedded/thirdparty/Micro-CDR + url = git@github.com:eProsima/Micro-CDR.git diff --git a/components/rtps_embedded/.gitmodules b/components/rtps_embedded/.gitmodules deleted file mode 100644 index 12a05f7ee..000000000 --- a/components/rtps_embedded/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "thirdparty/Micro-CDR"] - path = thirdparty/Micro-CDR - url = git@github.com:eProsima/Micro-CDR.git diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt index a3ee053d8..e50bd49f1 100644 --- a/components/rtps_embedded/CMakeLists.txt +++ b/components/rtps_embedded/CMakeLists.txt @@ -24,8 +24,9 @@ idf_component_register( "thirdparty/Micro-CDR/src/c/types/string.c" INCLUDE_DIRS "include" + "thirdparty/Micro-CDR/include" REQUIRES - base_component cdr task socket + base_component cdr task socket lwip freertos ) # target_compile_features(${COMPONENT_LIB} PUBLIC cxx_std_17) diff --git a/components/rtps_embedded/example/CMakeLists.txt b/components/rtps_embedded/example/CMakeLists.txt new file mode 100644 index 000000000..b65580252 --- /dev/null +++ b/components/rtps_embedded/example/CMakeLists.txt @@ -0,0 +1,21 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py logger rtps_embedded wifi" + CACHE STRING + "List of components to include" + ) + +project(rtps_embedded_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/rtps_embedded/example/main/CMakeLists.txt b/components/rtps_embedded/example/main/CMakeLists.txt new file mode 100644 index 000000000..71403499e --- /dev/null +++ b/components/rtps_embedded/example/main/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS "." + REQUIRES logger rtps_embedded wifi) diff --git a/components/rtps_embedded/example/main/Kconfig.projbuild b/components/rtps_embedded/example/main/Kconfig.projbuild new file mode 100644 index 000000000..7d4a64dbc --- /dev/null +++ b/components/rtps_embedded/example/main/Kconfig.projbuild @@ -0,0 +1,109 @@ +menu "RTPS Example Configuration" + + choice RTPS_EXAMPLE_ROLE + prompt "Example Role" + default RTPS_EXAMPLE_ROLE_INITIATOR + help + Build one board as the initiator and a second board as the responder + to verify RTPS discovery and end-to-end UInt32 sample exchange. + + config RTPS_EXAMPLE_ROLE_INITIATOR + bool "Initiator" + help + Publishes incrementing request samples after it discovers a + responder, then logs the matching responses. + + config RTPS_EXAMPLE_ROLE_RESPONDER + bool "Responder" + help + Waits for request samples and echoes the same value back on the + response topic. + + endchoice + + config RTPS_EXAMPLE_NODE_NAME + string "Participant node name" + default "espp_rtps_initiator" if RTPS_EXAMPLE_ROLE_INITIATOR + default "espp_rtps_responder" if RTPS_EXAMPLE_ROLE_RESPONDER + help + Logical RTPS participant name announced during discovery. + + config RTPS_EXAMPLE_DOMAIN_ID + int "RTPS domain ID" + range 0 231 + default 0 + help + Both boards must use the same domain ID to discover each other. + + config RTPS_EXAMPLE_PARTICIPANT_ID + int "RTPS participant ID" + range 0 119 + default 1 if RTPS_EXAMPLE_ROLE_INITIATOR + default 2 if RTPS_EXAMPLE_ROLE_RESPONDER + help + Each board should use a unique participant ID within the same domain. + + config RTPS_EXAMPLE_TOPIC_PREFIX + string "Topic prefix" + default "espp/rtps_example" + help + Prefix used to derive the request and response topics. + + config RTPS_EXAMPLE_ANNOUNCE_PERIOD_MS + int "Discovery announce period (ms)" + range 200 10000 + default 1500 + help + Period between periodic SPDP/SEDP discovery announcements. + + config RTPS_EXAMPLE_PUBLISH_PERIOD_MS + int "Initiator publish period (ms)" + range 250 60000 + default 2000 + depends on RTPS_EXAMPLE_ROLE_INITIATOR + help + Period between request messages sent by the initiator after a + responder has been discovered. + + config RTPS_EXAMPLE_USE_USER_MULTICAST + bool "Use best-effort user-data multicast" + default n + help + If enabled, the example advertises and uses topic-specific + multicast groups for the request and response topics instead of + per-participant unicast delivery. + + config RTPS_EXAMPLE_REQUEST_MULTICAST_GROUP + string "Request topic multicast group" + default "239.255.0.11" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /request topic. + + config RTPS_EXAMPLE_RESPONSE_MULTICAST_GROUP + string "Response topic multicast group" + default "239.255.0.12" + depends on RTPS_EXAMPLE_USE_USER_MULTICAST + help + IPv4 multicast group used for the /response topic. + + config ESP_WIFI_SSID + string "WiFi SSID" + default "" + help + SSID (network name) for the example to connect to. + + config ESP_WIFI_PASSWORD + string "WiFi Password" + default "" + help + WiFi password (WPA or WPA2) for the example to use. + + config ESP_MAXIMUM_RETRY + int "Maximum retry" + default 5 + help + Set the maximum retry count to avoid reconnecting forever when the + network is unavailable. + +endmenu diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp new file mode 100644 index 000000000..09ddab53b --- /dev/null +++ b/components/rtps_embedded/example/main/main.cpp @@ -0,0 +1,175 @@ + +#include +#include + +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +#include "rtps/entities/Domain.h" +#include "logger.hpp" +#include "wifi_sta.hpp" + +using namespace std::chrono_literals; + +namespace { + +static const char *TAG = "embedded_rtps"; +static bool s_started = false; +static rtps::Domain *s_domain = nullptr; +static rtps::Participant *s_participant = nullptr; +static rtps::Writer *s_writer = nullptr; +static rtps::Reader *s_reader = nullptr; +static char s_node_name[16] = "node"; + +bool is_dhcps_node() { + return (strcmp(s_node_name, "DHCPS") == 0 || + strcmp(s_node_name, "dhcps") == 0); +} + +bool is_dhcpc_node() { + return (strcmp(s_node_name, "DHCPC") == 0 || + strcmp(s_node_name, "dhcpc") == 0); +} + +bool send_text_message(const char *text) { + if (text == nullptr || s_writer == nullptr) { + return false; + } + + const size_t len = strnlen(text, 127); + const rtps::CacheChange *change = s_writer->newChange( + rtps::ChangeKind_t::ALIVE, reinterpret_cast(text), + static_cast(len + 1)); + + return change != nullptr; +} + +void reader_cb(void *callee, const rtps::ReaderCacheChange &change) { + (void)callee; + char buffer[128] = {0}; + const rtps::DataSize_t copy_len = + (change.getDataSize() < sizeof(buffer) - 1) ? change.getDataSize() + : (sizeof(buffer) - 1); + + if (copy_len == 0) { + return; + } + + if (!change.copyInto(reinterpret_cast(buffer), sizeof(buffer))) { + return; + } + + ESP_LOGI(TAG, "RTPS rx (%u B): %s", static_cast(copy_len), buffer); + + if (strcmp(buffer, "who are you? Are you PC?") == 0 && is_dhcpc_node()) { + if (send_text_message("i am dhcpc")) { + ESP_LOGI(TAG, "RTPS tx: i am dhcpc"); + } else { + ESP_LOGW(TAG, "RTPS tx dropped: i am dhcpc"); + } + } else if (is_dhcps_node()) { + ESP_LOGI(TAG, "RTPS Rx: %s", buffer); + } +} + +void publisher_task(void *arg) { + (void)arg; + while (true) { + if (is_dhcps_node()) { + if (!send_text_message("who are you? Are you PC?")) { + ESP_LOGW(TAG, "RTPS tx dropped (history full or no matched reader)"); + } else { + ESP_LOGI(TAG, "RTPS tx: who are you? Are you PC?"); + } + } + + vTaskDelay(pdMS_TO_TICKS(2000)); + } +} + +} // namespace + +extern "C" void embedded_rtps_start(const char *node_name, + const char *pub_topic, + const char *sub_topic) { + if (s_started) { + return; + } + + if (node_name == nullptr || pub_topic == nullptr || sub_topic == nullptr) { + ESP_LOGE(TAG, "RTPS start rejected due to invalid arguments"); + return; + } + + strncpy(s_node_name, node_name, sizeof(s_node_name) - 1); + s_node_name[sizeof(s_node_name) - 1] = '\0'; + + static rtps::Domain domain; + s_domain = &domain; + + s_participant = s_domain->createParticipant(); + if (s_participant == nullptr) { + ESP_LOGE(TAG, "Failed to create RTPS participant"); + return; + } + + // Complete domain init first so built-in discovery endpoints and worker + // threads are fully initialized before user endpoints are added. + if (!s_domain->completeInit()) { + ESP_LOGE(TAG, "Failed to complete RTPS domain init"); + return; + } + + s_writer = s_domain->createWriter(*s_participant, pub_topic, + "std_msgs::msg::String", false); + if (s_writer == nullptr) { + ESP_LOGE(TAG, "Failed to create RTPS writer"); + return; + } + + s_reader = s_domain->createReader(*s_participant, sub_topic, + "std_msgs::msg::String", false); + if (s_reader == nullptr) { + ESP_LOGE(TAG, "Failed to create RTPS reader"); + return; + } + + if (s_reader->registerCallback(reader_cb, nullptr) == 0) { + ESP_LOGE(TAG, "Failed to register RTPS reader callback"); + return; + } + + xTaskCreate(publisher_task, "rtps_pub", 4096, nullptr, 5, nullptr); + s_started = true; + ESP_LOGI(TAG, "EmbeddedRTPS started: pub=%s sub=%s", pub_topic, sub_topic); +} + + +extern "C" void app_main(void) +{ + espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); + + //! [rtps example] + std::string ip_address; + espp::WifiSta wifi_sta({.ssid = CONFIG_ESP_WIFI_SSID, + .password = CONFIG_ESP_WIFI_PASSWORD, + .num_connect_retries = CONFIG_ESP_MAXIMUM_RETRY, + .on_connected = nullptr, + .on_disconnected = nullptr, + .on_got_ip = [&ip_address](ip_event_got_ip_t *eventdata) { + ip_address = fmt::format("{}.{}.{}.{}", IP2STR(&eventdata->ip_info.ip)); + fmt::print("got IP: {}\n", ip_address); + }}); + + logger.info("Waiting for WiFi connection..."); + while (!wifi_sta.is_connected()) { + std::this_thread::sleep_for(100ms); + } + logger.info("WiFi connected, local IP {}", ip_address); + + embedded_rtps_start("DHCPS", "rtps_embedded_pub", "rtps_embedded_sub"); + while (true) { + vTaskDelay(pdMS_TO_TICKS(1000)); + } +} diff --git a/components/rtps_embedded/example/partitions.csv b/components/rtps_embedded/example/partitions.csv new file mode 100644 index 000000000..c4217ab9e --- /dev/null +++ b/components/rtps_embedded/example/partitions.csv @@ -0,0 +1,5 @@ +# ESP-IDF Partition Table +# Name, Type, SubType, Offset, Size, Flags +nvs, data, nvs, 0x9000, 0x6000, +phy_init, data, phy, 0xf000, 0x1000, +factory, app, factory, 0x10000, 1500K, diff --git a/components/rtps_embedded/example/sdkconfig.defaults b/components/rtps_embedded/example/sdkconfig.defaults new file mode 100644 index 000000000..e823df850 --- /dev/null +++ b/components/rtps_embedded/example/sdkconfig.defaults @@ -0,0 +1,13 @@ +# Common ESP-related +# +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 + +# +# Partition Table +# +CONFIG_PARTITION_TABLE_CUSTOM=y +CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_OFFSET=0x8000 +CONFIG_PARTITION_TABLE_MD5=y diff --git a/components/rtps_embedded/thirdparty/Micro-CDR b/components/rtps_embedded/thirdparty/Micro-CDR new file mode 160000 index 000000000..99672492c --- /dev/null +++ b/components/rtps_embedded/thirdparty/Micro-CDR @@ -0,0 +1 @@ +Subproject commit 99672492c5ef9fc378a8835b0bce9b2f7fa41306 From f81ef555f1884ed458997ef451f7564b2d1b5527 Mon Sep 17 00:00:00 2001 From: Guo Date: Thu, 9 Jul 2026 17:00:16 -0500 Subject: [PATCH 05/17] WIP; add substrct layer for platform --- components/rtps_embedded/CMakeLists.txt | 6 + .../rtps_embedded/include/rtps/ThreadPool.h | 10 +- .../include/rtps/common/Locator.h | 22 +- .../rtps_embedded/include/rtps/common/types.h | 2 +- .../rtps/communication/EsppTransport.h | 81 +++++++ .../include/rtps/communication/UdpDriver.h | 9 +- .../include/rtps/entities/Domain.h | 13 +- .../include/rtps/entities/StatefulReader.h | 4 +- .../include/rtps/entities/StatefulWriter.h | 6 +- .../include/rtps/entities/StatefulWriter.tpp | 26 +-- .../include/rtps/entities/StatelessWriter.h | 3 +- .../include/rtps/platform/bootstrap.h | 39 ++++ .../include/rtps/platform/platform_types.h | 44 ++++ .../include/rtps/platform/sync.h | 26 +++ .../include/rtps/platform/threading.h | 52 +++++ .../include/rtps/platform/transport.h | 27 +++ .../rtps/storages/ThreadSafeCircularBuffer.h | 9 +- .../storages/ThreadSafeCircularBuffer.tpp | 2 +- .../rtps_embedded/include/rtps/utils/Lock.h | 25 +- components/rtps_embedded/src/ThreadPool.cpp | 50 ++-- .../src/communication/EsppTransport.cpp | 216 ++++++++++++++++++ .../src/communication/UdpDriver.cpp | 2 +- .../rtps_embedded/src/discovery/SPDPAgent.cpp | 10 +- .../rtps_embedded/src/entities/Domain.cpp | 47 ++-- .../rtps_embedded/src/platform/sync.cpp | 31 +++ components/rtps_embedded/src/rtps.cpp | 19 +- components/rtps_embedded/src/utils/Lock.cpp | 14 +- 27 files changed, 666 insertions(+), 129 deletions(-) create mode 100644 components/rtps_embedded/include/rtps/communication/EsppTransport.h create mode 100644 components/rtps_embedded/include/rtps/platform/bootstrap.h create mode 100644 components/rtps_embedded/include/rtps/platform/platform_types.h create mode 100644 components/rtps_embedded/include/rtps/platform/sync.h create mode 100644 components/rtps_embedded/include/rtps/platform/threading.h create mode 100644 components/rtps_embedded/include/rtps/platform/transport.h create mode 100644 components/rtps_embedded/src/communication/EsppTransport.cpp create mode 100644 components/rtps_embedded/src/platform/sync.cpp diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt index e50bd49f1..aebe80af1 100644 --- a/components/rtps_embedded/CMakeLists.txt +++ b/components/rtps_embedded/CMakeLists.txt @@ -2,6 +2,7 @@ idf_component_register( SRCS "src/rtps.cpp" "src/ThreadPool.cpp" + "src/communication/EsppTransport.cpp" "src/communication/UdpDriver.cpp" "src/discovery/ParticipantProxyData.cpp" "src/discovery/SEDPAgent.cpp" @@ -14,6 +15,7 @@ idf_component_register( "src/entities/Writer.cpp" "src/messages/MessageReceiver.cpp" "src/messages/MessageTypes.cpp" + "src/platform/sync.cpp" "src/storages/PBufWrapper.cpp" "src/utils/Diagnostics.cpp" "src/utils/Lock.cpp" @@ -29,5 +31,9 @@ idf_component_register( base_component cdr task socket lwip freertos ) +if(DEFINED RTPS_USE_ESPP_TRANSPORT AND RTPS_USE_ESPP_TRANSPORT) + target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_USE_ESPP_TRANSPORT=1) +endif() + # target_compile_features(${COMPONENT_LIB} PUBLIC cxx_std_17) # target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER=\"rtps/config_esp32.h\") diff --git a/components/rtps_embedded/include/rtps/ThreadPool.h b/components/rtps_embedded/include/rtps/ThreadPool.h index 9ef5ed766..534bccbf3 100644 --- a/components/rtps_embedded/include/rtps/ThreadPool.h +++ b/components/rtps_embedded/include/rtps/ThreadPool.h @@ -25,10 +25,10 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_THREADPOOL_H #define RTPS_THREADPOOL_H -#include "lwip/sys.h" #include "rtps/communication/PacketInfo.h" #include "rtps/communication/UdpDriver.h" #include "rtps/config.h" +#include "rtps/platform/threading.h" #include "rtps/storages/PBufWrapper.h" #include "rtps/storages/ThreadSafeCircularBuffer.h" @@ -62,14 +62,14 @@ class ThreadPool { receiveJumppad_fp m_receiveJumppad; void *m_callee; bool m_running = false; - std::array m_writers; - std::array m_readers; + std::array m_writers; + std::array m_readers; std::array m_builtinPorts; size_t m_builtinPortsIdx = 0; - sys_sem_t m_readerNotificationSem; - sys_sem_t m_writerNotificationSem; + platform::threading::SemaphoreHandle m_readerNotificationSem; + platform::threading::SemaphoreHandle m_writerNotificationSem; void updateDiagnostics(); diff --git a/components/rtps_embedded/include/rtps/common/Locator.h b/components/rtps_embedded/include/rtps/common/Locator.h index b1c35df7e..08f7effed 100644 --- a/components/rtps_embedded/include/rtps/common/Locator.h +++ b/components/rtps_embedded/include/rtps/common/Locator.h @@ -26,11 +26,11 @@ Author: i11 - Embedded Software, RWTH Aachen University #define RTPS_LOCATOR_T_H #include "rtps/communication/UdpDriver.h" +#include "rtps/config.h" +#include "rtps/platform/platform_types.h" #include "rtps/utils/udpUtils.h" #include "ucdr/microcdr.h" -#include "lwip/netif.h" - #include namespace rtps { @@ -38,15 +38,7 @@ namespace rtps { inline std::array getLocalIp4AddressBytes() { std::array ip = {Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], Config::IP_ADDRESS[2], Config::IP_ADDRESS[3]}; - if (netif_default != nullptr) { - const ip4_addr_t *iface_ip = netif_ip4_addr(netif_default); - if (iface_ip != nullptr) { - ip[0] = ip4_addr1(iface_ip); - ip[1] = ip4_addr2(iface_ip); - ip[2] = ip4_addr3(iface_ip); - ip[3] = ip4_addr4(iface_ip); - } - } + (void)platform::tryGetDefaultIp4AddressBytes(ip); return ip; } @@ -103,13 +95,13 @@ struct FullLengthLocator { } } - ip4_addr_t getIp4Address() const { + platform::Ip4Address getIp4Address() const { return transformIP4ToU32(address[12], address[13], address[14], address[15]); } - bool isSameAddress(ip4_addr_t *address) { - ip4_addr_t ownaddress = getIp4Address(); + bool isSameAddress(platform::Ip4Address *address) { + platform::Ip4Address ownaddress = getIp4Address(); return ip4_addr_cmp(&ownaddress, address); } @@ -174,7 +166,7 @@ struct LocatorIPv4 { kind = locator.kind; } - ip4_addr_t getIp4Address() const { + platform::Ip4Address getIp4Address() const { return transformIP4ToU32(address[0], address[1], address[2], address[3]); } diff --git a/components/rtps_embedded/include/rtps/common/types.h b/components/rtps_embedded/include/rtps/common/types.h index 35123c0da..1a1563cb4 100644 --- a/components/rtps_embedded/include/rtps/common/types.h +++ b/components/rtps_embedded/include/rtps/common/types.h @@ -25,7 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_TYPES_H #define RTPS_TYPES_H -#include "lwip/ip4_addr.h" +#include "rtps/platform/platform_types.h" #include #include diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.h b/components/rtps_embedded/include/rtps/communication/EsppTransport.h new file mode 100644 index 000000000..5c54af806 --- /dev/null +++ b/components/rtps_embedded/include/rtps/communication/EsppTransport.h @@ -0,0 +1,81 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#ifndef RTPS_ESPPTRANSPORT_H +#define RTPS_ESPPTRANSPORT_H + +#include "lwip/pbuf.h" +#include "lwip/udp.h" +#include "rtps/communication/UdpConnection.h" +#include "rtps/config.h" +#include "rtps/platform/transport.h" +#include "udp_socket.hpp" + +#include +#include +#include +#include +#include + +namespace rtps { + +class EsppTransport : public platform::transport::ITransport { +public: + using RxCallback = + void (*)(void *arg, udp_pcb *pcb, pbuf *p, const ip_addr_t *addr, + Ip4Port_t port); + + EsppTransport(RxCallback callback, void *args); + ~EsppTransport() override = default; + + const rtps::UdpConnection *createUdpConnection(Ip4Port_t receivePort) override; + bool joinMultiCastGroup(platform::Ip4Address addr) const override; + void sendPacket(PacketInfo &info) override; + +private: + struct Channel { + rtps::UdpConnection connection{}; + std::unique_ptr socket{}; + bool in_use{false}; + }; + + Channel *findChannel(Ip4Port_t port); + const Channel *findChannel(Ip4Port_t port) const; + Channel *createChannel(Ip4Port_t receivePort); + bool startReceiver(Channel &channel, Ip4Port_t receivePort); + void onReceive(Ip4Port_t receivePort, std::vector &data, + const espp::Socket::Info &sender) const; + + static std::string ip4ToString(platform::Ip4Address addr); + + RxCallback m_rxCallback{nullptr}; + void *m_callbackArgs{nullptr}; + mutable std::recursive_mutex m_mutex; + std::array m_channels{}; + mutable std::vector m_multicastGroups; +}; + +} // namespace rtps + +#endif // RTPS_ESPPTRANSPORT_H diff --git a/components/rtps_embedded/include/rtps/communication/UdpDriver.h b/components/rtps_embedded/include/rtps/communication/UdpDriver.h index 80fcd399c..55f6c404b 100644 --- a/components/rtps_embedded/include/rtps/communication/UdpDriver.h +++ b/components/rtps_embedded/include/rtps/communication/UdpDriver.h @@ -30,13 +30,14 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/common/types.h" #include "rtps/communication/PacketInfo.h" #include "rtps/config.h" +#include "rtps/platform/transport.h" #include "rtps/storages/PBufWrapper.h" #include namespace rtps { -class UdpDriver { +class UdpDriver : public platform::transport::ITransport { public: typedef void (*udpRxFunc_fp)(void *arg, udp_pcb *pcb, pbuf *p, @@ -44,9 +45,9 @@ class UdpDriver { UdpDriver(udpRxFunc_fp callback, void *args); - const rtps::UdpConnection *createUdpConnection(Ip4Port_t receivePort); - bool joinMultiCastGroup(ip4_addr_t addr) const; - void sendPacket(PacketInfo &info); + const rtps::UdpConnection *createUdpConnection(Ip4Port_t receivePort) override; + bool joinMultiCastGroup(platform::Ip4Address addr) const override; + void sendPacket(PacketInfo &info) override; static bool isSameSubnet(ip4_addr_t addr); static bool isMulticastAddress(ip4_addr_t addr); diff --git a/components/rtps_embedded/include/rtps/entities/Domain.h b/components/rtps_embedded/include/rtps/entities/Domain.h index bdc981db3..8b094d4eb 100644 --- a/components/rtps_embedded/include/rtps/entities/Domain.h +++ b/components/rtps_embedded/include/rtps/entities/Domain.h @@ -26,12 +26,15 @@ Author: i11 - Embedded Software, RWTH Aachen University #define RTPS_DOMAIN_H #include "rtps/ThreadPool.h" +#include "rtps/communication/EsppTransport.h" +#include "rtps/communication/UdpDriver.h" #include "rtps/config.h" #include "rtps/entities/Participant.h" #include "rtps/entities/StatefulReader.h" #include "rtps/entities/StatefulWriter.h" #include "rtps/entities/StatelessReader.h" #include "rtps/entities/StatelessWriter.h" +#include "rtps/platform/transport.h" #include "rtps/storages/PBufWrapper.h" #include @@ -39,6 +42,7 @@ namespace rtps { class Domain { public: Domain(); + explicit Domain(platform::transport::ITransport &transport); ~Domain(); bool completeInit(); @@ -65,7 +69,13 @@ class Domain { private: friend class SizeInspector; ThreadPool m_threadPool; - UdpDriver m_transport; +#if defined(RTPS_USE_ESPP_TRANSPORT) + using DefaultTransport = EsppTransport; +#else + using DefaultTransport = UdpDriver; +#endif + DefaultTransport m_defaultTransport; + platform::transport::ITransport *m_transport = nullptr; std::array m_participants; const uint8_t PARTICIPANT_START_ID = 0; ParticipantId_t m_nextParticipantId = PARTICIPANT_START_ID; @@ -89,6 +99,7 @@ class Domain { void receiveCallback(const PacketInfo &packet); GuidPrefix_t generateGuidPrefix(ParticipantId_t id) const; void createBuiltinWritersAndReaders(Participant &part); + void initializeTransport(); void registerPort(const Participant &part); void registerMulticastPort(FullLengthLocator mcastLocator); static void receiveJumppad(void *callee, const PacketInfo &packet); diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.h b/components/rtps_embedded/include/rtps/entities/StatefulReader.h index 55497f943..c8b09bf34 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.h +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.h @@ -26,9 +26,9 @@ Author: i11 - Embedded Software, RWTH Aachen University #define RTPS_STATEFULREADER_H #include "lwip/sys.h" -#include "rtps/communication/UdpDriver.h" #include "rtps/config.h" #include "rtps/entities/Reader.h" +#include "rtps/platform/transport.h" #include "rtps/entities/WriterProxy.h" #include "rtps/storages/MemoryPool.h" @@ -53,7 +53,7 @@ template class StatefulReaderT final : public Reader { NetworkDriver *m_transport; }; -using StatefulReader = StatefulReaderT; +using StatefulReader = StatefulReaderT; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h index 60d71489c..ea547f996 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h @@ -27,6 +27,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/ReaderProxy.h" #include "rtps/entities/Writer.h" +#include "rtps/platform/transport.h" +#include "rtps/platform/threading.h" #include "rtps/storages/HistoryCacheWithDeletion.h" #include "rtps/storages/MemoryPool.h" @@ -67,7 +69,7 @@ template class StatefulWriterT final : public Writer { ThreadSafeCircularBuffer m_disposeWithDelay; void dropDisposeAfterWriteChanges(); - sys_thread_t m_heartbeatThread; + platform::threading::ThreadHandle m_heartbeatThread; Count_t m_hbCount{1}; @@ -83,7 +85,7 @@ template class StatefulWriterT final : public Writer { const SequenceNumber_t &nextValid); }; -using StatefulWriter = StatefulWriterT; +using StatefulWriter = StatefulWriterT; } // namespace rtps #include "StatefulWriter.tpp" diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp index 33bb4d7df..c86c95d01 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -22,10 +22,10 @@ This file is part of embeddedRTPS. Author: i11 - Embedded Software, RWTH Aachen University */ -#include "lwip/sys.h" #include "rtps/entities/StatefulWriter.h" #include "rtps/messages/MessageFactory.h" #include "rtps/messages/MessageTypes.h" +#include "rtps/platform/threading.h" #include "rtps/utils/Log.h" #include #include @@ -48,7 +48,7 @@ template StatefulWriterT::~StatefulWriterT() { m_running = false; while (m_thread_running) { - sys_msleep(500); // Required for tests/ Join currently not available / + platform::threading::sleepMs(500); // Required for tests/ Join currently not available / // increased because Segfault in Tests if(sys_mutex_valid(&m_mutex)){ // sys_mutex_free(&m_mutex); // } @@ -100,18 +100,18 @@ bool StatefulWriterT::init(TopicData attributes, if (m_attributes.endpointGuid.entityId == ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) { - m_heartbeatThread = sys_thread_new("HBThreadPub", hbFunctionJumppad, this, - Config::HEARTBEAT_STACKSIZE, - Config::THREAD_POOL_WRITER_PRIO); + m_heartbeatThread = platform::threading::startThread( + "HBThreadPub", hbFunctionJumppad, this, + Config::HEARTBEAT_STACKSIZE, Config::THREAD_POOL_WRITER_PRIO); } else if (m_attributes.endpointGuid.entityId == ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) { - m_heartbeatThread = sys_thread_new("HBThreadSub", hbFunctionJumppad, this, - Config::HEARTBEAT_STACKSIZE, - Config::THREAD_POOL_WRITER_PRIO); + m_heartbeatThread = platform::threading::startThread( + "HBThreadSub", hbFunctionJumppad, this, + Config::HEARTBEAT_STACKSIZE, Config::THREAD_POOL_WRITER_PRIO); } else { - m_heartbeatThread = sys_thread_new("HBThread", hbFunctionJumppad, this, - Config::HEARTBEAT_STACKSIZE, - Config::THREAD_POOL_WRITER_PRIO); + m_heartbeatThread = platform::threading::startThread( + "HBThread", hbFunctionJumppad, this, Config::HEARTBEAT_STACKSIZE, + Config::THREAD_POOL_WRITER_PRIO); } } @@ -458,9 +458,9 @@ void StatefulWriterT::sendHeartBeatLoop() { vTaskDelay(pdMS_TO_TICKS(Config::SF_WRITER_HB_PERIOD_MS)); } #else - sys_msleep(Config::SF_WRITER_HB_PERIOD_MS / 4); + platform::threading::sleepMs(Config::SF_WRITER_HB_PERIOD_MS / 4); } else { - sys_msleep(Config::SF_WRITER_HB_PERIOD_MS); + platform::threading::sleepMs(Config::SF_WRITER_HB_PERIOD_MS); } #endif } diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h index e2a05083c..8603c0ad7 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h @@ -29,6 +29,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/common/types.h" #include "rtps/config.h" #include "rtps/entities/Writer.h" +#include "rtps/platform/transport.h" #include "rtps/storages/MemoryPool.h" #include "rtps/storages/SimpleHistoryCache.h" @@ -59,7 +60,7 @@ template class StatelessWriterT : public Writer { SimpleHistoryCache m_history; }; -using StatelessWriter = StatelessWriterT; +using StatelessWriter = StatelessWriterT; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/platform/bootstrap.h b/components/rtps_embedded/include/rtps/platform/bootstrap.h new file mode 100644 index 000000000..3f803c58f --- /dev/null +++ b/components/rtps_embedded/include/rtps/platform/bootstrap.h @@ -0,0 +1,39 @@ +#ifndef RTPS_PLATFORM_BOOTSTRAP_H +#define RTPS_PLATFORM_BOOTSTRAP_H + +#include "lwip/err.h" +#include "lwip/sys.h" + +namespace rtps { +namespace platform { +namespace bootstrap { + +using InitSemaphoreHandle = sys_sem_t; + +inline bool createInitSemaphore(InitSemaphoreHandle *sem) { + return sem != nullptr && sys_sem_new(sem, 0) == ERR_OK; +} + +inline void signalInitSemaphore(InitSemaphoreHandle *sem) { + if (sem != nullptr) { + sys_sem_signal(sem); + } +} + +inline void waitInitSemaphore(InitSemaphoreHandle *sem) { + if (sem != nullptr) { + sys_sem_wait(sem); + } +} + +inline void freeInitSemaphore(InitSemaphoreHandle *sem) { + if (sem != nullptr) { + sys_sem_free(sem); + } +} + +} // namespace bootstrap +} // namespace platform +} // namespace rtps + +#endif // RTPS_PLATFORM_BOOTSTRAP_H diff --git a/components/rtps_embedded/include/rtps/platform/platform_types.h b/components/rtps_embedded/include/rtps/platform/platform_types.h new file mode 100644 index 000000000..00de3a327 --- /dev/null +++ b/components/rtps_embedded/include/rtps/platform/platform_types.h @@ -0,0 +1,44 @@ +#ifndef RTPS_PLATFORM_TYPES_H +#define RTPS_PLATFORM_TYPES_H + +#include +#include +#ifdef __cplusplus +#include +#endif + +#include "lwip/ip4_addr.h" +#include "lwip/netif.h" +#include "lwip/sys.h" + +namespace rtps { +namespace platform { + +using Ip4Address = ip4_addr_t; +using SemaphoreHandle = sys_sem_t; +using ThreadHandle = sys_thread_t; +#ifdef __cplusplus +using Mutex = std::recursive_mutex; +#endif + +inline bool tryGetDefaultIp4AddressBytes(std::array &ip) { + if (netif_default == nullptr) { + return false; + } + + const ip4_addr_t *iface_ip = netif_ip4_addr(netif_default); + if (iface_ip == nullptr) { + return false; + } + + ip[0] = ip4_addr1(iface_ip); + ip[1] = ip4_addr2(iface_ip); + ip[2] = ip4_addr3(iface_ip); + ip[3] = ip4_addr4(iface_ip); + return true; +} + +} // namespace platform +} // namespace rtps + +#endif // RTPS_PLATFORM_TYPES_H diff --git a/components/rtps_embedded/include/rtps/platform/sync.h b/components/rtps_embedded/include/rtps/platform/sync.h new file mode 100644 index 000000000..4e966e415 --- /dev/null +++ b/components/rtps_embedded/include/rtps/platform/sync.h @@ -0,0 +1,26 @@ +#ifndef RTPS_PLATFORM_SYNC_H +#define RTPS_PLATFORM_SYNC_H + +#if defined(ESP_PLATFORM) +#include "freertos/FreeRTOS.h" +#include "freertos/semphr.h" +#else +#include "FreeRTOS.h" +#include "semphr.h" +#endif + +namespace rtps { +namespace platform { +namespace sync { + +using RecursiveMutexHandle = SemaphoreHandle_t; + +bool createRecursiveMutex(RecursiveMutexHandle *mutex); +bool lockRecursive(RecursiveMutexHandle mutex); +bool unlockRecursive(RecursiveMutexHandle mutex); + +} // namespace sync +} // namespace platform +} // namespace rtps + +#endif // RTPS_PLATFORM_SYNC_H diff --git a/components/rtps_embedded/include/rtps/platform/threading.h b/components/rtps_embedded/include/rtps/platform/threading.h new file mode 100644 index 000000000..0d4a45667 --- /dev/null +++ b/components/rtps_embedded/include/rtps/platform/threading.h @@ -0,0 +1,52 @@ +#ifndef RTPS_PLATFORM_THREADING_H +#define RTPS_PLATFORM_THREADING_H + +#include "lwip/err.h" +#include "lwip/sys.h" + +namespace rtps { +namespace platform { +namespace threading { + +using ThreadHandle = sys_thread_t; +using SemaphoreHandle = sys_sem_t; +using ThreadFunction = lwip_thread_fn; + +inline bool createSemaphore(SemaphoreHandle *sem, u8_t initial_count = 0) { + return sem != nullptr && sys_sem_new(sem, initial_count) == ERR_OK; +} + +inline bool isSemaphoreValid(SemaphoreHandle *sem) { + return sem != nullptr && sys_sem_valid(sem) != 0; +} + +inline void freeSemaphore(SemaphoreHandle *sem) { + if (sem != nullptr) { + sys_sem_free(sem); + } +} + +inline void signalSemaphore(SemaphoreHandle *sem) { + if (sem != nullptr) { + sys_sem_signal(sem); + } +} + +inline void waitSemaphore(SemaphoreHandle *sem) { + if (sem != nullptr) { + sys_sem_wait(sem); + } +} + +inline ThreadHandle startThread(const char *name, ThreadFunction function, + void *arg, int stacksize, int prio) { + return sys_thread_new(name, function, arg, stacksize, prio); +} + +inline void sleepMs(u32_t milliseconds) { sys_msleep(milliseconds); } + +} // namespace threading +} // namespace platform +} // namespace rtps + +#endif // RTPS_PLATFORM_THREADING_H diff --git a/components/rtps_embedded/include/rtps/platform/transport.h b/components/rtps_embedded/include/rtps/platform/transport.h new file mode 100644 index 000000000..05cdf48e7 --- /dev/null +++ b/components/rtps_embedded/include/rtps/platform/transport.h @@ -0,0 +1,27 @@ +#ifndef RTPS_PLATFORM_TRANSPORT_H +#define RTPS_PLATFORM_TRANSPORT_H + +#include "rtps/common/types.h" +#include "rtps/platform/platform_types.h" + +namespace rtps { +struct PacketInfo; +struct UdpConnection; + +namespace platform { +namespace transport { + +class ITransport { +public: + virtual ~ITransport() = default; + + virtual const UdpConnection *createUdpConnection(Ip4Port_t receivePort) = 0; + virtual bool joinMultiCastGroup(platform::Ip4Address addr) const = 0; + virtual void sendPacket(PacketInfo &info) = 0; +}; + +} // namespace transport +} // namespace platform +} // namespace rtps + +#endif // RTPS_PLATFORM_TRANSPORT_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h index 4f4f7661e..26af5357d 100644 --- a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h @@ -25,12 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_THREADSAFEQUEUE_H #define RTPS_THREADSAFEQUEUE_H -#include "lwip/sys.h" -#if defined(ESP_PLATFORM) -#include "freertos/semphr.h" -#else -#include "semphr.h" -#endif +#include "rtps/platform/sync.h" #include #include @@ -67,7 +62,7 @@ template class ThreadSafeCircularBuffer { static_assert(SIZE + 1 < std::numeric_limits::max(), "Iterator is large enough for given size"); - SemaphoreHandle_t m_mutex = nullptr; + platform::sync::RecursiveMutexHandle m_mutex = nullptr; bool m_initialized = false; inline bool isFull(); diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp index be1d7e843..51fb9a3de 100644 --- a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp @@ -23,7 +23,7 @@ bool ThreadSafeCircularBuffer::init() { if (m_initialized) { return true; } - if (!createMutex(&m_mutex)) { + if (!platform::sync::createRecursiveMutex(&m_mutex)) { TSCB_LOG("Failed to create mutex \n"); return false; } else { diff --git a/components/rtps_embedded/include/rtps/utils/Lock.h b/components/rtps_embedded/include/rtps/utils/Lock.h index 7a3739213..4ab6bb70c 100644 --- a/components/rtps_embedded/include/rtps/utils/Lock.h +++ b/components/rtps_embedded/include/rtps/utils/Lock.h @@ -25,37 +25,30 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_LOCK_H #define RTPS_LOCK_H -#if defined(ESP_PLATFORM) -#include "freertos/FreeRTOS.h" -#include "freertos/semphr.h" -#else -#include "FreeRTOS.h" -#include "semphr.h" -#endif -#include "lwip/sys.h" +#include "rtps/platform/sync.h" namespace rtps { +using RecursiveMutexHandle = platform::sync::RecursiveMutexHandle; + class Lock { public: - explicit Lock(SemaphoreHandle_t &mutex) : m_mutex(mutex) { - if (m_mutex != nullptr) { - m_locked = (xSemaphoreTakeRecursive(m_mutex, portMAX_DELAY) == pdTRUE); - } + explicit Lock(RecursiveMutexHandle &mutex) : m_mutex(mutex) { + m_locked = platform::sync::lockRecursive(m_mutex); }; ~Lock() { - if (m_locked && m_mutex != nullptr) { - xSemaphoreGiveRecursive(m_mutex); + if (m_locked) { + platform::sync::unlockRecursive(m_mutex); } }; private: - SemaphoreHandle_t m_mutex = nullptr; + RecursiveMutexHandle m_mutex = nullptr; bool m_locked = false; }; -bool createMutex(SemaphoreHandle_t *mutex); +bool createMutex(RecursiveMutexHandle *mutex); } // namespace rtps #endif // RTPS_LOCK_H diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp index aca048b9f..934606a8f 100644 --- a/components/rtps_embedded/src/ThreadPool.cpp +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -52,10 +52,10 @@ ThreadPool::ThreadPool(receiveJumppad_fp receiveCallback, void *callee) !m_incomingMetaTraffic.init() || !m_incomingUserTraffic.init()) { return; } - err_t inputErr = sys_sem_new(&m_readerNotificationSem, 0); - err_t outputErr = sys_sem_new(&m_writerNotificationSem, 0); + bool inputOk = platform::threading::createSemaphore(&m_readerNotificationSem, 0); + bool outputOk = platform::threading::createSemaphore(&m_writerNotificationSem, 0); - if (inputErr != ERR_OK || outputErr != ERR_OK) { + if (!inputOk || !outputOk) { THREAD_POOL_LOG("ThreadPool: Failed to create Semaphores.\n"); } } @@ -63,14 +63,14 @@ ThreadPool::ThreadPool(receiveJumppad_fp receiveCallback, void *callee) ThreadPool::~ThreadPool() { if (m_running) { stopThreads(); - sys_msleep(500); + platform::threading::sleepMs(500); } - if (sys_sem_valid(&m_readerNotificationSem)) { - sys_sem_free(&m_readerNotificationSem); + if (platform::threading::isSemaphoreValid(&m_readerNotificationSem)) { + platform::threading::freeSemaphore(&m_readerNotificationSem); } - if (sys_sem_valid(&m_writerNotificationSem)) { - sys_sem_free(&m_writerNotificationSem); + if (platform::threading::isSemaphoreValid(&m_writerNotificationSem)) { + platform::threading::freeSemaphore(&m_writerNotificationSem); } } @@ -101,24 +101,24 @@ bool ThreadPool::startThreads() { if (m_running) { return true; } - if (!sys_sem_valid(&m_readerNotificationSem) || - !sys_sem_valid(&m_writerNotificationSem)) { + if (!platform::threading::isSemaphoreValid(&m_readerNotificationSem) || + !platform::threading::isSemaphoreValid(&m_writerNotificationSem)) { return false; } m_running = true; for (auto &thread : m_writers) { // TODO ID, err check, waitOnStop - thread = sys_thread_new("WriterThread", writerThreadFunction, this, - Config::THREAD_POOL_WRITER_STACKSIZE, - Config::THREAD_POOL_WRITER_PRIO); + thread = platform::threading::startThread( + "WriterThread", writerThreadFunction, this, + Config::THREAD_POOL_WRITER_STACKSIZE, Config::THREAD_POOL_WRITER_PRIO); } for (auto &thread : m_readers) { // TODO ID, err check, waitOnStop - thread = sys_thread_new("ReaderThread", readerThreadFunction, this, - Config::THREAD_POOL_READER_STACKSIZE, - Config::THREAD_POOL_READER_PRIO); + thread = platform::threading::startThread( + "ReaderThread", readerThreadFunction, this, + Config::THREAD_POOL_READER_STACKSIZE, Config::THREAD_POOL_READER_PRIO); } return true; } @@ -129,17 +129,17 @@ void ThreadPool::stopThreads() { // stuck before ended. for (auto &thread : m_writers) { (void)thread; - sys_sem_signal(&m_writerNotificationSem); - sys_msleep(10); + platform::threading::signalSemaphore(&m_writerNotificationSem); + platform::threading::sleepMs(10); } for (auto &thread : m_readers) { (void)thread; - sys_sem_signal(&m_readerNotificationSem); - sys_msleep(10); + platform::threading::signalSemaphore(&m_readerNotificationSem); + platform::threading::sleepMs(10); } // TODO make sure they have finished. Seems to be sufficient for tests. // Not sufficient if threads shall actually be stopped during runtime. - sys_msleep(10); + platform::threading::sleepMs(10); } void ThreadPool::clearQueues() { @@ -157,7 +157,7 @@ bool ThreadPool::addWorkload(Writer *workload) { res = m_outgoingUserTraffic.moveElementIntoBuffer(std::move(workload)); } if (res) { - sys_sem_signal(&m_writerNotificationSem); + platform::threading::signalSemaphore(&m_writerNotificationSem); } else { if(workload->isBuiltinEndpoint()){ rtps::Diagnostics::ThreadPool::dropped_outgoing_packets_metatraffic++; @@ -204,7 +204,7 @@ bool ThreadPool::addNewPacket(PacketInfo &&packet) { res = m_incomingUserTraffic.moveElementIntoBuffer(std::move(packet)); } if (res) { - sys_sem_signal(&m_readerNotificationSem); + platform::threading::signalSemaphore(&m_readerNotificationSem); } else { THREAD_POOL_LOG("failed to enqueue packet for port %u", static_cast(packet.destPort)); @@ -247,7 +247,7 @@ void ThreadPool::doWriterWork() { static_cast(Diagnostics::ThreadPool::processed_outgoing_usertraffic), static_cast(Diagnostics::ThreadPool::processed_outgoing_metatraffic)); updateDiagnostics(); - sys_sem_wait(&m_writerNotificationSem); + platform::threading::waitSemaphore(&m_writerNotificationSem); } } } @@ -318,7 +318,7 @@ void ThreadPool::doReaderWork() { static_cast(usertraffic), static_cast(metatraffic)); updateDiagnostics(); - sys_sem_wait(&m_readerNotificationSem); + platform::threading::waitSemaphore(&m_readerNotificationSem); } } diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps_embedded/src/communication/EsppTransport.cpp new file mode 100644 index 000000000..da54e4a6b --- /dev/null +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -0,0 +1,216 @@ +/* +The MIT License +Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +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 + +This file is part of embeddedRTPS. + +Author: i11 - Embedded Software, RWTH Aachen University +*/ + +#include "rtps/communication/EsppTransport.h" + +#include "lwip/ip_addr.h" +#include "lwip/ip4_addr.h" +#include "rtps/communication/PacketInfo.h" +#include "task.hpp" + +#include +#include +#include + +using rtps::EsppTransport; + +EsppTransport::EsppTransport(RxCallback callback, void *args) + : m_rxCallback(callback), m_callbackArgs(args) {} + +EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) { + for (auto &channel : m_channels) { + if (channel.in_use && channel.connection.port == port) { + return &channel; + } + } + return nullptr; +} + +const EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) const { + for (const auto &channel : m_channels) { + if (channel.in_use && channel.connection.port == port) { + return &channel; + } + } + return nullptr; +} + +std::string EsppTransport::ip4ToString(platform::Ip4Address addr) { + std::array buffer{}; + const char *result = ip4addr_ntoa_r(&addr, buffer.data(), + static_cast(buffer.size())); + if (result == nullptr) { + return "0.0.0.0"; + } + return std::string(result); +} + +bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { + if (!channel.socket) { + return false; + } + + espp::Task::BaseConfig task_config; + task_config.name = "rtps_rx_" + std::to_string(receivePort); + task_config.stack_size_bytes = Config::THREAD_POOL_READER_STACKSIZE; + task_config.priority = Config::THREAD_POOL_READER_PRIO; + + espp::UdpSocket::ReceiveConfig receive_config; + receive_config.port = receivePort; + receive_config.buffer_size = 4096; + receive_config.on_receive_callback = + [this, receivePort](std::vector &data, + const espp::Socket::Info &sender) + -> std::optional> { + onReceive(receivePort, data, sender); + return std::nullopt; + }; + + return channel.socket->start_receiving(task_config, receive_config); +} + +EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort) { + for (auto &channel : m_channels) { + if (channel.in_use) { + continue; + } + + espp::UdpSocket::Config socket_config; + socket_config.log_level = espp::Logger::Verbosity::WARN; + channel.socket = std::make_unique(socket_config); + if (!channel.socket || !channel.socket->is_valid()) { + channel.socket.reset(); + return nullptr; + } + + channel.connection.port = receivePort; + channel.in_use = true; + + if (!startReceiver(channel, receivePort)) { + channel.socket.reset(); + channel.connection.port = 0; + channel.in_use = false; + return nullptr; + } + + for (const auto &group : m_multicastGroups) { + (void)channel.socket->add_multicast_group(group); + } + return &channel; + } + return nullptr; +} + +void EsppTransport::onReceive(Ip4Port_t receivePort, std::vector &data, + const espp::Socket::Info &sender) const { + if (m_rxCallback == nullptr) { + return; + } + + pbuf *packet_buffer = pbuf_alloc(PBUF_TRANSPORT, + static_cast(data.size()), PBUF_POOL); + if (packet_buffer == nullptr) { + return; + } + + if (!data.empty() && + pbuf_take(packet_buffer, data.data(), static_cast(data.size())) != + ERR_OK) { + pbuf_free(packet_buffer); + return; + } + + udp_pcb pcb{}; + pcb.local_port = receivePort; + + ip_addr_t source_addr{}; + if (!ipaddr_aton(sender.address.c_str(), &source_addr)) { + ip_addr_set_any(IPADDR_TYPE_V4, &source_addr); + } + + m_rxCallback(m_callbackArgs, &pcb, packet_buffer, &source_addr, + static_cast(sender.port)); +} + +const rtps::UdpConnection *EsppTransport::createUdpConnection(Ip4Port_t receivePort) { + std::lock_guard lock(m_mutex); + + Channel *existing = findChannel(receivePort); + if (existing != nullptr) { + return &existing->connection; + } + + Channel *created = createChannel(receivePort); + return created != nullptr ? &created->connection : nullptr; +} + +bool EsppTransport::joinMultiCastGroup(platform::Ip4Address addr) const { + std::lock_guard lock(m_mutex); + + const std::string group = ip4ToString(addr); + for (const auto &existing : m_multicastGroups) { + if (existing == group) { + return true; + } + } + + bool any_joined = false; + for (auto &channel : m_channels) { + if (!channel.in_use || !channel.socket) { + continue; + } + any_joined = channel.socket->add_multicast_group(group) || any_joined; + } + + m_multicastGroups.push_back(group); + return any_joined || m_multicastGroups.size() == 1; +} + +void EsppTransport::sendPacket(PacketInfo &info) { + std::lock_guard lock(m_mutex); + + Channel *channel = findChannel(info.srcPort); + if (channel == nullptr) { + channel = createChannel(info.srcPort); + } + + if (channel == nullptr || !channel->socket || info.buffer.firstElement == nullptr) { + return; + } + + const size_t packet_size = info.buffer.firstElement->tot_len; + std::vector bytes(packet_size); + u16_t copied = pbuf_copy_partial(info.buffer.firstElement, bytes.data(), + static_cast(packet_size), 0); + if (copied != static_cast(packet_size)) { + return; + } + + espp::UdpSocket::SendConfig send_config; + send_config.ip_address = ip4ToString(info.destAddr); + send_config.port = info.destPort; + send_config.is_multicast_endpoint = ip4_addr_ismulticast(&info.destAddr) != 0; + + (void)channel->socket->send(bytes, send_config); +} diff --git a/components/rtps_embedded/src/communication/UdpDriver.cpp b/components/rtps_embedded/src/communication/UdpDriver.cpp index 3b14da023..6f6f31a5b 100644 --- a/components/rtps_embedded/src/communication/UdpDriver.cpp +++ b/components/rtps_embedded/src/communication/UdpDriver.cpp @@ -95,7 +95,7 @@ bool UdpDriver::isMulticastAddress(ip4_addr_t addr) { #endif } -bool UdpDriver::joinMultiCastGroup(ip4_addr_t addr) const { +bool UdpDriver::joinMultiCastGroup(platform::Ip4Address addr) const { err_t iret; { diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp index 9f287065a..3530d9ef1 100644 --- a/components/rtps_embedded/src/discovery/SPDPAgent.cpp +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -23,12 +23,12 @@ Author: i11 - Embedded Software, RWTH Aachen University */ #include "rtps/discovery/SPDPAgent.h" -#include "lwip/sys.h" #include "rtps/discovery/ParticipantProxyData.h" #include "rtps/entities/Participant.h" #include "rtps/entities/Reader.h" #include "rtps/entities/Writer.h" #include "rtps/messages/MessageTypes.h" +#include "rtps/platform/threading.h" #include "rtps/utils/Log.h" #include "rtps/utils/udpUtils.h" @@ -57,9 +57,9 @@ void SPDPAgent::start() { return; } m_running = true; - auto t = - sys_thread_new("SPDPThread", runBroadcast, this, - Config::SPDP_WRITER_STACKSIZE, Config::SPDP_WRITER_PRIO); + (void)platform::threading::startThread( + "SPDPThread", runBroadcast, this, Config::SPDP_WRITER_STACKSIZE, + Config::SPDP_WRITER_PRIO); } void SPDPAgent::stop() { m_running = false; } @@ -74,7 +74,7 @@ void SPDPAgent::runBroadcast(void *args) { #ifdef OS_IS_FREERTOS vTaskDelay(pdMS_TO_TICKS(Config::SPDP_RESEND_PERIOD_MS)); #else - sys_msleep(Config::SPDP_RESEND_PERIOD_MS); + platform::threading::sleepMs(Config::SPDP_RESEND_PERIOD_MS); #endif // StatelessWriter drops already-sent history; enqueue a fresh SPDP sample // for each announce cycle. diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps_embedded/src/entities/Domain.cpp index ac6563816..2d9b9f6e4 100644 --- a/components/rtps_embedded/src/entities/Domain.cpp +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/Domain.h" #include "rtps/utils/Log.h" #include "rtps/utils/udpUtils.h" +#include #if defined(ESP_PLATFORM) #include "esp_mac.h" @@ -45,13 +46,27 @@ using rtps::Domain; Domain::Domain() : m_threadPool(receiveJumppad, this), - m_transport(ThreadPool::readCallback, &m_threadPool) { - m_transport.createUdpConnection(getUserMulticastPort()); - m_transport.createUdpConnection(getBuiltInMulticastPort()); - m_transport.joinMultiCastGroup(transformIP4ToU32(239, 255, 0, 1)); + m_defaultTransport(ThreadPool::readCallback, &m_threadPool), + m_transport(&m_defaultTransport) { + initializeTransport(); createMutex(&m_mutex); } +Domain::Domain(platform::transport::ITransport &transport) + : m_threadPool(receiveJumppad, this), + m_defaultTransport(ThreadPool::readCallback, &m_threadPool), + m_transport(&transport) { + initializeTransport(); + createMutex(&m_mutex); +} + +void Domain::initializeTransport() { + assert(m_transport != nullptr); + m_transport->createUdpConnection(getUserMulticastPort()); + m_transport->createUdpConnection(getBuiltInMulticastPort()); + m_transport->joinMultiCastGroup(transformIP4ToU32(239, 255, 0, 1)); +} + Domain::~Domain() { stop(); } bool Domain::completeInit() { @@ -163,7 +178,7 @@ void Domain::createBuiltinWritersAndReaders(Participant &part) { spdpWriterAttributes.unicastLocator = getBuiltInMulticastLocator(); spdpWriter->init(spdpWriterAttributes, TopicKind_t::WITH_KEY, &m_threadPool, - m_transport); + *m_transport); spdpWriter->addNewMatchedReader( ReaderProxy{{part.m_guidPrefix, ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER}, getBuiltInMulticastLocator(), @@ -192,14 +207,14 @@ void Domain::createBuiltinWritersAndReaders(Participant &part) { m_statefulReaders); sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER; - sedpPubReader->init(sedpAttributes, m_transport); + sedpPubReader->init(sedpAttributes, *m_transport); StatefulReader *sedpSubReader = getNextUnusedEndpoint( m_statefulReaders); sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER; - sedpSubReader->init(sedpAttributes, m_transport); + sedpSubReader->init(sedpAttributes, *m_transport); // WRITER StatefulWriter *sedpPubWriter = @@ -208,7 +223,7 @@ void Domain::createBuiltinWritersAndReaders(Participant &part) { sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER; sedpPubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, &m_threadPool, - m_transport); + *m_transport); StatefulWriter *sedpSubWriter = getNextUnusedEndpoint( @@ -216,7 +231,7 @@ void Domain::createBuiltinWritersAndReaders(Participant &part) { sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER; sedpSubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, &m_threadPool, - m_transport); + *m_transport); // COLLECT BuiltInEndpoints endpoints{}; @@ -231,14 +246,14 @@ void Domain::createBuiltinWritersAndReaders(Participant &part) { } void Domain::registerPort(const Participant &part) { - m_transport.createUdpConnection(getUserUnicastPort(part.m_participantId)); - m_transport.createUdpConnection(getBuiltInUnicastPort(part.m_participantId)); + m_transport->createUdpConnection(getUserUnicastPort(part.m_participantId)); + m_transport->createUdpConnection(getBuiltInUnicastPort(part.m_participantId)); m_threadPool.addBuiltinPort(getBuiltInUnicastPort(part.m_participantId)); } void Domain::registerMulticastPort(FullLengthLocator mcastLocator) { if (mcastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { - m_transport.createUdpConnection(mcastLocator.getLocatorPort()); + m_transport->createUdpConnection(mcastLocator.getLocatorPort()); } } @@ -373,7 +388,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; if (!statefulWriter->init(attributes, TopicKind_t::NO_KEY, &m_threadPool, - m_transport, enforceUnicast)) { + *m_transport, enforceUnicast)) { DOMAIN_LOG("StatefulWriter init failed.\n"); return nullptr; } @@ -386,7 +401,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, attributes.reliabilityKind = ReliabilityKind_t::BEST_EFFORT; if (!statelessWriter->init(attributes, TopicKind_t::NO_KEY, &m_threadPool, - m_transport, enforceUnicast)) { + *m_transport, enforceUnicast)) { DOMAIN_LOG("StatelessWriter init failed.\n"); return nullptr; } @@ -437,7 +452,7 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, ip4_addr1(&mcastaddress), ip4_addr2(&mcastaddress), ip4_addr3(&mcastaddress), ip4_addr4(&mcastaddress), getUserMulticastPort()); - m_transport.joinMultiCastGroup( + m_transport->joinMultiCastGroup( attributes.multicastLocator.getIp4Address()); registerMulticastPort(attributes.multicastLocator); @@ -456,7 +471,7 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; - statefulReader->init(attributes, m_transport); + statefulReader->init(attributes, *m_transport); if (!part.addReader(statefulReader)) { DOMAIN_LOG("Failed to add reader to participant.\n"); diff --git a/components/rtps_embedded/src/platform/sync.cpp b/components/rtps_embedded/src/platform/sync.cpp new file mode 100644 index 000000000..3299dcb8b --- /dev/null +++ b/components/rtps_embedded/src/platform/sync.cpp @@ -0,0 +1,31 @@ +#include "rtps/platform/sync.h" + +namespace rtps { +namespace platform { +namespace sync { + +bool createRecursiveMutex(RecursiveMutexHandle *mutex) { + if (mutex == nullptr) { + return false; + } + *mutex = xSemaphoreCreateRecursiveMutex(); + return *mutex != nullptr; +} + +bool lockRecursive(RecursiveMutexHandle mutex) { + if (mutex == nullptr) { + return false; + } + return xSemaphoreTakeRecursive(mutex, portMAX_DELAY) == pdTRUE; +} + +bool unlockRecursive(RecursiveMutexHandle mutex) { + if (mutex == nullptr) { + return false; + } + return xSemaphoreGiveRecursive(mutex) == pdTRUE; +} + +} // namespace sync +} // namespace platform +} // namespace rtps diff --git a/components/rtps_embedded/src/rtps.cpp b/components/rtps_embedded/src/rtps.cpp index 67c14094d..e086fd933 100644 --- a/components/rtps_embedded/src/rtps.cpp +++ b/components/rtps_embedded/src/rtps.cpp @@ -23,7 +23,7 @@ Author: i11 - Embedded Software, RWTH Aachen University */ #include "rtps/rtps.h" -#include "lwip/sys.h" +#include "rtps/platform/bootstrap.h" #include #include @@ -58,7 +58,7 @@ static void init(void *arg) { #endif return; } - auto init_sem = static_cast(arg); + auto init_sem = static_cast(arg); srand((unsigned int)time(nullptr)); @@ -85,24 +85,23 @@ static void init(void *arg) { netif_set_default(&netif); netif_set_up(netif_default); - sys_sem_signal(init_sem); + platform::bootstrap::signalInitSemaphore(init_sem); } void LwIPInit() { /* no stdio-buffering, please! */ setvbuf(stdout, nullptr, _IONBF, 0); - err_t err; - sys_sem_t init_sem; + platform::bootstrap::InitSemaphoreHandle init_sem; - err = sys_sem_new(&init_sem, 0); - LWIP_ASSERT("failed to create init_sem", err == ERR_OK); - LWIP_UNUSED_ARG(err); + bool sem_created = platform::bootstrap::createInitSemaphore(&init_sem); + LWIP_ASSERT("failed to create init_sem", sem_created); + LWIP_UNUSED_ARG(sem_created); tcpip_init(init, &init_sem); /* we have to wait for initialization to finish before * calling update_adapter()! */ - sys_sem_wait(&init_sem); - sys_sem_free(&init_sem); + platform::bootstrap::waitInitSemaphore(&init_sem); + platform::bootstrap::freeInitSemaphore(&init_sem); } void rtps::init() { diff --git a/components/rtps_embedded/src/utils/Lock.cpp b/components/rtps_embedded/src/utils/Lock.cpp index d3b48fc68..f5199a308 100644 --- a/components/rtps_embedded/src/utils/Lock.cpp +++ b/components/rtps_embedded/src/utils/Lock.cpp @@ -1,13 +1,19 @@ #include "rtps/utils/Lock.h" +#include + namespace rtps { -bool createMutex(SemaphoreHandle_t *mutex) { - *mutex = xSemaphoreCreateRecursiveMutex(); - if (*mutex != NULL) { +bool createMutex(platform::sync::RecursiveMutexHandle *mutex) { + if (mutex == nullptr) { + assert(false && "Mutex pointer is null"); + return false; + } + + if (platform::sync::createRecursiveMutex(mutex)) { return true; } else { - LWIP_ASSERT("Mutex creation failed", true); + assert(false && "Mutex creation failed"); return false; } } From c223c55be0a709ead05d7784d98d4cdc73697d9b Mon Sep 17 00:00:00 2001 From: Guo Date: Fri, 10 Jul 2026 16:22:59 -0500 Subject: [PATCH 06/17] added thread pool; removed lwip and freertos --- components/rtps_embedded/CMakeLists.txt | 9 +- .../rtps_embedded/example/main/main.cpp | 15 +- .../rtps_embedded/include/rtps/ThreadPool.h | 32 ++-- .../include/rtps/common/Locator.h | 59 ++++--- .../rtps/communication/EsppTransport.h | 18 +- .../include/rtps/communication/PacketInfo.h | 11 +- .../rtps/communication/TcpipCoreLock.h | 38 ---- .../include/rtps/communication/UdpDriver.h | 66 ------- .../rtps/discovery/ParticipantProxyData.h | 28 +-- .../include/rtps/discovery/SEDPAgent.h | 5 +- .../include/rtps/discovery/SPDPAgent.h | 10 +- .../include/rtps/entities/Domain.h | 18 +- .../include/rtps/entities/Participant.h | 11 +- .../include/rtps/entities/Reader.h | 13 +- .../include/rtps/entities/StatefulReader.h | 2 +- .../include/rtps/entities/StatefulReader.tpp | 48 ++++-- .../include/rtps/entities/StatefulWriter.h | 7 +- .../include/rtps/entities/StatefulWriter.tpp | 135 +++++++++------ .../include/rtps/entities/StatelessWriter.h | 3 - .../include/rtps/entities/StatelessWriter.tpp | 31 ++-- .../include/rtps/entities/Writer.h | 8 +- .../include/rtps/messages/MessageFactory.h | 4 +- .../include/rtps/platform/bootstrap.h | 30 +++- .../include/rtps/platform/platform_types.h | 27 +-- .../include/rtps/platform/sync.h | 10 +- .../include/rtps/platform/threading.h | 52 ------ .../include/rtps/platform/transport.h | 15 +- components/rtps_embedded/include/rtps/rtps.h | 6 - .../include/rtps/storages/CacheChange.h | 14 +- .../include/rtps/storages/PBufWrapper.h | 84 --------- .../PayloadBuffer.h} | 62 +++---- .../include/rtps/utils/sysFunctions.h | 15 +- .../include/rtps/utils/udpUtils.h | 17 +- components/rtps_embedded/src/ThreadPool.cpp | 154 ++++++++--------- .../src/communication/EsppTransport.cpp | 108 ++++++------ .../src/communication/UdpDriver.cpp | 153 ---------------- .../src/discovery/ParticipantProxyData.cpp | 26 ++- .../rtps_embedded/src/discovery/SPDPAgent.cpp | 74 ++++---- .../rtps_embedded/src/entities/Domain.cpp | 74 ++++---- .../src/entities/Participant.cpp | 16 +- .../rtps_embedded/src/entities/Writer.cpp | 8 +- .../rtps_embedded/src/platform/sync.cpp | 13 +- components/rtps_embedded/src/rtps.cpp | 118 ------------- .../src/storages/PBufWrapper.cpp | 163 ------------------ components/socket/CMakeLists.txt | 2 +- components/thread_pool/CMakeLists.txt | 4 + components/thread_pool/README.md | 13 ++ components/thread_pool/example/CMakeLists.txt | 22 +++ components/thread_pool/example/README.md | 17 ++ .../thread_pool/example/main/CMakeLists.txt | 2 + .../example/main/thread_pool_example.cpp | 67 +++++++ .../thread_pool/example/sdkconfig.defaults | 1 + components/thread_pool/idf_component.yml | 21 +++ .../thread_pool/include/thread_pool.hpp | 81 +++++++++ components/thread_pool/src/thread_pool.cpp | 150 ++++++++++++++++ 55 files changed, 1003 insertions(+), 1187 deletions(-) delete mode 100644 components/rtps_embedded/include/rtps/communication/TcpipCoreLock.h delete mode 100644 components/rtps_embedded/include/rtps/communication/UdpDriver.h delete mode 100644 components/rtps_embedded/include/rtps/platform/threading.h delete mode 100644 components/rtps_embedded/include/rtps/storages/PBufWrapper.h rename components/rtps_embedded/include/rtps/{communication/UdpConnection.h => storages/PayloadBuffer.h} (59%) delete mode 100644 components/rtps_embedded/src/communication/UdpDriver.cpp delete mode 100644 components/rtps_embedded/src/rtps.cpp delete mode 100644 components/rtps_embedded/src/storages/PBufWrapper.cpp create mode 100644 components/thread_pool/CMakeLists.txt create mode 100644 components/thread_pool/README.md create mode 100644 components/thread_pool/example/CMakeLists.txt create mode 100644 components/thread_pool/example/README.md create mode 100644 components/thread_pool/example/main/CMakeLists.txt create mode 100644 components/thread_pool/example/main/thread_pool_example.cpp create mode 100644 components/thread_pool/example/sdkconfig.defaults create mode 100644 components/thread_pool/idf_component.yml create mode 100644 components/thread_pool/include/thread_pool.hpp create mode 100644 components/thread_pool/src/thread_pool.cpp diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt index aebe80af1..e8d158f38 100644 --- a/components/rtps_embedded/CMakeLists.txt +++ b/components/rtps_embedded/CMakeLists.txt @@ -1,9 +1,7 @@ idf_component_register( SRCS - "src/rtps.cpp" "src/ThreadPool.cpp" "src/communication/EsppTransport.cpp" - "src/communication/UdpDriver.cpp" "src/discovery/ParticipantProxyData.cpp" "src/discovery/SEDPAgent.cpp" "src/discovery/SPDPAgent.cpp" @@ -16,7 +14,6 @@ idf_component_register( "src/messages/MessageReceiver.cpp" "src/messages/MessageTypes.cpp" "src/platform/sync.cpp" - "src/storages/PBufWrapper.cpp" "src/utils/Diagnostics.cpp" "src/utils/Lock.cpp" "thirdparty/Micro-CDR/src/c/common.c" @@ -28,12 +25,8 @@ idf_component_register( "include" "thirdparty/Micro-CDR/include" REQUIRES - base_component cdr task socket lwip freertos + base_component cdr task thread_pool socket ) -if(DEFINED RTPS_USE_ESPP_TRANSPORT AND RTPS_USE_ESPP_TRANSPORT) - target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_USE_ESPP_TRANSPORT=1) -endif() - # target_compile_features(${COMPONENT_LIB} PUBLIC cxx_std_17) # target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_CONFIG_HEADER=\"rtps/config_esp32.h\") diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp index 09ddab53b..5850b585c 100644 --- a/components/rtps_embedded/example/main/main.cpp +++ b/components/rtps_embedded/example/main/main.cpp @@ -92,7 +92,8 @@ void publisher_task(void *arg) { extern "C" void embedded_rtps_start(const char *node_name, const char *pub_topic, - const char *sub_topic) { + const char *sub_topic, + const rtps::platform::transport::Ip4AddressBytes &local_ip) { if (s_started) { return; } @@ -105,7 +106,7 @@ extern "C" void embedded_rtps_start(const char *node_name, strncpy(s_node_name, node_name, sizeof(s_node_name) - 1); s_node_name[sizeof(s_node_name) - 1] = '\0'; - static rtps::Domain domain; + static rtps::Domain domain(local_ip); s_domain = &domain; s_participant = s_domain->createParticipant(); @@ -168,7 +169,15 @@ extern "C" void app_main(void) } logger.info("WiFi connected, local IP {}", ip_address); - embedded_rtps_start("DHCPS", "rtps_embedded_pub", "rtps_embedded_sub"); + rtps::platform::transport::Ip4AddressBytes local_ip{0, 0, 0, 0}; + if (std::sscanf(ip_address.c_str(), "%hhu.%hhu.%hhu.%hhu", &local_ip[0], + &local_ip[1], &local_ip[2], &local_ip[3]) != 4) { + logger.error("Failed to parse local IP {}", ip_address); + return; + } + + embedded_rtps_start("DHCPS", "rtps_embedded_pub", "rtps_embedded_sub", + local_ip); while (true) { vTaskDelay(pdMS_TO_TICKS(1000)); } diff --git a/components/rtps_embedded/include/rtps/ThreadPool.h b/components/rtps_embedded/include/rtps/ThreadPool.h index 534bccbf3..8ca6dcc2b 100644 --- a/components/rtps_embedded/include/rtps/ThreadPool.h +++ b/components/rtps_embedded/include/rtps/ThreadPool.h @@ -26,18 +26,26 @@ Author: i11 - Embedded Software, RWTH Aachen University #define RTPS_THREADPOOL_H #include "rtps/communication/PacketInfo.h" -#include "rtps/communication/UdpDriver.h" #include "rtps/config.h" -#include "rtps/platform/threading.h" -#include "rtps/storages/PBufWrapper.h" +#include "rtps/platform/transport.h" #include "rtps/storages/ThreadSafeCircularBuffer.h" #include +#include +#include namespace rtps { class Writer; +} // namespace rtps + +namespace espp { +class ThreadPool; +} + +namespace rtps { + class ThreadPool { public: using receiveJumppad_fp = void (*)(void *callee, const PacketInfo &packet); @@ -53,8 +61,10 @@ class ThreadPool { bool addWorkload(Writer *workload); bool addNewPacket(PacketInfo &&packet); - static void readCallback(void *arg, udp_pcb *pcb, pbuf *p, - const ip_addr_t *addr, Ip4Port_t port); + static void onDatagram( + void *arg, const uint8_t *data, std::size_t size, Ip4Port_t localPort, + Ip4Port_t remotePort, + const platform::transport::Ip4AddressBytes &remoteAddress); bool addBuiltinPort(const Ip4Port_t &port); @@ -62,15 +72,12 @@ class ThreadPool { receiveJumppad_fp m_receiveJumppad; void *m_callee; bool m_running = false; - std::array m_writers; - std::array m_readers; + std::unique_ptr m_writerWorkers; + std::unique_ptr m_readerWorkers; std::array m_builtinPorts; size_t m_builtinPortsIdx = 0; - platform::threading::SemaphoreHandle m_readerNotificationSem; - platform::threading::SemaphoreHandle m_writerNotificationSem; - void updateDiagnostics(); using BufferUsertrafficOutgoing = ThreadSafeCircularBuffer< @@ -88,9 +95,10 @@ class ThreadPool { BufferUsertrafficIncoming m_incomingUserTraffic; BufferMetatrafficIncoming m_incomingMetaTraffic; + void scheduleWriterWork(); + void scheduleReaderWork(); + bool isBuiltinPort(const Ip4Port_t &port); - static void writerThreadFunction(void *arg); - static void readerThreadFunction(void *arg); void doWriterWork(); void doReaderWork(); }; diff --git a/components/rtps_embedded/include/rtps/common/Locator.h b/components/rtps_embedded/include/rtps/common/Locator.h index 08f7effed..f2b3b023f 100644 --- a/components/rtps_embedded/include/rtps/common/Locator.h +++ b/components/rtps_embedded/include/rtps/common/Locator.h @@ -25,8 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_LOCATOR_T_H #define RTPS_LOCATOR_T_H -#include "rtps/communication/UdpDriver.h" -#include "rtps/config.h" +#include "rtps/common/types.h" #include "rtps/platform/platform_types.h" #include "rtps/utils/udpUtils.h" #include "ucdr/microcdr.h" @@ -35,11 +34,10 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { -inline std::array getLocalIp4AddressBytes() { - std::array ip = {Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], - Config::IP_ADDRESS[2], Config::IP_ADDRESS[3]}; - (void)platform::tryGetDefaultIp4AddressBytes(ip); - return ip; +inline bool isSameSubnetAddress( + const platform::Ip4Address &addr, + const std::array &local) { + return addr[0] == local[0] && addr[1] == local[1] && addr[2] == local[2]; } enum class LocatorKind_t : int32_t { @@ -100,17 +98,20 @@ struct FullLengthLocator { address[15]); } - bool isSameAddress(platform::Ip4Address *address) { - platform::Ip4Address ownaddress = getIp4Address(); - return ip4_addr_cmp(&ownaddress, address); + std::array getIp4AddressBytes() const { + return {address[12], address[13], address[14], address[15]}; } - inline bool isSameSubnet() const { - return UdpDriver::isSameSubnet(getIp4Address()); + bool isSameAddress(const platform::Ip4Address &ipAddress) { + return getIp4Address() == ipAddress; + } + + inline bool isSameSubnet(const std::array &localIp) const { + return isSameSubnetAddress(getIp4Address(), localIp); } inline bool isMulticastAddress() const { - return UdpDriver::isMulticastAddress(getIp4Address()); + return rtps::isMulticastAddress(getIp4Address()); } inline uint32_t getLocatorPort() { return static_cast(port); } @@ -118,10 +119,11 @@ struct FullLengthLocator { } __attribute__((packed)); inline FullLengthLocator -getBuiltInUnicastLocator(ParticipantId_t participantId) { - const auto ip = getLocalIp4AddressBytes(); +getBuiltInUnicastLocator(ParticipantId_t participantId, + const std::array &localIp) { return FullLengthLocator::createUDPv4Locator( - ip[0], ip[1], ip[2], ip[3], getBuiltInUnicastPort(participantId)); + localIp[0], localIp[1], localIp[2], localIp[3], + getBuiltInUnicastPort(participantId)); } inline FullLengthLocator getBuiltInMulticastLocator() { @@ -129,18 +131,21 @@ inline FullLengthLocator getBuiltInMulticastLocator() { getBuiltInMulticastPort()); } -inline FullLengthLocator getUserUnicastLocator(ParticipantId_t participantId) { - const auto ip = getLocalIp4AddressBytes(); +inline FullLengthLocator +getUserUnicastLocator(ParticipantId_t participantId, + const std::array &localIp) { return FullLengthLocator::createUDPv4Locator( - ip[0], ip[1], ip[2], ip[3], getUserUnicastPort(participantId)); + localIp[0], localIp[1], localIp[2], localIp[3], + getUserUnicastPort(participantId)); } inline FullLengthLocator -getUserMulticastLocator() { // this would be a unicastaddress, as - // defined in config - const auto ip = getLocalIp4AddressBytes(); +getUserMulticastLocator( + const std::array &localIp) { // this would be a unicastaddress, + // as defined in config return FullLengthLocator::createUDPv4Locator( - ip[0], ip[1], ip[2], ip[3], getUserMulticastPort()); + localIp[0], localIp[1], localIp[2], localIp[3], + getUserMulticastPort()); } inline FullLengthLocator getDefaultSendMulticastLocator() { @@ -170,16 +175,18 @@ struct LocatorIPv4 { return transformIP4ToU32(address[0], address[1], address[2], address[3]); } + std::array getIp4AddressBytes() const { return address; } + void setInvalid() { kind = LocatorKind_t::LOCATOR_KIND_INVALID; } bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; } - inline bool isSameSubnet() const { - return UdpDriver::isSameSubnet(getIp4Address()); + inline bool isSameSubnet(const std::array &localIp) const { + return isSameSubnetAddress(getIp4Address(), localIp); } inline bool isMulticastAddress() const { - return UdpDriver::isMulticastAddress(getIp4Address()); + return rtps::isMulticastAddress(getIp4Address()); } }; diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.h b/components/rtps_embedded/include/rtps/communication/EsppTransport.h index 5c54af806..03af218c7 100644 --- a/components/rtps_embedded/include/rtps/communication/EsppTransport.h +++ b/components/rtps_embedded/include/rtps/communication/EsppTransport.h @@ -25,9 +25,6 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_ESPPTRANSPORT_H #define RTPS_ESPPTRANSPORT_H -#include "lwip/pbuf.h" -#include "lwip/udp.h" -#include "rtps/communication/UdpConnection.h" #include "rtps/config.h" #include "rtps/platform/transport.h" #include "udp_socket.hpp" @@ -42,20 +39,19 @@ namespace rtps { class EsppTransport : public platform::transport::ITransport { public: - using RxCallback = - void (*)(void *arg, udp_pcb *pcb, pbuf *p, const ip_addr_t *addr, - Ip4Port_t port); + using RxCallback = platform::transport::ReceiveCallback; EsppTransport(RxCallback callback, void *args); ~EsppTransport() override = default; - const rtps::UdpConnection *createUdpConnection(Ip4Port_t receivePort) override; - bool joinMultiCastGroup(platform::Ip4Address addr) const override; + bool ensureReceivePort(Ip4Port_t receivePort) override; + bool joinMultiCastGroup( + const platform::transport::Ip4AddressBytes &addr) const override; void sendPacket(PacketInfo &info) override; private: struct Channel { - rtps::UdpConnection connection{}; + Ip4Port_t port{0}; std::unique_ptr socket{}; bool in_use{false}; }; @@ -67,12 +63,12 @@ class EsppTransport : public platform::transport::ITransport { void onReceive(Ip4Port_t receivePort, std::vector &data, const espp::Socket::Info &sender) const; - static std::string ip4ToString(platform::Ip4Address addr); + static std::string ip4ToString(const platform::transport::Ip4AddressBytes &addr); RxCallback m_rxCallback{nullptr}; void *m_callbackArgs{nullptr}; mutable std::recursive_mutex m_mutex; - std::array m_channels{}; + std::array m_channels{}; mutable std::vector m_multicastGroups; }; diff --git a/components/rtps_embedded/include/rtps/communication/PacketInfo.h b/components/rtps_embedded/include/rtps/communication/PacketInfo.h index 240158a3c..4c5006299 100644 --- a/components/rtps_embedded/include/rtps/communication/PacketInfo.h +++ b/components/rtps_embedded/include/rtps/communication/PacketInfo.h @@ -28,16 +28,18 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_PACKETINFO_H #define RTPS_PACKETINFO_H +#include +#include + #include "rtps/common/types.h" -#include "rtps/storages/PBufWrapper.h" namespace rtps { struct PacketInfo { Ip4Port_t srcPort; // TODO Do we need that? - ip4_addr_t destAddr; + std::array destAddr = {0, 0, 0, 0}; Ip4Port_t destPort; - PBufWrapper buffer; + std::vector payload; void copyTriviallyCopyable(const PacketInfo &other) { this->srcPort = other.srcPort; @@ -52,9 +54,10 @@ struct PacketInfo { PacketInfo &operator=(PacketInfo &&other) noexcept { copyTriviallyCopyable(other); - this->buffer = std::move(other.buffer); + this->payload = std::move(other.payload); return *this; } + }; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/communication/TcpipCoreLock.h b/components/rtps_embedded/include/rtps/communication/TcpipCoreLock.h deleted file mode 100644 index fcd26bb05..000000000 --- a/components/rtps_embedded/include/rtps/communication/TcpipCoreLock.h +++ /dev/null @@ -1,38 +0,0 @@ -/* -The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -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 - -This file is part of embeddedRTPS. - -Author: i11 - Embedded Software, RWTH Aachen University -*/ - -#ifndef RTPS_TCPIPCORELOCK_H -#define RTPS_TCPIPCORELOCK_H - -#include - -namespace rtps { -class TcpipCoreLock { -public: - TcpipCoreLock() { LOCK_TCPIP_CORE(); } - ~TcpipCoreLock() { UNLOCK_TCPIP_CORE(); } -}; -} // namespace rtps - -#endif // RTPS_TCPIPCORELOCK_H diff --git a/components/rtps_embedded/include/rtps/communication/UdpDriver.h b/components/rtps_embedded/include/rtps/communication/UdpDriver.h deleted file mode 100644 index 55f6c404b..000000000 --- a/components/rtps_embedded/include/rtps/communication/UdpDriver.h +++ /dev/null @@ -1,66 +0,0 @@ -/* -The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -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 - -This file is part of embeddedRTPS. - -Author: i11 - Embedded Software, RWTH Aachen University -*/ - -#ifndef RTPS_UDPDRIVER_H -#define RTPS_UDPDRIVER_H - -#include "UdpConnection.h" -#include "lwip/udp.h" -#include "rtps/common/types.h" -#include "rtps/communication/PacketInfo.h" -#include "rtps/config.h" -#include "rtps/platform/transport.h" -#include "rtps/storages/PBufWrapper.h" - -#include - -namespace rtps { - -class UdpDriver : public platform::transport::ITransport { - -public: - typedef void (*udpRxFunc_fp)(void *arg, udp_pcb *pcb, pbuf *p, - const ip_addr_t *addr, Ip4Port_t port); - - UdpDriver(udpRxFunc_fp callback, void *args); - - const rtps::UdpConnection *createUdpConnection(Ip4Port_t receivePort) override; - bool joinMultiCastGroup(platform::Ip4Address addr) const override; - void sendPacket(PacketInfo &info) override; - - static bool isSameSubnet(ip4_addr_t addr); - static bool isMulticastAddress(ip4_addr_t addr); - -private: - std::array m_conns; - std::size_t m_numConns = 0; - udpRxFunc_fp m_rxCallback = nullptr; - void *m_callbackArgs = nullptr; - - bool sendPacket(const UdpConnection &conn, ip4_addr_t &destAddr, - Ip4Port_t destPort, pbuf &buffer); -}; -} // namespace rtps - -#endif // RTPS_UDPDRIVER_H diff --git a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h index 6b251bd02..b9eb11235 100644 --- a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h +++ b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h @@ -29,9 +29,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/messages/MessageTypes.h" #include "ucdr/microcdr.h" #include -#if defined(unix) || defined(__unix__) #include -#endif #include namespace rtps { @@ -61,12 +59,7 @@ class ParticipantProxyData { m_defaultMulticastLocatorList; Count_t m_manualLivelinessCount{1}; Duration_t m_leaseDuration = Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION; -#if defined(unix) || defined(__unix__) - std::chrono::time_point - m_lastLivelinessReceivedTimestamp; -#else - TickType_t m_lastLivelinessReceivedTickCount = 0; -#endif + std::chrono::time_point m_lastLivelinessReceivedTimestamp; void reset(); bool readFromUcdrBuffer(ucdrBuffer &buffer, Participant *participant); @@ -145,23 +138,14 @@ bool ParticipantProxyData::hasSubscriptionReader() { } void ParticipantProxyData::onAliveSignal() { -#if defined(unix) || defined(__unix__) - m_lastLivelinessReceivedTimestamp = std::chrono::high_resolution_clock::now(); -#else - m_lastLivelinessReceivedTickCount = xTaskGetTickCount(); -#endif + m_lastLivelinessReceivedTimestamp = std::chrono::steady_clock::now(); } uint32_t ParticipantProxyData::getAliveSignalAgeInMilliseconds() { -#if defined(unix) || defined(__unix__) - auto now = std::chrono::high_resolution_clock::now(); - std::chrono::duration duration = - now - m_lastLivelinessReceivedTimestamp; - return duration.count(); -#else - return (xTaskGetTickCount() - m_lastLivelinessReceivedTickCount) * - (1000 / configTICK_RATE_HZ); -#endif + auto now = std::chrono::steady_clock::now(); + auto duration = + std::chrono::duration_cast(now - m_lastLivelinessReceivedTimestamp); + return static_cast(duration.count()); } /* diff --git a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h index 4b6928c97..3701ed54e 100644 --- a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h +++ b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h @@ -27,6 +27,9 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/discovery/BuiltInEndpoints.h" #include "rtps/discovery/TopicData.h" +#include "rtps/platform/sync.h" + +#include namespace rtps { @@ -61,7 +64,7 @@ class SEDPAgent { private: Participant *m_part; - SemaphoreHandle_t m_mutex; + platform::sync::RecursiveMutexHandle m_mutex = nullptr; uint8_t m_buffer[600]; // TODO check size, currently changed from 300 to 600 // (FastDDS gives too many options) BuiltInEndpoints m_endpoints; diff --git a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h index b14ca6f22..24d5a588f 100644 --- a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h +++ b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h @@ -25,14 +25,17 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_SPDP_H #define RTPS_SPDP_H -#include "lwip/sys.h" #include "rtps/common/types.h" #include "rtps/config.h" #include "rtps/discovery/BuiltInEndpoints.h" #include "rtps/discovery/ParticipantProxyData.h" +#include "rtps/platform/sync.h" #include "rtps/utils/Log.h" +#include "task.hpp" #include "ucdr/microcdr.h" +#include + #if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" #define SPDP_LOG(...) \ @@ -56,11 +59,12 @@ class SPDPAgent { void init(Participant &participant, BuiltInEndpoints &endpoints); void start(); void stop(); - SemaphoreHandle_t m_mutex; + platform::sync::RecursiveMutexHandle m_mutex = nullptr; private: Participant *mp_participant = nullptr; BuiltInEndpoints m_buildInEndpoints; + std::unique_ptr m_broadcastTask; bool m_running = false; std::array m_outputBuffer{}; // TODO check required size std::array m_inputBuffer{}; @@ -80,7 +84,7 @@ class SPDPAgent { void addParticipantParameters(); void endCurrentList(); - static void runBroadcast(void *args); + void runBroadcast(); }; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/entities/Domain.h b/components/rtps_embedded/include/rtps/entities/Domain.h index 8b094d4eb..6723a06b7 100644 --- a/components/rtps_embedded/include/rtps/entities/Domain.h +++ b/components/rtps_embedded/include/rtps/entities/Domain.h @@ -27,22 +27,22 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/ThreadPool.h" #include "rtps/communication/EsppTransport.h" -#include "rtps/communication/UdpDriver.h" #include "rtps/config.h" #include "rtps/entities/Participant.h" #include "rtps/entities/StatefulReader.h" #include "rtps/entities/StatefulWriter.h" #include "rtps/entities/StatelessReader.h" #include "rtps/entities/StatelessWriter.h" +#include "rtps/platform/sync.h" #include "rtps/platform/transport.h" -#include "rtps/storages/PBufWrapper.h" #include namespace rtps { class Domain { public: - Domain(); - explicit Domain(platform::transport::ITransport &transport); + explicit Domain(const platform::transport::Ip4AddressBytes &localIpAddress); + Domain(platform::transport::ITransport &transport, + const platform::transport::Ip4AddressBytes &localIpAddress); ~Domain(); bool completeInit(); @@ -54,7 +54,8 @@ class Domain { bool enforceUnicast = false); Reader *createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, - ip4_addr_t mcastaddress = {0}); + platform::transport::Ip4AddressBytes mcastaddress = + {0, 0, 0, 0}); Writer *writerExists(Participant &part, const char *topicName, const char *typeName, bool reliable); @@ -69,14 +70,11 @@ class Domain { private: friend class SizeInspector; ThreadPool m_threadPool; -#if defined(RTPS_USE_ESPP_TRANSPORT) using DefaultTransport = EsppTransport; -#else - using DefaultTransport = UdpDriver; -#endif DefaultTransport m_defaultTransport; platform::transport::ITransport *m_transport = nullptr; std::array m_participants; + platform::transport::Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; const uint8_t PARTICIPANT_START_ID = 0; ParticipantId_t m_nextParticipantId = PARTICIPANT_START_ID; @@ -94,7 +92,7 @@ class Domain { } bool m_initComplete = false; - SemaphoreHandle_t m_mutex; + platform::sync::RecursiveMutexHandle m_mutex = nullptr; void receiveCallback(const PacketInfo &packet); GuidPrefix_t generateGuidPrefix(ParticipantId_t id) const; diff --git a/components/rtps_embedded/include/rtps/entities/Participant.h b/components/rtps_embedded/include/rtps/entities/Participant.h index c9ca72b92..063bb236a 100644 --- a/components/rtps_embedded/include/rtps/entities/Participant.h +++ b/components/rtps_embedded/include/rtps/entities/Participant.h @@ -30,6 +30,10 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/discovery/SEDPAgent.h" #include "rtps/discovery/SPDPAgent.h" #include "rtps/messages/MessageReceiver.h" +#include "rtps/platform/sync.h" +#include "rtps/platform/transport.h" + +#include namespace rtps { @@ -40,6 +44,7 @@ class Participant { public: GuidPrefix_t m_guidPrefix; ParticipantId_t m_participantId; + platform::transport::Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; Participant(); explicit Participant(const GuidPrefix_t &guidPrefix, @@ -56,6 +61,8 @@ class Participant { bool isValid(); void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId); + void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, + const platform::transport::Ip4AddressBytes &localIpAddress); std::array getNextUserEntityKey(); @@ -97,7 +104,7 @@ class Participant { MessageReceiver *getMessageReceiver(); bool checkAndResetHeartbeats(); - bool hasReaderWithMulticastLocator(ip4_addr_t address); + bool hasReaderWithMulticastLocator(const std::array &address); void addBuiltInEndpoints(BuiltInEndpoints &endpoints); void newMessage(const uint8_t *data, DataSize_t size); @@ -115,7 +122,7 @@ class Participant { std::array m_readers = { nullptr}; - SemaphoreHandle_t m_mutex; + platform::sync::RecursiveMutexHandle m_mutex = nullptr; MemoryPool m_remoteParticipants; diff --git a/components/rtps_embedded/include/rtps/entities/Reader.h b/components/rtps_embedded/include/rtps/entities/Reader.h index f66ff037d..667920540 100644 --- a/components/rtps_embedded/include/rtps/entities/Reader.h +++ b/components/rtps_embedded/include/rtps/entities/Reader.h @@ -29,13 +29,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/config.h" #include "rtps/discovery/TopicData.h" #include "rtps/entities/WriterProxy.h" +#include "rtps/platform/sync.h" #include "rtps/storages/MemoryPool.h" -#include "rtps/storages/PBufWrapper.h" -#if defined(ESP_PLATFORM) -#include "freertos/semphr.h" -#else -#include "semphr.h" -#endif #include namespace rtps { @@ -77,7 +72,7 @@ class ReaderCacheChange { const uint8_t *getData() const { return data; } - const DataSize_t getDataSize() const { return size; } + DataSize_t getDataSize() const { return size; } }; typedef void (*ddsReaderCallback_fp)(void *callee, @@ -141,10 +136,10 @@ class Reader { std::array m_callbacks; // Guards manipulation of the proxies array - SemaphoreHandle_t m_proxies_mutex = nullptr; + platform::sync::RecursiveMutexHandle m_proxies_mutex = nullptr; // Guards manipulation of callback array - SemaphoreHandle_t m_callback_mutex = nullptr; + platform::sync::RecursiveMutexHandle m_callback_mutex = nullptr; }; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.h b/components/rtps_embedded/include/rtps/entities/StatefulReader.h index c8b09bf34..511cdaf42 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.h +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.h @@ -25,7 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_STATEFULREADER_H #define RTPS_STATEFULREADER_H -#include "lwip/sys.h" +#include "rtps/communication/PacketInfo.h" #include "rtps/config.h" #include "rtps/entities/Reader.h" #include "rtps/platform/transport.h" diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp index 117157f28..3be4c0282 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -22,10 +22,9 @@ This file is part of embeddedRTPS. Author: i11 - Embedded Software, RWTH Aachen University */ -#include "lwip/sys.h" -#include "lwip/tcpip.h" #include "rtps/entities/StatefulReader.h" #include "rtps/messages/MessageFactory.h" +#include "rtps/storages/PayloadBuffer.h" #include "rtps/utils/Diagnostics.h" #include "rtps/utils/Lock.h" #include "rtps/utils/Log.h" @@ -138,16 +137,21 @@ bool StatefulReaderT::onNewGapMessage( if (writer->expectedSN < msg.gapStart) { PacketInfo info; info.srcPort = m_srcPort; - info.destAddr = writer->remoteLocator.getIp4Address(); + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); info.destPort = writer->remoteLocator.port; - rtps::MessageFactory::addHeader(info.buffer, + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); SequenceNumber_t last_valid = msg.gapStart; --last_valid; auto missing_sns = writer->getMissing(writer->expectedSN, last_valid); - rtps::MessageFactory::addAckNack(info.buffer, msg.writerId, msg.readerId, + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, missing_sns, writer->getNextAckNackCount(), false); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } m_transport->sendPacket(info); return true; } @@ -155,7 +159,6 @@ bool StatefulReaderT::onNewGapMessage( // Case 2: We are expecting a message between [gapStart; gapList.base -1] // Advance expectedSN beyond gapList.base if (writer->expectedSN < msg.gapList.base) { - auto before = writer->expectedSN; writer->expectedSN = msg.gapList.base; // writer->expectedSN++; @@ -185,17 +188,22 @@ bool StatefulReaderT::onNewGapMessage( }else{ PacketInfo info; info.srcPort = m_srcPort; - info.destAddr = writer->remoteLocator.getIp4Address(); + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); info.destPort = writer->remoteLocator.port; - rtps::MessageFactory::addHeader(info.buffer, + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); SequenceNumberSet set; set.base = writer->expectedSN; set.numBits = 1; set.bitMap[0] = set.bitMap[0] |= uint32_t{1} << 31; - rtps::MessageFactory::addAckNack(info.buffer, msg.writerId, msg.readerId, + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, set, writer->getNextAckNackCount(), false); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } m_transport->sendPacket(info); return true; @@ -216,6 +224,7 @@ bool StatefulReaderT::onNewHeartbeat( } PacketInfo info; info.srcPort = m_srcPort; + PayloadBuffer payload; Guid_t writerProxyGuid; writerProxyGuid.prefix = sourceGuidPrefix; @@ -239,18 +248,22 @@ bool StatefulReaderT::onNewHeartbeat( } writer->hbCount.value = msg.count.value; - info.destAddr = writer->remoteLocator.getIp4Address(); + info.destAddr = writer->remoteLocator.getIp4AddressBytes(); info.destPort = writer->remoteLocator.port; - rtps::MessageFactory::addHeader(info.buffer, + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); auto missing_sns = writer->getMissing(msg.firstSN, msg.lastSN); bool final_flag = (missing_sns.numBits == 0); - rtps::MessageFactory::addAckNack(info.buffer, msg.writerId, msg.readerId, + rtps::MessageFactory::addAckNack(payload, msg.writerId, msg.readerId, missing_sns, writer->getNextAckNackCount(), final_flag); SFR_LOG("Sending acknack base %u bits %u .\n", (int)missing_sns.base.low, (int)missing_sns.numBits); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } m_transport->sendPacket(info); return true; } @@ -265,19 +278,24 @@ bool StatefulReaderT::sendPreemptiveAckNack( PacketInfo info; info.srcPort = m_attributes.unicastLocator.port; - info.destAddr = writer.remoteLocator.getIp4Address(); + info.destAddr = writer.remoteLocator.getIp4AddressBytes(); info.destPort = writer.remoteLocator.port; - rtps::MessageFactory::addHeader(info.buffer, + PayloadBuffer payload; + rtps::MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); SequenceNumberSet number_set; number_set.base.high = 0; number_set.base.low = 0; number_set.numBits = 0; rtps::MessageFactory::addAckNack( - info.buffer, writer.remoteWriterGuid.entityId, + payload, writer.remoteWriterGuid.entityId, m_attributes.endpointGuid.entityId, number_set, Count_t{1}, false); SFR_LOG("Sending preemptive acknack.\n"); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } m_transport->sendPacket(info); return true; } diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h index ea547f996..2ffd6fbe6 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h @@ -28,9 +28,11 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/ReaderProxy.h" #include "rtps/entities/Writer.h" #include "rtps/platform/transport.h" -#include "rtps/platform/threading.h" #include "rtps/storages/HistoryCacheWithDeletion.h" #include "rtps/storages/MemoryPool.h" +#include "task.hpp" + +#include namespace rtps { @@ -69,7 +71,7 @@ template class StatefulWriterT final : public Writer { ThreadSafeCircularBuffer m_disposeWithDelay; void dropDisposeAfterWriteChanges(); - platform::threading::ThreadHandle m_heartbeatThread; + std::unique_ptr m_heartbeatTask; Count_t m_hbCount{1}; @@ -78,7 +80,6 @@ template class StatefulWriterT final : public Writer { bool sendData(const ReaderProxy &reader, const CacheChange *next); bool sendDataWRMulticast(const ReaderProxy &reader, const CacheChange *next); - static void hbFunctionJumppad(void *thisPointer); void sendHeartBeatLoop(); void sendHeartBeat(); void sendGap(const ReaderProxy &reader, const SequenceNumber_t &firstMissing, diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp index c86c95d01..337545552 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -25,10 +25,12 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/StatefulWriter.h" #include "rtps/messages/MessageFactory.h" #include "rtps/messages/MessageTypes.h" -#include "rtps/platform/threading.h" +#include "rtps/storages/PayloadBuffer.h" #include "rtps/utils/Log.h" +#include #include #include +#include using rtps::StatefulWriterT; @@ -47,11 +49,12 @@ using rtps::StatefulWriterT; template StatefulWriterT::~StatefulWriterT() { m_running = false; + if (m_heartbeatTask) { + m_heartbeatTask->stop(); + } while (m_thread_running) { - platform::threading::sleepMs(500); // Required for tests/ Join currently not available / - // increased because Segfault in Tests if(sys_mutex_valid(&m_mutex)){ - // sys_mutex_free(&m_mutex); - // } + // Wait for the heartbeat loop to observe m_running and exit. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); } } @@ -98,21 +101,29 @@ bool StatefulWriterT::init(TopicData attributes, m_running = true; m_thread_running = false; + const char *task_name = "HBThread"; if (m_attributes.endpointGuid.entityId == ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) { - m_heartbeatThread = platform::threading::startThread( - "HBThreadPub", hbFunctionJumppad, this, - Config::HEARTBEAT_STACKSIZE, Config::THREAD_POOL_WRITER_PRIO); + task_name = "HBThreadPub"; } else if (m_attributes.endpointGuid.entityId == ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) { - m_heartbeatThread = platform::threading::startThread( - "HBThreadSub", hbFunctionJumppad, this, - Config::HEARTBEAT_STACKSIZE, Config::THREAD_POOL_WRITER_PRIO); - } else { - m_heartbeatThread = platform::threading::startThread( - "HBThread", hbFunctionJumppad, this, Config::HEARTBEAT_STACKSIZE, - Config::THREAD_POOL_WRITER_PRIO); + task_name = "HBThreadSub"; + } + + if (!m_heartbeatTask) { + espp::Task::Config config; + config.callback = [this]() { + sendHeartBeatLoop(); + return true; + }; + config.task_config.name = task_name; + config.task_config.stack_size_bytes = Config::HEARTBEAT_STACKSIZE; + config.task_config.priority = Config::THREAD_POOL_WRITER_PRIO; + config.log_level = espp::Logger::Verbosity::WARN; + m_heartbeatTask = espp::Task::make_unique(config); } + + (void)m_heartbeatTask->start(); } return true; @@ -191,7 +202,7 @@ template void StatefulWriterT::progress() { * during SEDP */ if (next->disposeAfterWrite) { - next->sentTickCount = xTaskGetTickCount(); + next->sentTime = std::chrono::steady_clock::now(); if (!m_disposeWithDelay.copyElementIntoBuffer(next->sequenceNumber)) { SFW_LOG("Failed to enqueue dispose after write!"); m_history.dropChange(next->sequenceNumber); @@ -339,25 +350,28 @@ template bool StatefulWriterT::sendData(const ReaderProxy &reader, const CacheChange *next) { INIT_GUARD() - // TODO smarter packaging e.g. by creating MessageStruct and serialize after - // adjusting values Reusing the pbuf is not possible. See - // https://www.nongnu.org/lwip/2_0_x/raw_api.html (Zero-Copy MACs) + // TODO smarter packaging, e.g. create a message struct and serialize once. PacketInfo info; info.srcPort = m_srcPort; + PayloadBuffer payload; - MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); - MessageFactory::addSubMessageTimeStamp(info.buffer); + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); // Just usable for IPv4 const LocatorIPv4 &locator = reader.remoteLocator; - info.destAddr = locator.getIp4Address(); + info.destAddr = locator.getIp4AddressBytes(); info.destPort = (Ip4Port_t)locator.port; MessageFactory::addSubMessageData( - info.buffer, next->data, next->inLineQoS, next->sequenceNumber, + payload, next->data, next->inLineQoS, next->sequenceNumber, m_attributes.endpointGuid.entityId, reader.remoteReaderGuid.entityId); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } m_transport->sendPacket(info); return true; @@ -368,25 +382,28 @@ void StatefulWriterT::sendGap( const ReaderProxy &reader, const SequenceNumber_t &firstMissing, const SequenceNumber_t &nextValid) { INIT_GUARD() - // TODO smarter packaging e.g. by creating MessageStruct and serialize after - // adjusting values Reusing the pbuf is not possible. See - // https://www.nongnu.org/lwip/2_0_x/raw_api.html (Zero-Copy MACs) + // TODO smarter packaging, e.g. create a message struct and serialize once. PacketInfo info; info.srcPort = m_srcPort; + PayloadBuffer payload; - MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); - MessageFactory::addSubMessageTimeStamp(info.buffer); + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); // Just usable for IPv4 const LocatorIPv4 &locator = reader.remoteLocator; - info.destAddr = locator.getIp4Address(); + info.destAddr = locator.getIp4AddressBytes(); info.destPort = (Ip4Port_t)locator.port; MessageFactory::addSubmessageGap( - info.buffer, m_attributes.endpointGuid.entityId, + payload, m_attributes.endpointGuid.entityId, reader.remoteReaderGuid.entityId, firstMissing, nextValid); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return; + } m_transport->sendPacket(info); } @@ -398,18 +415,19 @@ bool StatefulWriterT::sendDataWRMulticast( if (reader.useMulticast || reader.suppressUnicast == false) { PacketInfo info; info.srcPort = m_srcPort; + PayloadBuffer payload; - MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); - MessageFactory::addSubMessageTimeStamp(info.buffer); + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); - // Deceide whether multicast or not + // Decide whether to use multicast or unicast. if (reader.useMulticast) { const LocatorIPv4 &locator = reader.remoteMulticastLocator; - info.destAddr = locator.getIp4Address(); + info.destAddr = locator.getIp4AddressBytes(); info.destPort = (Ip4Port_t)locator.port; } else { const LocatorIPv4 &locator = reader.remoteLocator; - info.destAddr = locator.getIp4Address(); + info.destAddr = locator.getIp4AddressBytes(); info.destPort = (Ip4Port_t)locator.port; } @@ -420,21 +438,19 @@ bool StatefulWriterT::sendDataWRMulticast( reid = reader.remoteReaderGuid.entityId; } - MessageFactory::addSubMessageData(info.buffer, next->data, next->inLineQoS, + MessageFactory::addSubMessageData(payload, next->data, next->inLineQoS, next->sequenceNumber, m_attributes.endpointGuid.entityId, reid); + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + return false; + } m_transport->sendPacket(info); } return true; } -template -void StatefulWriterT::hbFunctionJumppad(void *thisPointer) { - auto *writer = static_cast *>(thisPointer); - writer->sendHeartBeatLoop(); -} - template void StatefulWriterT::sendHeartBeatLoop() { m_thread_running = true; @@ -452,17 +468,12 @@ void StatefulWriterT::sendHeartBeatLoop() { // Temporarily increase HB frequency if there are unconfirmed remote changes if (unconfirmed_changes) { SFW_LOG("HB SPEEDUP!\r\n"); -#ifdef OS_IS_FREERTOS - vTaskDelay(pdMS_TO_TICKS(Config::SF_WRITER_HB_PERIOD_MS / 4)); - } else { - vTaskDelay(pdMS_TO_TICKS(Config::SF_WRITER_HB_PERIOD_MS)); - } -#else - platform::threading::sleepMs(Config::SF_WRITER_HB_PERIOD_MS / 4); + std::this_thread::sleep_for( + std::chrono::milliseconds(Config::SF_WRITER_HB_PERIOD_MS / 4)); } else { - platform::threading::sleepMs(Config::SF_WRITER_HB_PERIOD_MS); + std::this_thread::sleep_for( + std::chrono::milliseconds(Config::SF_WRITER_HB_PERIOD_MS)); } -#endif } m_thread_running = false; } @@ -479,8 +490,15 @@ void StatefulWriterT::dropDisposeAfterWriteChanges() { return; } - auto age = (xTaskGetTickCount() - change->sentTickCount); - if (age > pdMS_TO_TICKS(4000)) { + if (change->sentTime == CacheChange::TimePoint{}) { + m_history.dropChange(change->sequenceNumber); + SequenceNumber_t tmp; + m_disposeWithDelay.moveFirstInto(tmp); + continue; + } + + auto age = std::chrono::steady_clock::now() - change->sentTime; + if (age > std::chrono::milliseconds(4000)) { m_history.dropChange(change->sequenceNumber); SFW_LOG("Removing SN %u %u for good\r\n", static_cast(oldest_retained.low), @@ -508,11 +526,12 @@ void StatefulWriterT::sendHeartBeat() { PacketInfo info; info.srcPort = m_srcPort; + PayloadBuffer payload; SequenceNumber_t firstSN; SequenceNumber_t lastSN; - MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); { Lock lock{m_mutex}; @@ -548,11 +567,15 @@ void StatefulWriterT::sendHeartBeat() { lastSN.low, lastSN.high); MessageFactory::addHeartbeat( - info.buffer, m_attributes.endpointGuid.entityId, + payload, m_attributes.endpointGuid.entityId, proxy.remoteReaderGuid.entityId, firstSN, lastSN, m_hbCount); - info.destAddr = proxy.remoteLocator.getIp4Address(); + info.destAddr = proxy.remoteLocator.getIp4AddressBytes(); info.destPort = proxy.remoteLocator.port; + info.payload = std::move(payload.bytes); + if (info.payload.empty()) { + continue; + } m_transport->sendPacket(info); } m_hbCount.value++; diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h index 8603c0ad7..701385db3 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h @@ -25,7 +25,6 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_RTPSWRITER_H #define RTPS_RTPSWRITER_H -#include "lwip/sys.h" #include "rtps/common/types.h" #include "rtps/config.h" #include "rtps/entities/Writer.h" @@ -35,8 +34,6 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { -struct PBufWrapper; - template class StatelessWriterT : public Writer { public: ~StatelessWriterT() override; diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp index 5a1aa0aaf..f6e66dd03 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp @@ -25,12 +25,9 @@ Author: i11 - Embedded Software, RWTH Aachen University #include #include -#include "lwip/sys.h" -#include "lwip/tcpip.h" #include "rtps/ThreadPool.h" -#include "rtps/communication/UdpDriver.h" #include "rtps/messages/MessageFactory.h" -#include "rtps/storages/PBufWrapper.h" +#include "rtps/storages/PayloadBuffer.h" #include "rtps/utils/Log.h" #include "rtps/utils/udpUtils.h" @@ -156,9 +153,8 @@ void StatelessWriterT::onNewAckNack( template void StatelessWriterT::progress() { INIT_GUARD(); - // TODO smarter packaging e.g. by creating MessageStruct and serialize after - // adjusting values Reusing the pbuf is not possible. See - // https://www.nongnu.org/lwip/2_1_x/raw_api.html (Zero-Copy MACs) + // TODO smarter packaging e.g. by creating MessageStruct and serializing + // after adjusting values. if (m_proxies.getNumElements() == 0) { SLW_LOG("No Proxy!\n"); @@ -171,9 +167,10 @@ void StatelessWriterT::progress() { if (proxy.useMulticast || !proxy.suppressUnicast || m_enforceUnicast) { PacketInfo info; info.srcPort = m_srcPort; + PayloadBuffer payload; - MessageFactory::addHeader(info.buffer, m_attributes.endpointGuid.prefix); - MessageFactory::addSubMessageTimeStamp(info.buffer); + MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); + MessageFactory::addSubMessageTimeStamp(payload); { Lock lock(m_mutex); @@ -200,25 +197,29 @@ void StatelessWriterT::progress() { } else { reid = proxy.remoteReaderGuid.entityId; } - MessageFactory::addSubMessageData(info.buffer, next->data, false, + MessageFactory::addSubMessageData(payload, next->data, false, next->sequenceNumber, m_attributes.endpointGuid.entityId, reid); // TODO } + info.payload = std::move(payload.bytes); + // Just usable for IPv4 // Decide which locator to be used unicast/multicast if (proxy.useMulticast && !m_enforceUnicast) { - info.destAddr = proxy.remoteMulticastLocator.getIp4Address(); + info.destAddr = proxy.remoteMulticastLocator.getIp4AddressBytes(); info.destPort = (Ip4Port_t)proxy.remoteMulticastLocator.port; } else { - info.destAddr = proxy.remoteLocator.getIp4Address(); + info.destAddr = proxy.remoteLocator.getIp4AddressBytes(); info.destPort = (Ip4Port_t)proxy.remoteLocator.port; } - SLW_LOG("Sending to %ld.%ld.%ld.%ld:%d\n", (info.destAddr.addr & 0xFF), - (info.destAddr.addr >> 8) & 0xFF, (info.destAddr.addr >> 16) & 0xFF, - (info.destAddr.addr >> 24) & 0xFF, info.destPort); + SLW_LOG("Sending to %u.%u.%u.%u:%d\n", info.destAddr[0], info.destAddr[1], + info.destAddr[2], info.destAddr[3], info.destPort); + if (info.payload.empty()) { + continue; + } m_transport->sendPacket(info); } } diff --git a/components/rtps_embedded/include/rtps/entities/Writer.h b/components/rtps_embedded/include/rtps/entities/Writer.h index d1693b7bb..da371fbcb 100644 --- a/components/rtps_embedded/include/rtps/entities/Writer.h +++ b/components/rtps_embedded/include/rtps/entities/Writer.h @@ -28,9 +28,11 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/ThreadPool.h" #include "rtps/discovery/TopicData.h" #include "rtps/entities/ReaderProxy.h" +#include "rtps/platform/sync.h" #include "rtps/storages/CacheChange.h" #include "rtps/storages/MemoryPool.h" -#include "rtps/storages/PBufWrapper.h" + +#include #ifdef DEBUG_BUILD #define COMPILE_INIT_GUARD @@ -74,7 +76,7 @@ class Writer { int dumpAllProxies(dumpProxyCallback target, void *arg); bool isInitialized(); - uint32_t getProxiesCount(); + std::uint32_t getProxiesCount(); void setSEDPSequenceNumber(const SequenceNumber_t &sn); const SequenceNumber_t &getSEDPSequenceNumber(); @@ -84,7 +86,7 @@ class Writer { protected: SequenceNumber_t m_sedp_sequence_number; - SemaphoreHandle_t m_mutex = nullptr; + platform::sync::RecursiveMutexHandle m_mutex = nullptr; ThreadPool *mp_threadPool = nullptr; Ip4Port_t m_srcPort; diff --git a/components/rtps_embedded/include/rtps/messages/MessageFactory.h b/components/rtps_embedded/include/rtps/messages/MessageFactory.h index fae2f73f0..711bf8c98 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageFactory.h +++ b/components/rtps_embedded/include/rtps/messages/MessageFactory.h @@ -101,8 +101,8 @@ void addSubMessageTimeStamp(Buffer &buffer, bool setInvalid = false) { } } -template -void addSubMessageData(Buffer &buffer, const Buffer &filledPayload, +template +void addSubMessageData(Buffer &buffer, const PayloadBuffer &filledPayload, bool containsInlineQos, const SequenceNumber_t &SN, const EntityId_t &writerID, const EntityId_t &readerID) { SubmessageData msg; diff --git a/components/rtps_embedded/include/rtps/platform/bootstrap.h b/components/rtps_embedded/include/rtps/platform/bootstrap.h index 3f803c58f..15cd7cbee 100644 --- a/components/rtps_embedded/include/rtps/platform/bootstrap.h +++ b/components/rtps_embedded/include/rtps/platform/bootstrap.h @@ -1,35 +1,47 @@ #ifndef RTPS_PLATFORM_BOOTSTRAP_H #define RTPS_PLATFORM_BOOTSTRAP_H -#include "lwip/err.h" -#include "lwip/sys.h" +#include +#include namespace rtps { namespace platform { namespace bootstrap { -using InitSemaphoreHandle = sys_sem_t; +struct InitSemaphoreHandle { + std::mutex mutex; + std::condition_variable cv; + bool signaled{false}; +}; inline bool createInitSemaphore(InitSemaphoreHandle *sem) { - return sem != nullptr && sys_sem_new(sem, 0) == ERR_OK; + if (sem == nullptr) { + return false; + } + std::lock_guard lock(sem->mutex); + sem->signaled = false; + return true; } inline void signalInitSemaphore(InitSemaphoreHandle *sem) { if (sem != nullptr) { - sys_sem_signal(sem); + { + std::lock_guard lock(sem->mutex); + sem->signaled = true; + } + sem->cv.notify_one(); } } inline void waitInitSemaphore(InitSemaphoreHandle *sem) { if (sem != nullptr) { - sys_sem_wait(sem); + std::unique_lock lock(sem->mutex); + sem->cv.wait(lock, [sem]() { return sem->signaled; }); } } inline void freeInitSemaphore(InitSemaphoreHandle *sem) { - if (sem != nullptr) { - sys_sem_free(sem); - } + (void)sem; } } // namespace bootstrap diff --git a/components/rtps_embedded/include/rtps/platform/platform_types.h b/components/rtps_embedded/include/rtps/platform/platform_types.h index 00de3a327..a7ce239e7 100644 --- a/components/rtps_embedded/include/rtps/platform/platform_types.h +++ b/components/rtps_embedded/include/rtps/platform/platform_types.h @@ -2,40 +2,25 @@ #define RTPS_PLATFORM_TYPES_H #include +#include #include #ifdef __cplusplus #include #endif -#include "lwip/ip4_addr.h" -#include "lwip/netif.h" -#include "lwip/sys.h" - namespace rtps { namespace platform { -using Ip4Address = ip4_addr_t; -using SemaphoreHandle = sys_sem_t; -using ThreadHandle = sys_thread_t; +using Ip4Address = std::array; +using SemaphoreHandle = void *; +using ThreadHandle = void *; #ifdef __cplusplus using Mutex = std::recursive_mutex; #endif inline bool tryGetDefaultIp4AddressBytes(std::array &ip) { - if (netif_default == nullptr) { - return false; - } - - const ip4_addr_t *iface_ip = netif_ip4_addr(netif_default); - if (iface_ip == nullptr) { - return false; - } - - ip[0] = ip4_addr1(iface_ip); - ip[1] = ip4_addr2(iface_ip); - ip[2] = ip4_addr3(iface_ip); - ip[3] = ip4_addr4(iface_ip); - return true; + (void)ip; + return false; } } // namespace platform diff --git a/components/rtps_embedded/include/rtps/platform/sync.h b/components/rtps_embedded/include/rtps/platform/sync.h index 4e966e415..e3f41a812 100644 --- a/components/rtps_embedded/include/rtps/platform/sync.h +++ b/components/rtps_embedded/include/rtps/platform/sync.h @@ -1,19 +1,13 @@ #ifndef RTPS_PLATFORM_SYNC_H #define RTPS_PLATFORM_SYNC_H -#if defined(ESP_PLATFORM) -#include "freertos/FreeRTOS.h" -#include "freertos/semphr.h" -#else -#include "FreeRTOS.h" -#include "semphr.h" -#endif +#include namespace rtps { namespace platform { namespace sync { -using RecursiveMutexHandle = SemaphoreHandle_t; +using RecursiveMutexHandle = std::recursive_mutex *; bool createRecursiveMutex(RecursiveMutexHandle *mutex); bool lockRecursive(RecursiveMutexHandle mutex); diff --git a/components/rtps_embedded/include/rtps/platform/threading.h b/components/rtps_embedded/include/rtps/platform/threading.h deleted file mode 100644 index 0d4a45667..000000000 --- a/components/rtps_embedded/include/rtps/platform/threading.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef RTPS_PLATFORM_THREADING_H -#define RTPS_PLATFORM_THREADING_H - -#include "lwip/err.h" -#include "lwip/sys.h" - -namespace rtps { -namespace platform { -namespace threading { - -using ThreadHandle = sys_thread_t; -using SemaphoreHandle = sys_sem_t; -using ThreadFunction = lwip_thread_fn; - -inline bool createSemaphore(SemaphoreHandle *sem, u8_t initial_count = 0) { - return sem != nullptr && sys_sem_new(sem, initial_count) == ERR_OK; -} - -inline bool isSemaphoreValid(SemaphoreHandle *sem) { - return sem != nullptr && sys_sem_valid(sem) != 0; -} - -inline void freeSemaphore(SemaphoreHandle *sem) { - if (sem != nullptr) { - sys_sem_free(sem); - } -} - -inline void signalSemaphore(SemaphoreHandle *sem) { - if (sem != nullptr) { - sys_sem_signal(sem); - } -} - -inline void waitSemaphore(SemaphoreHandle *sem) { - if (sem != nullptr) { - sys_sem_wait(sem); - } -} - -inline ThreadHandle startThread(const char *name, ThreadFunction function, - void *arg, int stacksize, int prio) { - return sys_thread_new(name, function, arg, stacksize, prio); -} - -inline void sleepMs(u32_t milliseconds) { sys_msleep(milliseconds); } - -} // namespace threading -} // namespace platform -} // namespace rtps - -#endif // RTPS_PLATFORM_THREADING_H diff --git a/components/rtps_embedded/include/rtps/platform/transport.h b/components/rtps_embedded/include/rtps/platform/transport.h index 05cdf48e7..361c70576 100644 --- a/components/rtps_embedded/include/rtps/platform/transport.h +++ b/components/rtps_embedded/include/rtps/platform/transport.h @@ -1,22 +1,31 @@ #ifndef RTPS_PLATFORM_TRANSPORT_H #define RTPS_PLATFORM_TRANSPORT_H +#include +#include + #include "rtps/common/types.h" #include "rtps/platform/platform_types.h" namespace rtps { struct PacketInfo; -struct UdpConnection; namespace platform { namespace transport { +using Ip4AddressBytes = std::array; + +using ReceiveCallback = void (*)(void *arg, const uint8_t *data, + std::size_t size, Ip4Port_t localPort, + Ip4Port_t remotePort, + const Ip4AddressBytes &remoteAddress); + class ITransport { public: virtual ~ITransport() = default; - virtual const UdpConnection *createUdpConnection(Ip4Port_t receivePort) = 0; - virtual bool joinMultiCastGroup(platform::Ip4Address addr) const = 0; + virtual bool ensureReceivePort(Ip4Port_t receivePort) = 0; + virtual bool joinMultiCastGroup(const Ip4AddressBytes &addr) const = 0; virtual void sendPacket(PacketInfo &info) = 0; }; diff --git a/components/rtps_embedded/include/rtps/rtps.h b/components/rtps_embedded/include/rtps/rtps.h index 4995e0162..09f31d559 100644 --- a/components/rtps_embedded/include/rtps/rtps.h +++ b/components/rtps_embedded/include/rtps/rtps.h @@ -28,12 +28,6 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/Domain.h" namespace rtps { - -#if defined(unix) || defined(__unix__) || defined(WIN32) || defined(_WIN32) || \ - defined(__WIN32) && !defined(__CYGWIN__) -void init(); -#endif - } // namespace rtps #endif // RTPS_RTPS_H diff --git a/components/rtps_embedded/include/rtps/storages/CacheChange.h b/components/rtps_embedded/include/rtps/storages/CacheChange.h index dc76bb974..a6ae4482b 100644 --- a/components/rtps_embedded/include/rtps/storages/CacheChange.h +++ b/components/rtps_embedded/include/rtps/storages/CacheChange.h @@ -26,16 +26,20 @@ Author: i11 - Embedded Software, RWTH Aachen University #define PROJECT_CACHECHANGE_H #include "rtps/common/types.h" -#include "rtps/storages/PBufWrapper.h" +#include "rtps/storages/PayloadBuffer.h" + +#include namespace rtps { struct CacheChange { + using TimePoint = std::chrono::steady_clock::time_point; + ChangeKind_t kind = ChangeKind_t::INVALID; bool inLineQoS = false; bool disposeAfterWrite = false; - TickType_t sentTickCount = 0; + TimePoint sentTime{}; SequenceNumber_t sequenceNumber = SEQUENCENUMBER_UNKNOWN; - PBufWrapper data; + PayloadBuffer data; CacheChange &operator=(const CacheChange &other) = delete; @@ -43,7 +47,7 @@ struct CacheChange { kind = other.kind; inLineQoS = other.inLineQoS; disposeAfterWrite = other.disposeAfterWrite; - sentTickCount = other.sentTickCount; + sentTime = other.sentTime; sequenceNumber = other.sequenceNumber; data = std::move(other.data); return *this; @@ -58,7 +62,7 @@ struct CacheChange { sequenceNumber = SEQUENCENUMBER_UNKNOWN; inLineQoS = false; disposeAfterWrite = false; - sentTickCount = 0; + sentTime = TimePoint{}; } bool isInitialized() { return (kind != ChangeKind_t::INVALID); } diff --git a/components/rtps_embedded/include/rtps/storages/PBufWrapper.h b/components/rtps_embedded/include/rtps/storages/PBufWrapper.h deleted file mode 100644 index 7a5ff2a70..000000000 --- a/components/rtps_embedded/include/rtps/storages/PBufWrapper.h +++ /dev/null @@ -1,84 +0,0 @@ -/* -The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -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 - -This file is part of embeddedRTPS. - -Author: i11 - Embedded Software, RWTH Aachen University -*/ - -// Copyright 2023 Apex.AI, Inc. -// All rights reserved. - -#ifndef RTPS_PBUFWRAPPER_H -#define RTPS_PBUFWRAPPER_H - -#include "lwip/pbuf.h" -#include "rtps/common/types.h" - -namespace rtps { - -struct PBufWrapper { - - pbuf *firstElement = nullptr; - - PBufWrapper() = default; - explicit PBufWrapper(pbuf *bufferToWrap); - explicit PBufWrapper(DataSize_t length); - - PBufWrapper(const PBufWrapper &other) = delete; - PBufWrapper &operator=(const PBufWrapper &other) = delete; - - PBufWrapper(PBufWrapper &&other) noexcept; - PBufWrapper &operator=(PBufWrapper &&other) noexcept; - - ~PBufWrapper(); - - bool isValid() const; - - bool append(const uint8_t *data, DataSize_t length); - - /// Note that unused reserved memory is now part of the wrapper. New calls to - /// append(uint8_t*[...]) will continue behind the appended wrapper - void append(const PBufWrapper &other); - - bool reserve(DataSize_t length); - - void destroy(); - - /// After calling this function, data is added starting from the beginning - /// again. It does not revert reserve. - void reset(); - - DataSize_t spaceLeft() const; - DataSize_t spaceUsed() const; - -private: - constexpr static pbuf_layer m_layer = PBUF_TRANSPORT; - constexpr static pbuf_type m_type = PBUF_POOL; - - DataSize_t m_freeSpace = 0; - - bool increaseSizeBy(uint16_t length); - - void copySimpleMembersAndResetBuffer(const PBufWrapper &other); -}; - -} // namespace rtps - -#endif // RTPS_PBUFWRAPPER_H diff --git a/components/rtps_embedded/include/rtps/communication/UdpConnection.h b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.h similarity index 59% rename from components/rtps_embedded/include/rtps/communication/UdpConnection.h rename to components/rtps_embedded/include/rtps/storages/PayloadBuffer.h index b95ac6fe6..b0abdbb0e 100644 --- a/components/rtps_embedded/include/rtps/communication/UdpConnection.h +++ b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.h @@ -22,47 +22,49 @@ This file is part of embeddedRTPS. Author: i11 - Embedded Software, RWTH Aachen University */ -#ifndef RTPS_UDPCONNECTION_H -#define RTPS_UDPCONNECTION_H +#ifndef RTPS_PAYLOADBUFFER_H +#define RTPS_PAYLOADBUFFER_H -#include "TcpipCoreLock.h" -#include "lwip/udp.h" +#include + +#include "rtps/common/types.h" namespace rtps { -struct UdpConnection { - udp_pcb *pcb = nullptr; - uint16_t port = 0; +struct PayloadBuffer { + std::vector bytes; - UdpConnection() = default; // Required for static allocation + bool isValid() const { return true; } - explicit UdpConnection(uint16_t port) : port(port) { - LOCK_TCPIP_CORE(); - pcb = udp_new(); - UNLOCK_TCPIP_CORE(); + bool reserve(DataSize_t length) { + if (length > bytes.max_size() - bytes.size()) { + return false; + } + bytes.reserve(bytes.size() + length); + return true; } - UdpConnection &operator=(UdpConnection &&other) noexcept { - port = other.port; - - if (pcb != nullptr) { - LOCK_TCPIP_CORE(); - udp_remove(pcb); - UNLOCK_TCPIP_CORE(); + bool append(const uint8_t *data, DataSize_t length) { + if (length == 0) { + return true; } - pcb = other.pcb; - other.pcb = nullptr; - return *this; + if (data == nullptr) { + return false; + } + + bytes.insert(bytes.end(), data, data + length); + return true; } - ~UdpConnection() { - if (pcb != nullptr) { - LOCK_TCPIP_CORE(); - udp_remove(pcb); - UNLOCK_TCPIP_CORE(); - pcb = nullptr; - } + void append(const PayloadBuffer &other) { + bytes.insert(bytes.end(), other.bytes.begin(), other.bytes.end()); } + + DataSize_t spaceUsed() const { return static_cast(bytes.size()); } + + void reset() { bytes.clear(); } }; + } // namespace rtps -#endif // RTPS_UDPCONNECTION_H + +#endif // RTPS_PAYLOADBUFFER_H diff --git a/components/rtps_embedded/include/rtps/utils/sysFunctions.h b/components/rtps_embedded/include/rtps/utils/sysFunctions.h index df6841262..b7175eca2 100644 --- a/components/rtps_embedded/include/rtps/utils/sysFunctions.h +++ b/components/rtps_embedded/include/rtps/utils/sysFunctions.h @@ -25,16 +25,21 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef PROJECT_SYSFUNCTIONS_H #define PROJECT_SYSFUNCTIONS_H -#include "lwip/sys.h" #include "rtps/common/types.h" +#include + namespace rtps { inline Time_t getCurrentTimeStamp() { + static const auto start = std::chrono::steady_clock::now(); + const auto elapsed = std::chrono::steady_clock::now() - start; + const auto nowMs = + std::chrono::duration_cast(elapsed).count(); + Time_t now; - // TODO FIX - uint32_t nowMs = sys_now(); - now.seconds = (int32_t)nowMs / 1000; - now.fraction = ((nowMs % 1000) / 1000); + now.seconds = static_cast(nowMs / 1000); + now.fraction = static_cast((nowMs % 1000) * + ((1ULL << 32) / 1000)); return now; } } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/utils/udpUtils.h b/components/rtps_embedded/include/rtps/utils/udpUtils.h index 5500e2025..7368c0c07 100644 --- a/components/rtps_embedded/include/rtps/utils/udpUtils.h +++ b/components/rtps_embedded/include/rtps/utils/udpUtils.h @@ -27,6 +27,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/config.h" +#include + namespace rtps { namespace { const uint16_t PB = 7400; // Port Base Number @@ -39,9 +41,9 @@ const uint16_t D2 = 1; // User multicast const uint16_t D3 = 11; // User unicast } // namespace -constexpr ip4_addr transformIP4ToU32(uint8_t MSB, uint8_t p2, uint8_t p1, - uint8_t LSB) { - return ip4_addr{PP_HTONL(LWIP_MAKEU32(MSB, p2, p1, LSB))}; +constexpr std::array +transformIP4ToU32(uint8_t MSB, uint8_t p2, uint8_t p1, uint8_t LSB) { + return {MSB, p2, p1, LSB}; } constexpr Ip4Port_t getBuiltInUnicastPort(ParticipantId_t participantId) { @@ -81,7 +83,14 @@ inline bool isUserMultiCastPort(Ip4Port_t port) { return (idWithoutBase >= D2 && idWithoutBase < D1); } -inline bool isZeroAddress(ip4_addr_t address) { return address.addr == 0; } +inline bool isZeroAddress(const std::array &address) { + return address[0] == 0 && address[1] == 0 && address[2] == 0 && + address[3] == 0; +} + +inline bool isMulticastAddress(const std::array &address) { + return address[0] >= 224 && address[0] <= 239; +} inline ParticipantId_t getParticipantIdFromUnicastPort(Ip4Port_t port, bool isUserPort) { diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp index 934606a8f..06cc56d2c 100644 --- a/components/rtps_embedded/src/ThreadPool.cpp +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -24,12 +24,12 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/ThreadPool.h" -#include "lwip/tcpip.h" #include "rtps/entities/Domain.h" #include "rtps/entities/Writer.h" #include "rtps/utils/Diagnostics.h" #include "rtps/utils/Log.h" #include "rtps/utils/udpUtils.h" +#include "thread_pool.hpp" using rtps::ThreadPool; @@ -52,25 +52,39 @@ ThreadPool::ThreadPool(receiveJumppad_fp receiveCallback, void *callee) !m_incomingMetaTraffic.init() || !m_incomingUserTraffic.init()) { return; } - bool inputOk = platform::threading::createSemaphore(&m_readerNotificationSem, 0); - bool outputOk = platform::threading::createSemaphore(&m_writerNotificationSem, 0); - if (!inputOk || !outputOk) { - THREAD_POOL_LOG("ThreadPool: Failed to create Semaphores.\n"); - } + m_writerWorkers = std::make_unique(espp::ThreadPool::Config{ + .worker_count = Config::THREAD_POOL_NUM_WRITERS, + .max_queue_size = 0, + .auto_start = false, + .block_on_submit_when_full = false, + .worker_task_config = { + .name = "rtps_writer", + .stack_size_bytes = static_cast(Config::THREAD_POOL_WRITER_STACKSIZE), + .priority = static_cast(Config::THREAD_POOL_WRITER_PRIO), + .core_id = -1, + }, + .log_level = espp::Logger::Verbosity::WARN, + }); + + m_readerWorkers = std::make_unique(espp::ThreadPool::Config{ + .worker_count = Config::THREAD_POOL_NUM_READERS, + .max_queue_size = 0, + .auto_start = false, + .block_on_submit_when_full = false, + .worker_task_config = { + .name = "rtps_reader", + .stack_size_bytes = static_cast(Config::THREAD_POOL_READER_STACKSIZE), + .priority = static_cast(Config::THREAD_POOL_READER_PRIO), + .core_id = -1, + }, + .log_level = espp::Logger::Verbosity::WARN, + }); } ThreadPool::~ThreadPool() { if (m_running) { stopThreads(); - platform::threading::sleepMs(500); - } - - if (platform::threading::isSemaphoreValid(&m_readerNotificationSem)) { - platform::threading::freeSemaphore(&m_readerNotificationSem); - } - if (platform::threading::isSemaphoreValid(&m_writerNotificationSem)) { - platform::threading::freeSemaphore(&m_writerNotificationSem); } } @@ -101,45 +115,27 @@ bool ThreadPool::startThreads() { if (m_running) { return true; } - if (!platform::threading::isSemaphoreValid(&m_readerNotificationSem) || - !platform::threading::isSemaphoreValid(&m_writerNotificationSem)) { + if (!m_writerWorkers || !m_readerWorkers) { return false; } m_running = true; - for (auto &thread : m_writers) { - // TODO ID, err check, waitOnStop - thread = platform::threading::startThread( - "WriterThread", writerThreadFunction, this, - Config::THREAD_POOL_WRITER_STACKSIZE, Config::THREAD_POOL_WRITER_PRIO); - } + m_writerWorkers->start(); + m_readerWorkers->start(); - for (auto &thread : m_readers) { - // TODO ID, err check, waitOnStop - thread = platform::threading::startThread( - "ReaderThread", readerThreadFunction, this, - Config::THREAD_POOL_READER_STACKSIZE, Config::THREAD_POOL_READER_PRIO); - } + scheduleWriterWork(); + scheduleReaderWork(); return true; } void ThreadPool::stopThreads() { m_running = false; - // This should call all the semaphores for each thread once, so they don't - // stuck before ended. - for (auto &thread : m_writers) { - (void)thread; - platform::threading::signalSemaphore(&m_writerNotificationSem); - platform::threading::sleepMs(10); + if (m_writerWorkers) { + m_writerWorkers->stop(); } - for (auto &thread : m_readers) { - (void)thread; - platform::threading::signalSemaphore(&m_readerNotificationSem); - platform::threading::sleepMs(10); + if (m_readerWorkers) { + m_readerWorkers->stop(); } - // TODO make sure they have finished. Seems to be sufficient for tests. - // Not sufficient if threads shall actually be stopped during runtime. - platform::threading::sleepMs(10); } void ThreadPool::clearQueues() { @@ -156,17 +152,18 @@ bool ThreadPool::addWorkload(Writer *workload) { } else { res = m_outgoingUserTraffic.moveElementIntoBuffer(std::move(workload)); } - if (res) { - platform::threading::signalSemaphore(&m_writerNotificationSem); - } else { + if (!res) { if(workload->isBuiltinEndpoint()){ rtps::Diagnostics::ThreadPool::dropped_outgoing_packets_metatraffic++; }else{ rtps::Diagnostics::ThreadPool::dropped_outgoing_packets_usertraffic++; } THREAD_POOL_LOG("Failed to enqueue outgoing packet."); + return false; } + scheduleWriterWork(); + return res; } @@ -203,25 +200,22 @@ bool ThreadPool::addNewPacket(PacketInfo &&packet) { } else { res = m_incomingUserTraffic.moveElementIntoBuffer(std::move(packet)); } - if (res) { - platform::threading::signalSemaphore(&m_readerNotificationSem); - } else { + if (!res) { THREAD_POOL_LOG("failed to enqueue packet for port %u", static_cast(packet.destPort)); + return false; } + + scheduleReaderWork(); return res; } -void ThreadPool::writerThreadFunction(void *arg) { - auto pool = static_cast(arg); - if (pool == nullptr) { - - THREAD_POOL_LOG("nullptr passed to writer function\n"); - +void ThreadPool::scheduleWriterWork() { + if (!m_running || !m_writerWorkers) { return; } - pool->doWriterWork(); + (void)m_writerWorkers->try_submit([this]() { doWriterWork(); }); } void ThreadPool::doWriterWork() { @@ -242,39 +236,34 @@ void ThreadPool::doWriterWork() { if (workload_usertraffic_available || workload_metatraffic_available) { continue; - } else { - THREAD_POOL_LOG("WriterWorker | User = %u, Meta = %u\r\n", - static_cast(Diagnostics::ThreadPool::processed_outgoing_usertraffic), - static_cast(Diagnostics::ThreadPool::processed_outgoing_metatraffic)); - updateDiagnostics(); - platform::threading::waitSemaphore(&m_writerNotificationSem); } + + THREAD_POOL_LOG("WriterWorker | User = %u, Meta = %u\r\n", + static_cast(Diagnostics::ThreadPool::processed_outgoing_usertraffic), + static_cast(Diagnostics::ThreadPool::processed_outgoing_metatraffic)); + updateDiagnostics(); + return; } } -void ThreadPool::readCallback(void *args, udp_pcb *target, pbuf *pbuf, - const ip_addr_t *addr, Ip4Port_t port) { - auto &pool = *static_cast(args); +void ThreadPool::onDatagram( + void *arg, const uint8_t *data, std::size_t size, Ip4Port_t localPort, + Ip4Port_t remotePort, + const platform::transport::Ip4AddressBytes &remoteAddress) { + auto &pool = *static_cast(arg); PacketInfo packet; + packet.destAddr = remoteAddress; + packet.destPort = localPort; + packet.srcPort = remotePort; - // TODO This is a workaround for chained pbufs caused by hardware limitations, - // not a general fix - if (pbuf->next != nullptr) { - struct pbuf *test = pbuf_alloc(PBUF_RAW, pbuf->tot_len, PBUF_POOL); - pbuf_copy(test, pbuf); - pbuf_free(pbuf); - pbuf = test; + if (size > 0 && data != nullptr) { + packet.payload.assign(data, data + size); } - packet.destAddr = {0}; // not relevant - packet.destPort = target->local_port; - packet.srcPort = port; - packet.buffer = PBufWrapper{pbuf}; - if (!pool.addNewPacket(std::move(packet))) { THREAD_POOL_LOG("ThreadPool: dropped packet\n"); - if (pool.isBuiltinPort(port)) { + if (pool.isBuiltinPort(remotePort)) { rtps::Diagnostics::ThreadPool::dropped_incoming_packets_metatraffic++; } else { rtps::Diagnostics::ThreadPool::dropped_incoming_packets_usertraffic++; @@ -282,15 +271,12 @@ void ThreadPool::readCallback(void *args, udp_pcb *target, pbuf *pbuf, } } -void ThreadPool::readerThreadFunction(void *arg) { - auto pool = static_cast(arg); - if (pool == nullptr) { - - THREAD_POOL_LOG("nullptr passed to reader function\n"); - +void ThreadPool::scheduleReaderWork() { + if (!m_running || !m_readerWorkers) { return; } - pool->doReaderWork(); + + (void)m_readerWorkers->try_submit([this]() { doReaderWork(); }); } void ThreadPool::doReaderWork() { @@ -318,7 +304,7 @@ void ThreadPool::doReaderWork() { static_cast(usertraffic), static_cast(metatraffic)); updateDiagnostics(); - platform::threading::waitSemaphore(&m_readerNotificationSem); + return; } } diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps_embedded/src/communication/EsppTransport.cpp index da54e4a6b..aab8fdf0e 100644 --- a/components/rtps_embedded/src/communication/EsppTransport.cpp +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -24,23 +24,57 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/communication/EsppTransport.h" -#include "lwip/ip_addr.h" -#include "lwip/ip4_addr.h" #include "rtps/communication/PacketInfo.h" #include "task.hpp" #include +#include #include #include using rtps::EsppTransport; +namespace { + +bool parseIp4Address(const std::string &address, + rtps::platform::transport::Ip4AddressBytes &out) { + rtps::platform::transport::Ip4AddressBytes parsed{0, 0, 0, 0}; + const char *cursor = address.c_str(); + + for (std::size_t i = 0; i < parsed.size(); ++i) { + char *end = nullptr; + const unsigned long value = std::strtoul(cursor, &end, 10); + if (end == cursor || value > 255) { + return false; + } + + parsed[i] = static_cast(value); + if (i + 1 < parsed.size()) { + if (*end != '.') { + return false; + } + cursor = end + 1; + } else if (*end != '\0') { + return false; + } + } + + out = parsed; + return true; +} + +bool isMulticastAddress(const rtps::platform::transport::Ip4AddressBytes &addr) { + return addr[0] >= 224 && addr[0] <= 239; +} + +} // namespace + EsppTransport::EsppTransport(RxCallback callback, void *args) : m_rxCallback(callback), m_callbackArgs(args) {} EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) { for (auto &channel : m_channels) { - if (channel.in_use && channel.connection.port == port) { + if (channel.in_use && channel.port == port) { return &channel; } } @@ -49,21 +83,17 @@ EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) { const EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) const { for (const auto &channel : m_channels) { - if (channel.in_use && channel.connection.port == port) { + if (channel.in_use && channel.port == port) { return &channel; } } return nullptr; } -std::string EsppTransport::ip4ToString(platform::Ip4Address addr) { - std::array buffer{}; - const char *result = ip4addr_ntoa_r(&addr, buffer.data(), - static_cast(buffer.size())); - if (result == nullptr) { - return "0.0.0.0"; - } - return std::string(result); +std::string EsppTransport::ip4ToString( + const platform::transport::Ip4AddressBytes &addr) { + return std::to_string(addr[0]) + "." + std::to_string(addr[1]) + "." + + std::to_string(addr[2]) + "." + std::to_string(addr[3]); } bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { @@ -104,12 +134,12 @@ EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort) { return nullptr; } - channel.connection.port = receivePort; + channel.port = receivePort; channel.in_use = true; if (!startReceiver(channel, receivePort)) { channel.socket.reset(); - channel.connection.port = 0; + channel.port = 0; channel.in_use = false; return nullptr; } @@ -128,44 +158,27 @@ void EsppTransport::onReceive(Ip4Port_t receivePort, std::vector &data, return; } - pbuf *packet_buffer = pbuf_alloc(PBUF_TRANSPORT, - static_cast(data.size()), PBUF_POOL); - if (packet_buffer == nullptr) { - return; - } - - if (!data.empty() && - pbuf_take(packet_buffer, data.data(), static_cast(data.size())) != - ERR_OK) { - pbuf_free(packet_buffer); - return; - } - - udp_pcb pcb{}; - pcb.local_port = receivePort; - - ip_addr_t source_addr{}; - if (!ipaddr_aton(sender.address.c_str(), &source_addr)) { - ip_addr_set_any(IPADDR_TYPE_V4, &source_addr); - } + platform::transport::Ip4AddressBytes remoteAddress{0, 0, 0, 0}; + (void)parseIp4Address(sender.address, remoteAddress); - m_rxCallback(m_callbackArgs, &pcb, packet_buffer, &source_addr, - static_cast(sender.port)); + m_rxCallback(m_callbackArgs, data.data(), data.size(), receivePort, + static_cast(sender.port), remoteAddress); } -const rtps::UdpConnection *EsppTransport::createUdpConnection(Ip4Port_t receivePort) { +bool EsppTransport::ensureReceivePort(Ip4Port_t receivePort) { std::lock_guard lock(m_mutex); Channel *existing = findChannel(receivePort); if (existing != nullptr) { - return &existing->connection; + return true; } Channel *created = createChannel(receivePort); - return created != nullptr ? &created->connection : nullptr; + return created != nullptr; } -bool EsppTransport::joinMultiCastGroup(platform::Ip4Address addr) const { +bool EsppTransport::joinMultiCastGroup( + const platform::transport::Ip4AddressBytes &addr) const { std::lock_guard lock(m_mutex); const std::string group = ip4ToString(addr); @@ -195,22 +208,19 @@ void EsppTransport::sendPacket(PacketInfo &info) { channel = createChannel(info.srcPort); } - if (channel == nullptr || !channel->socket || info.buffer.firstElement == nullptr) { + if (channel == nullptr || !channel->socket) { return; } - const size_t packet_size = info.buffer.firstElement->tot_len; - std::vector bytes(packet_size); - u16_t copied = pbuf_copy_partial(info.buffer.firstElement, bytes.data(), - static_cast(packet_size), 0); - if (copied != static_cast(packet_size)) { + if (info.payload.empty()) { return; } espp::UdpSocket::SendConfig send_config; - send_config.ip_address = ip4ToString(info.destAddr); + const platform::transport::Ip4AddressBytes destination = info.destAddr; + send_config.ip_address = ip4ToString(destination); send_config.port = info.destPort; - send_config.is_multicast_endpoint = ip4_addr_ismulticast(&info.destAddr) != 0; + send_config.is_multicast_endpoint = isMulticastAddress(info.destAddr); - (void)channel->socket->send(bytes, send_config); + (void)channel->socket->send(info.payload, send_config); } diff --git a/components/rtps_embedded/src/communication/UdpDriver.cpp b/components/rtps_embedded/src/communication/UdpDriver.cpp deleted file mode 100644 index 6f6f31a5b..000000000 --- a/components/rtps_embedded/src/communication/UdpDriver.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/* -The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -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 - -This file is part of embeddedRTPS. - -Author: i11 - Embedded Software, RWTH Aachen University -*/ - -#include "rtps/communication/UdpDriver.h" -#include "rtps/communication/TcpipCoreLock.h" -#include "rtps/utils/Lock.h" -#include "rtps/utils/Log.h" - -#include -#include - -using rtps::UdpDriver; - -#if UDP_DRIVER_VERBOSE && RTPS_GLOBAL_VERBOSE -#include "rtps/utils/printutils.h" -#define UDP_DRIVER_LOG(...) \ - if (true) { \ - printf("[UDP Driver] "); \ - printf(__VA_ARGS__); \ - printf("\r\n"); \ - } -#else -#define UDP_DRIVER_LOG(...) // -#endif - -UdpDriver::UdpDriver(rtps::UdpDriver::udpRxFunc_fp callback, void *args) - : m_rxCallback(callback), m_callbackArgs(args) {} - -const rtps::UdpConnection * -UdpDriver::createUdpConnection(Ip4Port_t receivePort) { - for (uint8_t i = 0; i < m_numConns; ++i) { - if (m_conns[i].port == receivePort) { - return &m_conns[i]; - } - } - - if (m_numConns == m_conns.size()) { - return nullptr; - } - - UdpConnection udp_conn(receivePort); - - { - TcpipCoreLock lock; - err_t err = udp_bind(udp_conn.pcb, IP_ADDR_ANY, - receivePort); // to receive multicast - - if (err != ERR_OK && err != ERR_USE) { - return nullptr; - } - - udp_recv(udp_conn.pcb, m_rxCallback, m_callbackArgs); - } - - m_conns[m_numConns] = std::move(udp_conn); - m_numConns++; - - UDP_DRIVER_LOG("Successfully created UDP connection on port %u \n", - receivePort); - - return &m_conns[m_numConns - 1]; -} - -bool UdpDriver::isSameSubnet(ip4_addr_t addr) { - return (ip4_addr_netcmp(&addr, netif_ip4_addr(netif_default), - netif_ip4_netmask(netif_default)) != 0); -} - -bool UdpDriver::isMulticastAddress(ip4_addr_t addr) { -#if IS_LITTLE_ENDIAN - return (addr.addr & 0xF0) == 0xE0; -#else - return (addr.addr >> 28) == 14; -#endif -} - -bool UdpDriver::joinMultiCastGroup(platform::Ip4Address addr) const { - err_t iret; - - { - TcpipCoreLock lock; - iret = igmp_joingroup(ip_2_ip4(IP_ADDR_ANY), (&addr)); - } - - if (iret != ERR_OK) { - - UDP_DRIVER_LOG("Failed to join IGMP multicast group %s\n", - ip4addr_ntoa(&addr)); - - return false; - } else { - - UDP_DRIVER_LOG("Succesfully joined IGMP multicast group %s\n", - ip4addr_ntoa(&addr)); - } - return true; -} - -bool UdpDriver::sendPacket(const UdpConnection &conn, ip4_addr_t &destAddr, - Ip4Port_t destPort, pbuf &buffer) { - err_t err; - { - TcpipCoreLock lock; - ip_addr_t dest; - ip_addr_copy_from_ip4(dest, destAddr); - err = udp_sendto(conn.pcb, &buffer, &dest, destPort); - } - - if (err != ERR_OK) { - ; - - UDP_DRIVER_LOG("UDP TRANSMIT NOT SUCCESSFUL %s:%u size: %u err: %i\n", - ip4addr_ntoa(&destAddr), destPort, buffer.tot_len, err); - - return false; - } - return true; -} - -void UdpDriver::sendPacket(PacketInfo &packet) { - auto p_conn = createUdpConnection(packet.srcPort); - if (p_conn == nullptr) { - ; - - UDP_DRIVER_LOG("Failed to create connection on port %u \n", packet.srcPort); - - return; - } - - sendPacket(*p_conn, packet.destAddr, packet.destPort, - *packet.buffer.firstElement); -} diff --git a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp index 7a95938c5..9e2e3222b 100644 --- a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp +++ b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp @@ -50,10 +50,18 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, uint16_t length; SPDP_LOG("Start deserializing ParticipantProxyData\n"); SPDP_LOG("Buffer has %u bytes remaining\n", ucdr_buffer_remaining(&buffer)); + SPDP_LOG("first 20 bytes of data: "); + for (int i = 0; i < 20 && i < ucdr_buffer_remaining(&buffer); ++i) { + SPDP_LOG("%02x ", buffer.iterator[i]); + } + SPDP_LOG("\n"); + SPDP_LOG("before start, buff length: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); while (ucdr_buffer_remaining(&buffer) >= 4) { ucdr_deserialize_uint16_t(&buffer, reinterpret_cast(&pid)); + SPDP_LOG("buff length after get id: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); ucdr_deserialize_uint16_t(&buffer, &length); + SPDP_LOG("buff length after get length: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); SPDP_LOG("Deserializing parameter with id %u and length %u\n", static_cast(pid), length); if (ucdr_buffer_remaining(&buffer) < length) { @@ -77,11 +85,18 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, } else { ucdr_deserialize_uint8_t(&buffer, &m_protocolVersion.minor); } + SPDP_LOG("Protocol version: %u.%u\n", m_protocolVersion.major, + m_protocolVersion.minor); + SPDP_LOG("buff length: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); break; } case ParameterId::PID_VENDORID: { ucdr_deserialize_array_uint8_t(&buffer, m_vendorId.vendorId.data(), m_vendorId.vendorId.size()); + SPDP_LOG(" vendor id struct size: %d\n", m_vendorId.vendorId.size()); + SPDP_LOG("Vendor ID: %u %u\n", m_vendorId.vendorId[0], + m_vendorId.vendorId[1]); + SPDP_LOG("buff length: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); break; } @@ -169,13 +184,16 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, // SPDP_LOG("unknow ID. could be vender defined.\n"); // should not return false for unknown parameter, just skip it, otherwise we might miss some important information if the remote participant is using some vendor specific parameters that we do not know about. // TODO: GUO: need read out the data for the length of the parameter, otherwise the buffer will be in wrong state and the following parameters cannot be read correctly. For now just skip the data by moving the iterator forward, but we might want to actually read out the data and store it for future use if needed, especially for some vendor specific parameters that we do not know about. - buffer.iterator += length; + // buffer.iterator += length; + ucdr_advance_buffer(&buffer, length); break; } } - // Parameter lists are 4-byte aligned + // Parameter lists are 4-byte aligned uint32_t alignment = ucdr_buffer_alignment(&buffer, 4); - buffer.iterator += alignment; - buffer.last_data_size = 4; + SPDP_LOG("Alignment for next parameter: %lu\n", alignment); + ucdr_advance_buffer(&buffer, alignment); + // buffer.iterator += alignment; + // buffer.last_data_size = 4; } return true; } diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp index 3530d9ef1..21cb24f7e 100644 --- a/components/rtps_embedded/src/discovery/SPDPAgent.cpp +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -28,10 +28,12 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/Reader.h" #include "rtps/entities/Writer.h" #include "rtps/messages/MessageTypes.h" -#include "rtps/platform/threading.h" #include "rtps/utils/Log.h" #include "rtps/utils/udpUtils.h" +#include +#include + using rtps::SPDPAgent; using rtps::SMElement::BuildInEndpointSet; using rtps::SMElement::ParameterId; @@ -57,34 +59,44 @@ void SPDPAgent::start() { return; } m_running = true; - (void)platform::threading::startThread( - "SPDPThread", runBroadcast, this, Config::SPDP_WRITER_STACKSIZE, - Config::SPDP_WRITER_PRIO); + if (!m_broadcastTask) { + espp::Task::Config config; + config.callback = [this]() { + runBroadcast(); + return true; + }; + config.task_config.name = "SPDPThread"; + config.task_config.stack_size_bytes = Config::SPDP_WRITER_STACKSIZE; + config.task_config.priority = Config::SPDP_WRITER_PRIO; + config.log_level = espp::Logger::Verbosity::WARN; + m_broadcastTask = espp::Task::make_unique(config); + } + (void)m_broadcastTask->start(); } -void SPDPAgent::stop() { m_running = false; } - -void SPDPAgent::runBroadcast(void *args) { - SPDPAgent &agent = *static_cast(args); - const DataSize_t size = ucdr_buffer_length(&agent.m_microbuffer); - const uint8_t *payload = agent.m_microbuffer.init; - agent.m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, payload, - size); - while (agent.m_running) { -#ifdef OS_IS_FREERTOS - vTaskDelay(pdMS_TO_TICKS(Config::SPDP_RESEND_PERIOD_MS)); -#else - platform::threading::sleepMs(Config::SPDP_RESEND_PERIOD_MS); -#endif +void SPDPAgent::stop() { + m_running = false; + if (m_broadcastTask) { + m_broadcastTask->stop(); + } +} + +void SPDPAgent::runBroadcast() { + const DataSize_t size = ucdr_buffer_length(&m_microbuffer); + const uint8_t *payload = m_microbuffer.init; + m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, payload, size); + while (m_running) { + std::this_thread::sleep_for( + std::chrono::milliseconds(Config::SPDP_RESEND_PERIOD_MS)); // StatelessWriter drops already-sent history; enqueue a fresh SPDP sample // for each announce cycle. - agent.m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, - payload, size); - if (agent.m_cycleHB == Config::SPDP_CYCLECOUNT_HEARTBEAT) { - agent.m_cycleHB = 0; - agent.mp_participant->checkAndResetHeartbeats(); + m_buildInEndpoints.spdpWriter->newChange(ChangeKind_t::ALIVE, payload, + size); + if (m_cycleHB == Config::SPDP_CYCLECOUNT_HEARTBEAT) { + m_cycleHB = 0; + mp_participant->checkAndResetHeartbeats(); } else { - agent.m_cycleHB++; + m_cycleHB++; } } } @@ -111,6 +123,7 @@ void SPDPAgent::handleSPDPPackage(const ReaderCacheChange &cacheChange) { if (!cacheChange.copyInto(m_inputBuffer.data(), m_inputBuffer.size())) { return; } + SPDP_LOG("SPDPPackage size: %d", cacheChange.size); ucdrBuffer buffer; ucdr_init_buffer(&buffer, m_inputBuffer.data(), m_inputBuffer.size()); @@ -189,7 +202,7 @@ bool SPDPAgent::addProxiesForBuiltInEndpoints() { for (unsigned int i = 0; i < m_proxyDataBuffer.m_metatrafficUnicastLocatorList.size(); i++) { LocatorIPv4 *l = &(m_proxyDataBuffer.m_metatrafficUnicastLocatorList[i]); - if (l->isValid() && l->isSameSubnet()) { + if (l->isValid() && l->isSameSubnet(mp_participant->m_localIpAddress)) { locator = l; break; } @@ -212,11 +225,6 @@ bool SPDPAgent::addProxiesForBuiltInEndpoints() { return false; } -#if SPDP_VERBOSE - ip4_addr_t ip4addr = locator->getIp4Address(); - const char *addr = ip4addr_ntoa(&ip4addr); -#endif - if (m_proxyDataBuffer.hasPublicationReader()) { const ReaderProxy proxy{{m_proxyDataBuffer.m_guid.prefix, ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER}, @@ -289,9 +297,11 @@ void SPDPAgent::addParticipantParameters() { const uint16_t guidSize = sizeof(GuidPrefix_t::id) + entityIdSize; const FullLengthLocator userUniCastLocator = - getUserUnicastLocator(mp_participant->m_participantId); + getUserUnicastLocator(mp_participant->m_participantId, + mp_participant->m_localIpAddress); const FullLengthLocator builtInUniCastLocator = - getBuiltInUnicastLocator(mp_participant->m_participantId); + getBuiltInUnicastLocator(mp_participant->m_participantId, + mp_participant->m_localIpAddress); const FullLengthLocator builtInMultiCastLocator = getBuiltInMulticastLocator(); diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps_embedded/src/entities/Domain.cpp index 2d9b9f6e4..9e85592bb 100644 --- a/components/rtps_embedded/src/entities/Domain.cpp +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -44,27 +44,30 @@ Author: i11 - Embedded Software, RWTH Aachen University using rtps::Domain; -Domain::Domain() +Domain::Domain(const platform::transport::Ip4AddressBytes &localIpAddress) : m_threadPool(receiveJumppad, this), - m_defaultTransport(ThreadPool::readCallback, &m_threadPool), - m_transport(&m_defaultTransport) { + m_defaultTransport(ThreadPool::onDatagram, &m_threadPool), + m_transport(&m_defaultTransport), + m_localIpAddress(localIpAddress) { initializeTransport(); createMutex(&m_mutex); } -Domain::Domain(platform::transport::ITransport &transport) +Domain::Domain(platform::transport::ITransport &transport, + const platform::transport::Ip4AddressBytes &localIpAddress) : m_threadPool(receiveJumppad, this), - m_defaultTransport(ThreadPool::readCallback, &m_threadPool), - m_transport(&transport) { + m_defaultTransport(ThreadPool::onDatagram, &m_threadPool), + m_transport(&transport), + m_localIpAddress(localIpAddress) { initializeTransport(); createMutex(&m_mutex); } void Domain::initializeTransport() { assert(m_transport != nullptr); - m_transport->createUdpConnection(getUserMulticastPort()); - m_transport->createUdpConnection(getBuiltInMulticastPort()); - m_transport->joinMultiCastGroup(transformIP4ToU32(239, 255, 0, 1)); + m_transport->ensureReceivePort(getUserMulticastPort()); + m_transport->ensureReceivePort(getBuiltInMulticastPort()); + m_transport->joinMultiCastGroup({239, 255, 0, 1}); } Domain::~Domain() { stop(); } @@ -90,19 +93,19 @@ void Domain::receiveJumppad(void *callee, const PacketInfo &packet) { } void Domain::receiveCallback(const PacketInfo &packet) { - if (packet.buffer.firstElement->next != nullptr) { - - DOMAIN_LOG("Cannot handle multiple elements chained. You might " - "want to increase PBUF_POOL_BUFSIZE\n"); + if (packet.payload.empty()) { + DOMAIN_LOG("Dropping packet without payload\n"); + return; } + const uint8_t *payload = packet.payload.data(); + DataSize_t payload_size = static_cast(packet.payload.size()); + if (isMetaMultiCastPort(packet.destPort)) { // Pass to all DOMAIN_LOG("Domain: Multicast to port %u\n", packet.destPort); for (auto i = 0; i < m_nextParticipantId - PARTICIPANT_START_ID; ++i) { - m_participants[i].newMessage( - static_cast(packet.buffer.firstElement->payload), - packet.buffer.firstElement->len); + m_participants[i].newMessage(payload, payload_size); } // First Check if UserTraffic Multicast } else if (isUserMultiCastPort(packet.destPort)) { @@ -113,9 +116,7 @@ void Domain::receiveCallback(const PacketInfo &packet) { for (auto i = 0; i < m_nextParticipantId - PARTICIPANT_START_ID; ++i) { if (m_participants[i].hasReaderWithMulticastLocator(packet.destAddr)) { DOMAIN_LOG("Domain: Forward Multicast only to Participant: %u\n", i); - m_participants[i].newMessage( - static_cast(packet.buffer.firstElement->payload), - packet.buffer.firstElement->len); + m_participants[i].newMessage(payload, payload_size); } } } else { @@ -128,8 +129,7 @@ void Domain::receiveCallback(const PacketInfo &packet) { id >= PARTICIPANT_START_ID) { // added extra check to avoid segfault // (id below START_ID) m_participants[id - PARTICIPANT_START_ID].newMessage( - static_cast(packet.buffer.firstElement->payload), - packet.buffer.firstElement->len); + payload, payload_size); } else { DOMAIN_LOG("Domain: Participant id too high or unplausible.\n"); } @@ -151,7 +151,8 @@ rtps::Participant *Domain::createParticipant() { } auto &entry = m_participants[nextSlot]; - entry.reuse(generateGuidPrefix(m_nextParticipantId), m_nextParticipantId); + entry.reuse(generateGuidPrefix(m_nextParticipantId), m_nextParticipantId, + m_localIpAddress); registerPort(entry); createBuiltinWritersAndReaders(entry); ++m_nextParticipantId; @@ -199,7 +200,7 @@ void Domain::createBuiltinWritersAndReaders(Participant &part) { sedpAttributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; sedpAttributes.endpointGuid.prefix = part.m_guidPrefix; sedpAttributes.unicastLocator = - getBuiltInUnicastLocator(part.m_participantId); + getBuiltInUnicastLocator(part.m_participantId, m_localIpAddress); // READER StatefulReader *sedpPubReader = @@ -246,14 +247,14 @@ void Domain::createBuiltinWritersAndReaders(Participant &part) { } void Domain::registerPort(const Participant &part) { - m_transport->createUdpConnection(getUserUnicastPort(part.m_participantId)); - m_transport->createUdpConnection(getBuiltInUnicastPort(part.m_participantId)); + m_transport->ensureReceivePort(getUserUnicastPort(part.m_participantId)); + m_transport->ensureReceivePort(getBuiltInUnicastPort(part.m_participantId)); m_threadPool.addBuiltinPort(getBuiltInUnicastPort(part.m_participantId)); } void Domain::registerMulticastPort(FullLengthLocator mcastLocator) { if (mcastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { - m_transport->createUdpConnection(mcastLocator.getLocatorPort()); + m_transport->ensureReceivePort(mcastLocator.getLocatorPort()); } } @@ -379,7 +380,8 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, attributes.endpointGuid.entityId = { part.getNextUserEntityKey(), EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; - attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); + attributes.unicastLocator = + getUserUnicastLocator(part.m_participantId, m_localIpAddress); attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; DOMAIN_LOG("Creating writer[%s, %s]\n", topicName, typeName); @@ -415,7 +417,8 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, rtps::Reader *Domain::createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, - ip4_addr_t mcastaddress) { + platform::transport::Ip4AddressBytes + mcastaddress) { Lock lock{m_mutex}; StatelessReader *statelessReader = getNextUnusedEndpoint( @@ -445,15 +448,18 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, attributes.endpointGuid.entityId = { part.getNextUserEntityKey(), EntityKind_t::USER_DEFINED_READER_WITHOUT_KEY}; - attributes.unicastLocator = getUserUnicastLocator(part.m_participantId); + attributes.unicastLocator = + getUserUnicastLocator(part.m_participantId, m_localIpAddress); if (!isZeroAddress(mcastaddress)) { - if (ip4_addr_ismulticast(&mcastaddress)) { + if (isMulticastAddress(mcastaddress)) { attributes.multicastLocator = rtps::FullLengthLocator::createUDPv4Locator( - ip4_addr1(&mcastaddress), ip4_addr2(&mcastaddress), - ip4_addr3(&mcastaddress), ip4_addr4(&mcastaddress), + mcastaddress[0], mcastaddress[1], mcastaddress[2], mcastaddress[3], getUserMulticastPort()); - m_transport->joinMultiCastGroup( - attributes.multicastLocator.getIp4Address()); + m_transport->joinMultiCastGroup( + {attributes.multicastLocator.address[12], + attributes.multicastLocator.address[13], + attributes.multicastLocator.address[14], + attributes.multicastLocator.address[15]}); registerMulticastPort(attributes.multicastLocator); DOMAIN_LOG("Multicast enabled!\n"); diff --git a/components/rtps_embedded/src/entities/Participant.cpp b/components/rtps_embedded/src/entities/Participant.cpp index 57efa137a..fe64c31d1 100644 --- a/components/rtps_embedded/src/entities/Participant.cpp +++ b/components/rtps_embedded/src/entities/Participant.cpp @@ -65,6 +65,16 @@ void Participant::reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId) { m_guidPrefix = guidPrefix; m_participantId = participantId; + m_localIpAddress = {Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], + Config::IP_ADDRESS[2], Config::IP_ADDRESS[3]}; +} + +void Participant::reuse( + const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, + const platform::transport::Ip4AddressBytes &localIpAddress) { + m_guidPrefix = guidPrefix; + m_participantId = participantId; + m_localIpAddress = localIpAddress; } bool Participant::isValid() { @@ -381,13 +391,15 @@ void Participant::refreshRemoteParticipantLiveliness( } } -bool Participant::hasReaderWithMulticastLocator(ip4_addr_t address) { +bool Participant::hasReaderWithMulticastLocator( + const std::array &address) { Lock lock{m_mutex}; for (uint8_t i = 0; i < m_readers.size(); i++) { if (m_readers[i] == nullptr) { continue; } - if (m_readers[i]->m_attributes.multicastLocator.isSameAddress(&address)) { + if (m_readers[i]->m_attributes.multicastLocator.getIp4AddressBytes() == + address) { return true; } } diff --git a/components/rtps_embedded/src/entities/Writer.cpp b/components/rtps_embedded/src/entities/Writer.cpp index be21ffee0..aff59e0ba 100644 --- a/components/rtps_embedded/src/entities/Writer.cpp +++ b/components/rtps_embedded/src/entities/Writer.cpp @@ -70,10 +70,10 @@ void rtps::Writer::manageSendOptions() { for (auto &avproxy : m_proxies) { if (avproxy.remoteMulticastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4 && - avproxy.remoteMulticastLocator.getIp4Address().addr == - proxy.remoteMulticastLocator.getIp4Address().addr && - avproxy.remoteLocator.getIp4Address().addr != - proxy.remoteLocator.getIp4Address().addr) { + avproxy.remoteMulticastLocator.getIp4AddressBytes() == + proxy.remoteMulticastLocator.getIp4AddressBytes() && + avproxy.remoteLocator.getIp4AddressBytes() != + proxy.remoteLocator.getIp4AddressBytes()) { if (avproxy.suppressUnicast == false) { avproxy.useMulticast = false; avproxy.suppressUnicast = true; diff --git a/components/rtps_embedded/src/platform/sync.cpp b/components/rtps_embedded/src/platform/sync.cpp index 3299dcb8b..768fe6e3a 100644 --- a/components/rtps_embedded/src/platform/sync.cpp +++ b/components/rtps_embedded/src/platform/sync.cpp @@ -1,5 +1,7 @@ #include "rtps/platform/sync.h" +#include + namespace rtps { namespace platform { namespace sync { @@ -8,7 +10,8 @@ bool createRecursiveMutex(RecursiveMutexHandle *mutex) { if (mutex == nullptr) { return false; } - *mutex = xSemaphoreCreateRecursiveMutex(); + + *mutex = new (std::nothrow) std::recursive_mutex(); return *mutex != nullptr; } @@ -16,14 +19,18 @@ bool lockRecursive(RecursiveMutexHandle mutex) { if (mutex == nullptr) { return false; } - return xSemaphoreTakeRecursive(mutex, portMAX_DELAY) == pdTRUE; + + mutex->lock(); + return true; } bool unlockRecursive(RecursiveMutexHandle mutex) { if (mutex == nullptr) { return false; } - return xSemaphoreGiveRecursive(mutex) == pdTRUE; + + mutex->unlock(); + return true; } } // namespace sync diff --git a/components/rtps_embedded/src/rtps.cpp b/components/rtps_embedded/src/rtps.cpp deleted file mode 100644 index e086fd933..000000000 --- a/components/rtps_embedded/src/rtps.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/* -The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -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 - -This file is part of embeddedRTPS. - -Author: i11 - Embedded Software, RWTH Aachen University -*/ - -#include "rtps/rtps.h" -#include "rtps/platform/bootstrap.h" -#include -#include - -// Currently initialization is expected to be done in project main for e.g. -// Aurix and STM32 -#if defined(unix) || defined(__unix__) || defined(WIN32) || defined(_WIN32) || \ - defined(__WIN32) && !defined(__CYGWIN__) - -#include "lwip/ip4_addr.h" -#include "lwip/netif.h" -#include "lwipcfg.h" -#include - -#if defined(unix) || defined(__unix__) -#include "netif/tapif.h" -#elif defined(WIN32) || defined(_WIN32) || \ - defined(__WIN32) && !defined(__CYGWIN__) -#include "../pcapif.h" -#include "default_netif.h" -#else -#include "ethernetif.h" -#endif - -#define INIT_VERBOSE 0 - -static struct netif netif; - -static void init(void *arg) { - if (arg == nullptr) { -#if INIT_VERBOSE - printf("Failed to init. nullptr passed"); -#endif - return; - } - auto init_sem = static_cast(arg); - - srand((unsigned int)time(nullptr)); - - ip4_addr_t ipaddr; - ip4_addr_t netmask; - ip4_addr_t gw; - LWIP_PORT_INIT_GW(&gw); - LWIP_PORT_INIT_IPADDR(&ipaddr); - LWIP_PORT_INIT_NETMASK(&netmask); -#if INIT_VERBOSE - printf("Starting lwIP, local interface IP is %s\n", ip4addr_ntoa(&ipaddr)); -#endif - -#if defined(unix) || defined(__unix__) - netif_add(&netif, &ipaddr, &netmask, &gw, nullptr, tapif_init, tcpip_input); -#elif defined(WIN32) || defined(_WIN32) || \ - defined(__WIN32) && !defined(__CYGWIN__) - netif_add(&netif, &ipaddr, &netmask, &gw, nullptr, pcapif_init, tcpip_input); -#else - netif_add(&netif, &ipaddr, &netmask, &gw, nullptr, ethernetif_init, - tcpip_input); -#endif - - netif_set_default(&netif); - netif_set_up(netif_default); - - platform::bootstrap::signalInitSemaphore(init_sem); -} - -void LwIPInit() { - /* no stdio-buffering, please! */ - setvbuf(stdout, nullptr, _IONBF, 0); - - platform::bootstrap::InitSemaphoreHandle init_sem; - - bool sem_created = platform::bootstrap::createInitSemaphore(&init_sem); - LWIP_ASSERT("failed to create init_sem", sem_created); - LWIP_UNUSED_ARG(sem_created); - tcpip_init(init, &init_sem); - /* we have to wait for initialization to finish before - * calling update_adapter()! */ - platform::bootstrap::waitInitSemaphore(&init_sem); - platform::bootstrap::freeInitSemaphore(&init_sem); -} - -void rtps::init() { - // TODO This is not threadsafe. Might cause problems in tests. For now, it - // seems to work. - static bool initialized = false; - if (!initialized) { - LwIPInit(); - initialized = true; - } -} -#endif - -#undef INIT_VERBOSE diff --git a/components/rtps_embedded/src/storages/PBufWrapper.cpp b/components/rtps_embedded/src/storages/PBufWrapper.cpp deleted file mode 100644 index c183f049f..000000000 --- a/components/rtps_embedded/src/storages/PBufWrapper.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/* -The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -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 - -This file is part of embeddedRTPS. - -Author: i11 - Embedded Software, RWTH Aachen University -*/ - -// Copyright 2023 Apex.AI, Inc. -// All rights reserved. - -#include "rtps/storages/PBufWrapper.h" -#include "rtps/utils/Log.h" - -using rtps::PBufWrapper; - -#if PBUF_WRAP_VERBOSE && RTPS_GLOBAL_VERBOSE -#include "rtps/utils/printutils.h" -#define PBUF_WRAP_LOG(...) \ - if (true) { \ - printf("[PBUF Wrapper] "); \ - printf(__VA_ARGS__); \ - printf("\n"); \ - } -#else -#define PBUF_WRAP_LOG(...) // -#endif - -PBufWrapper::PBufWrapper(pbuf *bufferToWrap) : firstElement(bufferToWrap) { - m_freeSpace = 0; // Assume it to be full -} - -PBufWrapper::PBufWrapper(DataSize_t length) - : firstElement(pbuf_alloc(m_layer, length, m_type)) { - - if (isValid()) { - m_freeSpace = length; - } -} - -// TODO: Uses move assignment. Improvement possible -PBufWrapper::PBufWrapper(PBufWrapper &&other) noexcept { - *this = std::move(other); -} - -PBufWrapper &PBufWrapper::operator=(PBufWrapper &&other) noexcept { - copySimpleMembersAndResetBuffer(other); - - if (other.firstElement != nullptr) { - firstElement = other.firstElement; - other.firstElement = nullptr; - } - return *this; -} - -void PBufWrapper::copySimpleMembersAndResetBuffer(const PBufWrapper &other) { - m_freeSpace = other.m_freeSpace; - - if (firstElement != nullptr) { - pbuf_free(firstElement); - firstElement = nullptr; - } -} - -void PBufWrapper::destroy() -{ - if (firstElement != nullptr) { - pbuf_free(firstElement); - firstElement = nullptr; - } - m_freeSpace = 0; -} - -PBufWrapper::~PBufWrapper() { - destroy(); -} - -bool PBufWrapper::isValid() const { return firstElement != nullptr; } - -rtps::DataSize_t PBufWrapper::spaceLeft() const { return m_freeSpace; } - -rtps::DataSize_t PBufWrapper::spaceUsed() const { - if (firstElement == nullptr) { - return 0; - } - - return firstElement->tot_len - m_freeSpace; -} - -bool PBufWrapper::append(const uint8_t *data, DataSize_t length) { - if (data == nullptr) { - return false; - } - - err_t err = pbuf_take_at(firstElement, data, length, spaceUsed()); - if (err != ERR_OK) { - return false; - } - m_freeSpace -= length; - return true; -} - -void PBufWrapper::append(const PBufWrapper &other) { - if (this->firstElement == nullptr) { - m_freeSpace = other.m_freeSpace; - this->firstElement = other.firstElement; - pbuf_ref(this->firstElement); - return; - } - - m_freeSpace += other.m_freeSpace; - pbuf_chain(this->firstElement, other.firstElement); -} - -bool PBufWrapper::reserve(DataSize_t length) { - int16_t additionalAllocation = length - m_freeSpace; - if (additionalAllocation <= 0) { - return true; - } - - return increaseSizeBy(additionalAllocation); -} - -void PBufWrapper::reset() { - if (firstElement != nullptr) { - m_freeSpace = firstElement->tot_len; - } -} - -bool PBufWrapper::increaseSizeBy(uint16_t length) { - pbuf *allocation = pbuf_alloc(m_layer, length, m_type); - if (allocation == nullptr) { - return false; - } - - m_freeSpace += length; - - if (firstElement == nullptr) { - firstElement = allocation; - } else { - pbuf_cat(firstElement, allocation); - } - - return true; -} - -#undef PBUF_WRAP_VERBOSE diff --git a/components/socket/CMakeLists.txt b/components/socket/CMakeLists.txt index 5bc6ad34a..6c90d2f4e 100644 --- a/components/socket/CMakeLists.txt +++ b/components/socket/CMakeLists.txt @@ -1,4 +1,4 @@ idf_component_register( INCLUDE_DIRS "include" SRC_DIRS "src" - REQUIRES base_component task lwip) + REQUIRES base_component task ) diff --git a/components/thread_pool/CMakeLists.txt b/components/thread_pool/CMakeLists.txt new file mode 100644 index 000000000..cb1e7ccab --- /dev/null +++ b/components/thread_pool/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src" + REQUIRES base_component task) diff --git a/components/thread_pool/README.md b/components/thread_pool/README.md new file mode 100644 index 000000000..271e12ce7 --- /dev/null +++ b/components/thread_pool/README.md @@ -0,0 +1,13 @@ +# Thread Pool Component + +The `ThreadPool` component provides a reusable pool of worker tasks for executing queued jobs asynchronously. + +It is implemented with `espp::Task` workers and `std::condition_variable` synchronization. + +## Features + +- Configurable worker count +- Bounded or unbounded queue +- Optional blocking submit mode for backpressure +- Graceful stop (drains queued jobs) +- Thread-safe stats for submitted / executed / rejected jobs diff --git a/components/thread_pool/example/CMakeLists.txt b/components/thread_pool/example/CMakeLists.txt new file mode 100644 index 000000000..d480eb92c --- /dev/null +++ b/components/thread_pool/example/CMakeLists.txt @@ -0,0 +1,22 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +# add the component directories that we want to use +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py thread_pool" + CACHE STRING + "List of components to include" + ) + +project(thread_pool_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/thread_pool/example/README.md b/components/thread_pool/example/README.md new file mode 100644 index 000000000..4c0790c06 --- /dev/null +++ b/components/thread_pool/example/README.md @@ -0,0 +1,17 @@ +# Thread Pool Example + +This example shows how to: + +- Create an `espp::ThreadPool` +- Submit jobs +- Wait for all jobs to complete +- Read execution statistics + +## Build + +From this directory, run: + +```bash +idf.py set-target esp32 +idf.py build flash monitor +``` diff --git a/components/thread_pool/example/main/CMakeLists.txt b/components/thread_pool/example/main/CMakeLists.txt new file mode 100644 index 000000000..a941e22ba --- /dev/null +++ b/components/thread_pool/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/thread_pool/example/main/thread_pool_example.cpp b/components/thread_pool/example/main/thread_pool_example.cpp new file mode 100644 index 000000000..4191e5b25 --- /dev/null +++ b/components/thread_pool/example/main/thread_pool_example.cpp @@ -0,0 +1,67 @@ +#include +#include +#include +#include +#include + +#include "logger.hpp" +#include "thread_pool.hpp" + +using namespace std::chrono_literals; + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "ThreadPool Example", .level = espp::Logger::Verbosity::INFO}); + + std::mutex done_mutex; + std::condition_variable done_cv; + constexpr int total_jobs = 8; + std::atomic completed_jobs = 0; + + espp::ThreadPool pool({ + .worker_count = 2, + .max_queue_size = 16, + .auto_start = true, + .block_on_submit_when_full = false, + .worker_task_config = { + .name = "tp_worker", + .stack_size_bytes = 4096, + .priority = 5, + .core_id = -1, + }, + .log_level = espp::Logger::Verbosity::WARN, + }); + + logger.info("Submitting {} jobs", total_jobs); + + for (int i = 0; i < total_jobs; ++i) { + bool queued = pool.submit([&, i]() { + std::this_thread::sleep_for(100ms); + int done = ++completed_jobs; + logger.info("Job {} done ({}/{})", i, done, total_jobs); + if (done == total_jobs) { + std::lock_guard lock(done_mutex); + done_cv.notify_one(); + } + }); + + if (!queued) { + logger.error("Failed to queue job {}", i); + } + } + + { + std::unique_lock lock(done_mutex); + done_cv.wait(lock, [&]() { return completed_jobs.load() == total_jobs; }); + } + + auto stats = pool.stats(); + logger.info("ThreadPool stats: submitted={} executed={} rejected={}", stats.submitted, + stats.executed, stats.rejected); + + pool.stop(); + logger.info("ThreadPool example complete"); + + while (true) { + std::this_thread::sleep_for(1s); + } +} diff --git a/components/thread_pool/example/sdkconfig.defaults b/components/thread_pool/example/sdkconfig.defaults new file mode 100644 index 000000000..75ebeae1c --- /dev/null +++ b/components/thread_pool/example/sdkconfig.defaults @@ -0,0 +1 @@ +CONFIG_COMPILER_CXX_EXCEPTIONS=y diff --git a/components/thread_pool/idf_component.yml b/components/thread_pool/idf_component.yml new file mode 100644 index 000000000..71a535f71 --- /dev/null +++ b/components/thread_pool/idf_component.yml @@ -0,0 +1,21 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Generic thread pool component implemented with espp::Task and condition variables." +url: "https://github.com/esp-cpp/espp/tree/main/components/thread_pool" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/core/thread_pool.html" +examples: + - path: example +tags: + - cpp + - Component + - ThreadPool + - Task + - Concurrent +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' + espp/task: '>=1.0' diff --git a/components/thread_pool/include/thread_pool.hpp b/components/thread_pool/include/thread_pool.hpp new file mode 100644 index 000000000..b2f903c57 --- /dev/null +++ b/components/thread_pool/include/thread_pool.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "base_component.hpp" +#include "task.hpp" + +namespace espp { + +class ThreadPool : public espp::BaseComponent { +public: + using Job = std::function; + + struct Stats { + std::uint64_t submitted = 0; + std::uint64_t executed = 0; + std::uint64_t rejected = 0; + }; + + struct Config { + std::size_t worker_count = 1; + std::size_t max_queue_size = 0; // 0 means unbounded + bool auto_start = true; + bool block_on_submit_when_full = false; + espp::Task::BaseConfig worker_task_config = { + .name = "thread_pool_worker", + .stack_size_bytes = 4096, + .priority = 5, + .core_id = -1, + }; + espp::Logger::Verbosity log_level = espp::Logger::Verbosity::WARN; + }; + + explicit ThreadPool(const Config &config); + ~ThreadPool(); + + void start(); + void stop(); + + bool is_running() const; + + bool submit(const Job &job); + bool try_submit(const Job &job); + + std::size_t queue_size() const; + std::size_t worker_count() const; + + Stats stats() const; + +private: + bool worker_task_fn(std::mutex &task_mutex, std::condition_variable &task_cv, + bool &task_notified); + + bool submit_impl(const Job &job, bool allow_blocking_when_full); + + Config config_; + + mutable std::mutex queue_mutex_; + std::condition_variable queue_has_work_cv_; + std::condition_variable queue_has_space_cv_; + std::deque queue_; + + std::vector> workers_; + std::atomic running_{false}; + bool stopping_ = false; + + std::atomic submitted_{0}; + std::atomic executed_{0}; + std::atomic rejected_{0}; +}; + +} // namespace espp diff --git a/components/thread_pool/src/thread_pool.cpp b/components/thread_pool/src/thread_pool.cpp new file mode 100644 index 000000000..55b6d2faa --- /dev/null +++ b/components/thread_pool/src/thread_pool.cpp @@ -0,0 +1,150 @@ +#include "thread_pool.hpp" + +#include + +using namespace espp; + +ThreadPool::ThreadPool(const Config &config) + : BaseComponent("ThreadPool", config.log_level), config_(config) { + if (config_.worker_count == 0) { + config_.worker_count = 1; + } + + workers_.reserve(config_.worker_count); + for (std::size_t i = 0; i < config_.worker_count; ++i) { + auto worker_config = config_.worker_task_config; + worker_config.name = config_.worker_task_config.name + "_" + std::to_string(i); + using namespace std::placeholders; + workers_.emplace_back(espp::Task::make_unique({ + .callback = std::bind(&ThreadPool::worker_task_fn, this, _1, _2, _3), + .task_config = worker_config, + .log_level = config_.log_level, + })); + } + + if (config_.auto_start) { + start(); + } +} + +ThreadPool::~ThreadPool() { stop(); } + +void ThreadPool::start() { + if (running_.exchange(true)) { + return; + } + + { + std::lock_guard lock(queue_mutex_); + stopping_ = false; + } + + for (auto &worker : workers_) { + worker->start(); + } +} + +void ThreadPool::stop() { + if (!running_.exchange(false)) { + return; + } + + { + std::lock_guard lock(queue_mutex_); + stopping_ = true; + } + + queue_has_work_cv_.notify_all(); + queue_has_space_cv_.notify_all(); + + for (auto &worker : workers_) { + worker->stop(); + } +} + +bool ThreadPool::is_running() const { return running_.load(); } + +bool ThreadPool::submit(const Job &job) { + return submit_impl(job, config_.block_on_submit_when_full); +} + +bool ThreadPool::try_submit(const Job &job) { return submit_impl(job, false); } + +bool ThreadPool::submit_impl(const Job &job, bool allow_blocking_when_full) { + if (!job) { + rejected_++; + return false; + } + + std::unique_lock lock(queue_mutex_); + if (!running_.load() || stopping_) { + rejected_++; + return false; + } + + if (config_.max_queue_size > 0) { + if (allow_blocking_when_full) { + queue_has_space_cv_.wait(lock, [&]() { + return stopping_ || queue_.size() < config_.max_queue_size; + }); + if (stopping_) { + rejected_++; + return false; + } + } else if (queue_.size() >= config_.max_queue_size) { + rejected_++; + return false; + } + } + + queue_.push_back(job); + submitted_++; + lock.unlock(); + queue_has_work_cv_.notify_one(); + return true; +} + +std::size_t ThreadPool::queue_size() const { + std::lock_guard lock(queue_mutex_); + return queue_.size(); +} + +std::size_t ThreadPool::worker_count() const { return workers_.size(); } + +ThreadPool::Stats ThreadPool::stats() const { + return { + .submitted = submitted_.load(), + .executed = executed_.load(), + .rejected = rejected_.load(), + }; +} + +bool ThreadPool::worker_task_fn(std::mutex &task_mutex, + std::condition_variable &task_cv, + bool &task_notified) { + (void)task_cv; + + Job job; + { + std::unique_lock lock(queue_mutex_); + queue_has_work_cv_.wait(lock, [&]() { return stopping_ || !queue_.empty(); }); + + if (queue_.empty()) { + std::unique_lock task_lock(task_mutex); + return stopping_ || task_notified; + } + + job = std::move(queue_.front()); + queue_.pop_front(); + + if (config_.max_queue_size > 0) { + queue_has_space_cv_.notify_one(); + } + } + + job(); + executed_++; + + std::unique_lock task_lock(task_mutex); + return task_notified; +} From ea3f3a5b35c82f675469bd61879da4e129291178 Mon Sep 17 00:00:00 2001 From: Guo Date: Mon, 13 Jul 2026 10:00:13 -0500 Subject: [PATCH 07/17] fix bug of sedp acknack; fix bug for parse topic data --- .../include/rtps/entities/StatefulReader.tpp | 4 +++- .../include/rtps/messages/MessageTypes.h | 5 +---- .../rtps_embedded/include/rtps/utils/Log.h | 6 +++--- .../rtps_embedded/src/discovery/TopicData.cpp | 18 +++++++++++------- 4 files changed, 18 insertions(+), 15 deletions(-) diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp index 3be4c0282..249be6785 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -115,7 +115,9 @@ bool StatefulReaderT::onNewGapMessage( if (!m_is_initialized_) { return false; } - SFR_LOG("Processing gap message %u %u", msg.gapStart, msg.gapList.base); + SFR_LOG("Processing gap message %d.%u %d.%u", (int)msg.gapStart.high, + (unsigned int)msg.gapStart.low, (int)msg.gapList.base.high, + (unsigned int)msg.gapList.base.low); Guid_t writerProxyGuid; writerProxyGuid.prefix = remotePrefix; diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.h b/components/rtps_embedded/include/rtps/messages/MessageTypes.h index 0d62cbbda..471a1da4d 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageTypes.h +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.h @@ -234,7 +234,7 @@ struct SubmessageAckNack { static uint16_t getRawSize(const SequenceNumberSet &set) { uint16_t bitMapSize = 0; if (set.numBits != 0) { - bitMapSize = SNS_NUM_BYTES; + bitMapSize = static_cast(4 * (((set.numBits - 1) / 32) + 1)); } return getRawSizeWithoutSNSet() + sizeof(SequenceNumber_t) + sizeof(uint32_t) + bitMapSize; // SequenceNumberSet @@ -398,9 +398,6 @@ bool serializeMessage(Buffer &buffer, SubmessageGap &msg) { buffer.append(reinterpret_cast(&msg.gapList.numBits), sizeof(uint32_t)); - buffer.append(reinterpret_cast(&msg.gapList.numBits), - sizeof(uint32_t)); - return true; } diff --git a/components/rtps_embedded/include/rtps/utils/Log.h b/components/rtps_embedded/include/rtps/utils/Log.h index 8c195c2cf..1026da704 100644 --- a/components/rtps_embedded/include/rtps/utils/Log.h +++ b/components/rtps_embedded/include/rtps/utils/Log.h @@ -31,7 +31,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #define RTPS_GLOBAL_VERBOSE 1 #define SFW_VERBOSE 0 -#define SPDP_VERBOSE 1 +#define SPDP_VERBOSE 0 #define PBUF_WRAP_VERBOSE 0 #define SEDP_VERBOSE 1 #define RECV_VERBOSE 0 @@ -39,8 +39,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #define DOMAIN_VERBOSE 0 #define UDP_DRIVER_VERBOSE 1 #define TSCB_VERBOSE 0 -#define SLW_VERBOSE 1 -#define SFR_VERBOSE 0 +#define SLW_VERBOSE 0 +#define SFR_VERBOSE 1 #define SLR_VERBOSE 1 #define THREAD_POOL_VERBOSE 0 diff --git a/components/rtps_embedded/src/discovery/TopicData.cpp b/components/rtps_embedded/src/discovery/TopicData.cpp index 783d6b83d..9f0ad73db 100644 --- a/components/rtps_embedded/src/discovery/TopicData.cpp +++ b/components/rtps_embedded/src/discovery/TopicData.cpp @@ -51,9 +51,11 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { while (ucdr_buffer_remaining(&buffer) >= 4) { if (ucdr_buffer_has_error(&buffer)) { - while (1) { - printf("FAILED TO DESERIALIZE TOPIC DATA\n"); - } + printf("FAILED TO DESERIALIZE TOPIC DATA\n"); + return false; + // while (1) { + // printf("FAILED TO DESERIALIZE TOPIC DATA\n"); + // } } ParameterId pid; uint16_t length; @@ -138,13 +140,15 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { } } break; default: - buffer.iterator += length; - buffer.last_data_size = 1; + ucdr_advance_buffer(&buffer, length); + // buffer.iterator += length; + // buffer.last_data_size = 1; } uint32_t alignment = ucdr_buffer_alignment(&buffer, 4); - buffer.iterator += alignment; - buffer.last_data_size = 4; // 4 Byte alignment per element + ucdr_advance_buffer(&buffer, alignment); + // buffer.iterator += alignment; + // buffer.last_data_size = 4; // 4 Byte alignment per element } return ucdr_buffer_remaining(&buffer) == 0; } From 939acff939e5a2ad22371f0dfdfe9d72b32455db Mon Sep 17 00:00:00 2001 From: Guo Date: Tue, 14 Jul 2026 13:34:18 -0500 Subject: [PATCH 08/17] removed sync and fix circular buffer bug --- components/rtps_embedded/CMakeLists.txt | 2 - .../rtps_embedded/include/rtps/config_esp32.h | 2 +- .../include/rtps/discovery/SEDPAgent.h | 5 +- .../include/rtps/discovery/SPDPAgent.h | 4 +- .../include/rtps/entities/Domain.h | 4 +- .../include/rtps/entities/Participant.h | 4 +- .../include/rtps/entities/Reader.h | 6 +- .../include/rtps/entities/StatefulReader.tpp | 10 +-- .../include/rtps/entities/StatefulWriter.tpp | 22 +++---- .../include/rtps/entities/StatelessWriter.tpp | 16 ++--- .../include/rtps/entities/Writer.h | 4 +- .../include/rtps/platform/sync.h | 20 ------ .../rtps/storages/ThreadSafeCircularBuffer.h | 7 ++- .../storages/ThreadSafeCircularBuffer.tpp | 52 ++++++++++----- .../rtps_embedded/include/rtps/utils/Lock.h | 54 ---------------- .../rtps_embedded/include/rtps/utils/Log.h | 6 +- components/rtps_embedded/src/ThreadPool.cpp | 10 +-- .../rtps_embedded/src/discovery/SEDPAgent.cpp | 21 +++---- .../rtps_embedded/src/discovery/SPDPAgent.cpp | 7 +-- .../rtps_embedded/src/entities/Domain.cpp | 15 +++-- .../src/entities/Participant.cpp | 63 ++++++++----------- .../rtps_embedded/src/entities/Reader.cpp | 34 +++------- .../src/entities/StatelessReader.cpp | 1 - .../rtps_embedded/src/entities/Writer.cpp | 13 ++-- .../rtps_embedded/src/platform/sync.cpp | 38 ----------- components/rtps_embedded/src/utils/Lock.cpp | 21 ------- 26 files changed, 141 insertions(+), 300 deletions(-) delete mode 100644 components/rtps_embedded/include/rtps/platform/sync.h delete mode 100644 components/rtps_embedded/include/rtps/utils/Lock.h delete mode 100644 components/rtps_embedded/src/platform/sync.cpp delete mode 100644 components/rtps_embedded/src/utils/Lock.cpp diff --git a/components/rtps_embedded/CMakeLists.txt b/components/rtps_embedded/CMakeLists.txt index e8d158f38..4d13bd42f 100644 --- a/components/rtps_embedded/CMakeLists.txt +++ b/components/rtps_embedded/CMakeLists.txt @@ -13,9 +13,7 @@ idf_component_register( "src/entities/Writer.cpp" "src/messages/MessageReceiver.cpp" "src/messages/MessageTypes.cpp" - "src/platform/sync.cpp" "src/utils/Diagnostics.cpp" - "src/utils/Lock.cpp" "thirdparty/Micro-CDR/src/c/common.c" "thirdparty/Micro-CDR/src/c/types/array.c" "thirdparty/Micro-CDR/src/c/types/basic.c" diff --git a/components/rtps_embedded/include/rtps/config_esp32.h b/components/rtps_embedded/include/rtps/config_esp32.h index 5cdd2fff8..80e1323d1 100644 --- a/components/rtps_embedded/include/rtps/config_esp32.h +++ b/components/rtps_embedded/include/rtps/config_esp32.h @@ -63,7 +63,7 @@ const uint8_t MAX_TOPICNAME_LENGTH = 64; const int HEARTBEAT_STACKSIZE = 3072; // byte const int THREAD_POOL_WRITER_STACKSIZE = 4096; // byte const int THREAD_POOL_READER_STACKSIZE = 4096; // byte -const uint16_t SPDP_WRITER_STACKSIZE = 2048; // byte +const uint16_t SPDP_WRITER_STACKSIZE = 4096; // byte const uint16_t SF_WRITER_HB_PERIOD_MS = 4000; const uint16_t SPDP_RESEND_PERIOD_MS = 250; diff --git a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h index 3701ed54e..f7e651070 100644 --- a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h +++ b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h @@ -26,10 +26,11 @@ Author: i11 - Embedded Software, RWTH Aachen University #define RTPS_SEDPAGENT_H #include "rtps/discovery/BuiltInEndpoints.h" +#include "rtps/config.h" #include "rtps/discovery/TopicData.h" -#include "rtps/platform/sync.h" #include +#include namespace rtps { @@ -64,7 +65,7 @@ class SEDPAgent { private: Participant *m_part; - platform::sync::RecursiveMutexHandle m_mutex = nullptr; + std::recursive_mutex m_mutex; uint8_t m_buffer[600]; // TODO check size, currently changed from 300 to 600 // (FastDDS gives too many options) BuiltInEndpoints m_endpoints; diff --git a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h index 24d5a588f..fc0ac56ba 100644 --- a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h +++ b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h @@ -29,11 +29,11 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/config.h" #include "rtps/discovery/BuiltInEndpoints.h" #include "rtps/discovery/ParticipantProxyData.h" -#include "rtps/platform/sync.h" #include "rtps/utils/Log.h" #include "task.hpp" #include "ucdr/microcdr.h" +#include #include #if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE @@ -59,7 +59,7 @@ class SPDPAgent { void init(Participant &participant, BuiltInEndpoints &endpoints); void start(); void stop(); - platform::sync::RecursiveMutexHandle m_mutex = nullptr; + std::recursive_mutex m_mutex; private: Participant *mp_participant = nullptr; diff --git a/components/rtps_embedded/include/rtps/entities/Domain.h b/components/rtps_embedded/include/rtps/entities/Domain.h index 6723a06b7..9ad885741 100644 --- a/components/rtps_embedded/include/rtps/entities/Domain.h +++ b/components/rtps_embedded/include/rtps/entities/Domain.h @@ -33,8 +33,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/StatefulWriter.h" #include "rtps/entities/StatelessReader.h" #include "rtps/entities/StatelessWriter.h" -#include "rtps/platform/sync.h" #include "rtps/platform/transport.h" +#include #include namespace rtps { @@ -92,7 +92,7 @@ class Domain { } bool m_initComplete = false; - platform::sync::RecursiveMutexHandle m_mutex = nullptr; + std::recursive_mutex m_mutex; void receiveCallback(const PacketInfo &packet); GuidPrefix_t generateGuidPrefix(ParticipantId_t id) const; diff --git a/components/rtps_embedded/include/rtps/entities/Participant.h b/components/rtps_embedded/include/rtps/entities/Participant.h index 063bb236a..7a8a0994d 100644 --- a/components/rtps_embedded/include/rtps/entities/Participant.h +++ b/components/rtps_embedded/include/rtps/entities/Participant.h @@ -30,10 +30,10 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/discovery/SEDPAgent.h" #include "rtps/discovery/SPDPAgent.h" #include "rtps/messages/MessageReceiver.h" -#include "rtps/platform/sync.h" #include "rtps/platform/transport.h" #include +#include namespace rtps { @@ -122,7 +122,7 @@ class Participant { std::array m_readers = { nullptr}; - platform::sync::RecursiveMutexHandle m_mutex = nullptr; + std::recursive_mutex m_mutex; MemoryPool m_remoteParticipants; diff --git a/components/rtps_embedded/include/rtps/entities/Reader.h b/components/rtps_embedded/include/rtps/entities/Reader.h index 667920540..506e8e569 100644 --- a/components/rtps_embedded/include/rtps/entities/Reader.h +++ b/components/rtps_embedded/include/rtps/entities/Reader.h @@ -29,9 +29,9 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/config.h" #include "rtps/discovery/TopicData.h" #include "rtps/entities/WriterProxy.h" -#include "rtps/platform/sync.h" #include "rtps/storages/MemoryPool.h" #include +#include namespace rtps { @@ -136,10 +136,10 @@ class Reader { std::array m_callbacks; // Guards manipulation of the proxies array - platform::sync::RecursiveMutexHandle m_proxies_mutex = nullptr; + std::recursive_mutex m_proxies_mutex; // Guards manipulation of callback array - platform::sync::RecursiveMutexHandle m_callback_mutex = nullptr; + std::recursive_mutex m_callback_mutex; }; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp index 249be6785..a05f151d6 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -26,8 +26,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/messages/MessageFactory.h" #include "rtps/storages/PayloadBuffer.h" #include "rtps/utils/Diagnostics.h" -#include "rtps/utils/Lock.h" #include "rtps/utils/Log.h" +#include #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" @@ -67,7 +67,7 @@ void StatefulReaderT::newChange( if (m_callback_count == 0 || !m_is_initialized_) { return; } - Lock lock{m_proxies_mutex}; + std::lock_guard lock(m_proxies_mutex); for (auto &proxy : m_proxies) { if (proxy.remoteWriterGuid == cacheChange.writerGuid) { if (proxy.expectedSN == cacheChange.sn) { @@ -111,7 +111,7 @@ bool StatefulReaderT::addNewMatchedWriter( template bool StatefulReaderT::onNewGapMessage( const SubmessageGap &msg, const GuidPrefix_t &remotePrefix) { - Lock lock{m_proxies_mutex}; + std::lock_guard lock(m_proxies_mutex); if (!m_is_initialized_) { return false; } @@ -220,7 +220,7 @@ bool StatefulReaderT::onNewGapMessage( template bool StatefulReaderT::onNewHeartbeat( const SubmessageHeartbeat &msg, const GuidPrefix_t &sourceGuidPrefix) { - Lock lock{m_proxies_mutex}; + std::lock_guard lock(m_proxies_mutex); if (!m_is_initialized_) { return false; } @@ -273,7 +273,7 @@ bool StatefulReaderT::onNewHeartbeat( template bool StatefulReaderT::sendPreemptiveAckNack( const WriterProxy &writer) { - Lock lock{m_proxies_mutex}; + std::lock_guard lock(m_proxies_mutex); if (!m_is_initialized_) { return false; } diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp index 337545552..b77b3c587 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -29,6 +29,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/utils/Log.h" #include #include +#include #include #include @@ -65,15 +66,6 @@ bool StatefulWriterT::init(TopicData attributes, NetworkDriver &driver, bool enfUnicast) { - if (m_mutex == nullptr) { - if (!createMutex(&m_mutex)) { - - SFW_LOG("Failed to create mutex.\n"); - - return false; - } - } - m_attributes = attributes; mp_threadPool = threadPool; @@ -143,7 +135,7 @@ const rtps::CacheChange *StatefulWriterT::newChange( return nullptr; } - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); if (!m_is_initialized_) { return nullptr; } @@ -173,7 +165,7 @@ const rtps::CacheChange *StatefulWriterT::newChange( template void StatefulWriterT::progress() { INIT_GUARD() - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); CacheChange *next = m_history.getChangeBySN(m_nextSequenceNumberToSend); if (next != nullptr) { uint32_t i = 0; @@ -224,7 +216,7 @@ template void StatefulWriterT::progress() { template void StatefulWriterT::setAllChangesToUnsent() { INIT_GUARD() - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); m_nextSequenceNumberToSend = m_history.getCurrentSeqNumMin(); @@ -237,7 +229,7 @@ template void StatefulWriterT::onNewAckNack( const SubmessageAckNack &msg, const GuidPrefix_t &sourceGuidPrefix) { INIT_GUARD() - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); if (!m_is_initialized_) { return; } @@ -342,7 +334,7 @@ void StatefulWriterT::onNewAckNack( template bool rtps::StatefulWriterT::removeFromHistory( const SequenceNumber_t &s) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); return m_history.dropChange(s); } @@ -534,7 +526,7 @@ void StatefulWriterT::sendHeartBeat() { MessageFactory::addHeader(payload, m_attributes.endpointGuid.prefix); { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); if (!m_history.isEmpty()) { firstSN = m_history.getCurrentSeqNumMin(); diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp index f6e66dd03..c670ffcb0 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp @@ -30,6 +30,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/storages/PayloadBuffer.h" #include "rtps/utils/Log.h" #include "rtps/utils/udpUtils.h" +#include using rtps::CacheChange; using rtps::SequenceNumber_t; @@ -63,15 +64,6 @@ bool StatelessWriterT::init(TopicData attributes, m_attributes = attributes; - if (m_mutex == nullptr) { - if (!createMutex(&m_mutex)) { -#if SLW_VERBOSE - SLW_LOG("Failed to create mutex \n"); -#endif - return false; - } - } - mp_threadPool = threadPool; m_srcPort = attributes.unicastLocator.port; m_enforceUnicast = enfUnicast; @@ -101,7 +93,7 @@ const CacheChange *StatelessWriterT::newChange( if (isIrrelevant(kind)) { return nullptr; } - Lock lock(m_mutex); + std::lock_guard lock(m_mutex); if (!m_is_initialized_) { return nullptr; } @@ -134,7 +126,7 @@ bool StatelessWriterT::removeFromHistory( template void StatelessWriterT::setAllChangesToUnsent() { INIT_GUARD(); - Lock lock(m_mutex); + std::lock_guard lock(m_mutex); m_nextSequenceNumberToSend = m_history.getSeqNumMin(); @@ -173,7 +165,7 @@ void StatelessWriterT::progress() { MessageFactory::addSubMessageTimeStamp(payload); { - Lock lock(m_mutex); + std::lock_guard lock(m_mutex); const CacheChange *next = m_history.getChangeBySN(m_nextSequenceNumberToSend); if (next == nullptr) { diff --git a/components/rtps_embedded/include/rtps/entities/Writer.h b/components/rtps_embedded/include/rtps/entities/Writer.h index da371fbcb..76c895ca8 100644 --- a/components/rtps_embedded/include/rtps/entities/Writer.h +++ b/components/rtps_embedded/include/rtps/entities/Writer.h @@ -28,11 +28,11 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/ThreadPool.h" #include "rtps/discovery/TopicData.h" #include "rtps/entities/ReaderProxy.h" -#include "rtps/platform/sync.h" #include "rtps/storages/CacheChange.h" #include "rtps/storages/MemoryPool.h" #include +#include #ifdef DEBUG_BUILD #define COMPILE_INIT_GUARD @@ -86,7 +86,7 @@ class Writer { protected: SequenceNumber_t m_sedp_sequence_number; - platform::sync::RecursiveMutexHandle m_mutex = nullptr; + std::recursive_mutex m_mutex; ThreadPool *mp_threadPool = nullptr; Ip4Port_t m_srcPort; diff --git a/components/rtps_embedded/include/rtps/platform/sync.h b/components/rtps_embedded/include/rtps/platform/sync.h deleted file mode 100644 index e3f41a812..000000000 --- a/components/rtps_embedded/include/rtps/platform/sync.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef RTPS_PLATFORM_SYNC_H -#define RTPS_PLATFORM_SYNC_H - -#include - -namespace rtps { -namespace platform { -namespace sync { - -using RecursiveMutexHandle = std::recursive_mutex *; - -bool createRecursiveMutex(RecursiveMutexHandle *mutex); -bool lockRecursive(RecursiveMutexHandle mutex); -bool unlockRecursive(RecursiveMutexHandle mutex); - -} // namespace sync -} // namespace platform -} // namespace rtps - -#endif // RTPS_PLATFORM_SYNC_H diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h index 26af5357d..47f1e2fd2 100644 --- a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h @@ -25,10 +25,10 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_THREADSAFEQUEUE_H #define RTPS_THREADSAFEQUEUE_H -#include "rtps/platform/sync.h" - #include +#include #include +#include namespace rtps { @@ -62,7 +62,8 @@ template class ThreadSafeCircularBuffer { static_assert(SIZE + 1 < std::numeric_limits::max(), "Iterator is large enough for given size"); - platform::sync::RecursiveMutexHandle m_mutex = nullptr; + std::mutex m_mutex; + std::mutex m_init_mutex; bool m_initialized = false; inline bool isFull(); diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp index 51fb9a3de..589963a89 100644 --- a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.tpp @@ -2,7 +2,6 @@ #ifndef RTPS_THREADSAFECIRCULARBUFFER_TPP #define RTPS_THREADSAFECIRCULARBUFFER_TPP -#include "rtps/utils/Lock.h" #include "rtps/utils/Log.h" #if TSCB_VERBOSE && RTPS_GLOBAL_VERBOSE @@ -20,23 +19,21 @@ namespace rtps { template bool ThreadSafeCircularBuffer::init() { + std::lock_guard init_lock(m_init_mutex); if (m_initialized) { return true; } - if (!platform::sync::createRecursiveMutex(&m_mutex)) { - TSCB_LOG("Failed to create mutex \n"); - return false; - } else { - TSCB_LOG("Successfully created mutex at %p\n", - static_cast(&m_mutex)); - m_initialized = true; - return true; - } + TSCB_LOG("Using std::mutex at %p\n", static_cast(&m_mutex)); + m_initialized = true; + return true; } template bool ThreadSafeCircularBuffer::moveElementIntoBuffer(T &&elem) { - Lock lock(m_mutex); + if (!init()) { + return false; + } + std::lock_guard lock(m_mutex); if (!isFull()) { m_buffer[m_head] = std::move(elem); incrementHead(); @@ -49,7 +46,10 @@ bool ThreadSafeCircularBuffer::moveElementIntoBuffer(T &&elem) { template bool ThreadSafeCircularBuffer::copyElementIntoBuffer(const T &elem) { - Lock lock(m_mutex); + if (!init()) { + return false; + } + std::lock_guard lock(m_mutex); if (!isFull()) { m_buffer[m_head] = elem; incrementHead(); @@ -62,7 +62,10 @@ bool ThreadSafeCircularBuffer::copyElementIntoBuffer(const T &elem) { template bool ThreadSafeCircularBuffer::moveFirstInto(T &hull) { - Lock lock(m_mutex); + if (!init()) { + return false; + } + std::lock_guard lock(m_mutex); if (m_head != m_tail) { hull = std::move(m_buffer[m_tail]); incrementTail(); @@ -74,7 +77,10 @@ bool ThreadSafeCircularBuffer::moveFirstInto(T &hull) { template bool ThreadSafeCircularBuffer::peakFirst(T &hull) { - Lock lock(m_mutex); + if (!init()) { + return false; + } + std::lock_guard lock(m_mutex); if (m_head != m_tail) { hull = m_buffer[m_tail]; return true; @@ -85,12 +91,28 @@ bool ThreadSafeCircularBuffer::peakFirst(T &hull) { template uint32_t ThreadSafeCircularBuffer::numElements() { + if (!init()) { + return 0; + } + std::lock_guard lock(m_mutex); return m_num_elements; } +template +uint32_t ThreadSafeCircularBuffer::insertionFailures() { + if (!init()) { + return 0; + } + std::lock_guard lock(m_mutex); + return m_insertion_failures; +} + template void ThreadSafeCircularBuffer::clear() { - Lock lock(m_mutex); + if (!init()) { + return; + } + std::lock_guard lock(m_mutex); m_head = m_tail; m_num_elements = 0; } diff --git a/components/rtps_embedded/include/rtps/utils/Lock.h b/components/rtps_embedded/include/rtps/utils/Lock.h deleted file mode 100644 index 4ab6bb70c..000000000 --- a/components/rtps_embedded/include/rtps/utils/Lock.h +++ /dev/null @@ -1,54 +0,0 @@ -/* -The MIT License -Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University -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 - -This file is part of embeddedRTPS. - -Author: i11 - Embedded Software, RWTH Aachen University -*/ - -#ifndef RTPS_LOCK_H -#define RTPS_LOCK_H - -#include "rtps/platform/sync.h" - -namespace rtps { - -using RecursiveMutexHandle = platform::sync::RecursiveMutexHandle; - -class Lock { -public: - explicit Lock(RecursiveMutexHandle &mutex) : m_mutex(mutex) { - m_locked = platform::sync::lockRecursive(m_mutex); - }; - - ~Lock() { - if (m_locked) { - platform::sync::unlockRecursive(m_mutex); - } - }; - -private: - RecursiveMutexHandle m_mutex = nullptr; - bool m_locked = false; -}; - -bool createMutex(RecursiveMutexHandle *mutex); - -} // namespace rtps -#endif // RTPS_LOCK_H diff --git a/components/rtps_embedded/include/rtps/utils/Log.h b/components/rtps_embedded/include/rtps/utils/Log.h index 1026da704..ae52abcb1 100644 --- a/components/rtps_embedded/include/rtps/utils/Log.h +++ b/components/rtps_embedded/include/rtps/utils/Log.h @@ -36,12 +36,12 @@ Author: i11 - Embedded Software, RWTH Aachen University #define SEDP_VERBOSE 1 #define RECV_VERBOSE 0 #define PARTICIPANT_VERBOSE 0 -#define DOMAIN_VERBOSE 0 +#define DOMAIN_VERBOSE 1 #define UDP_DRIVER_VERBOSE 1 -#define TSCB_VERBOSE 0 +#define TSCB_VERBOSE 1 #define SLW_VERBOSE 0 #define SFR_VERBOSE 1 #define SLR_VERBOSE 1 -#define THREAD_POOL_VERBOSE 0 +#define THREAD_POOL_VERBOSE 1 #endif // RTPS_LOG_H diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp index 06cc56d2c..1a557cae9 100644 --- a/components/rtps_embedded/src/ThreadPool.cpp +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -238,9 +238,9 @@ void ThreadPool::doWriterWork() { continue; } - THREAD_POOL_LOG("WriterWorker | User = %u, Meta = %u\r\n", - static_cast(Diagnostics::ThreadPool::processed_outgoing_usertraffic), - static_cast(Diagnostics::ThreadPool::processed_outgoing_metatraffic)); + // THREAD_POOL_LOG("WriterWorker | User = %u, Meta = %u\r\n", + // static_cast(Diagnostics::ThreadPool::processed_outgoing_usertraffic), + // static_cast(Diagnostics::ThreadPool::processed_outgoing_metatraffic)); updateDiagnostics(); return; } @@ -301,8 +301,8 @@ void ThreadPool::doReaderWork() { continue; } THREAD_POOL_LOG("ReaderWorker | User = %u, Meta = %u\r\n", - static_cast(usertraffic), - static_cast(metatraffic)); + static_cast(Diagnostics::ThreadPool::processed_incoming_usertraffic), + static_cast(Diagnostics::ThreadPool::processed_incoming_metatraffic)); updateDiagnostics(); return; } diff --git a/components/rtps_embedded/src/discovery/SEDPAgent.cpp b/components/rtps_embedded/src/discovery/SEDPAgent.cpp index 000da1012..a5c0a01ec 100644 --- a/components/rtps_embedded/src/discovery/SEDPAgent.cpp +++ b/components/rtps_embedded/src/discovery/SEDPAgent.cpp @@ -30,6 +30,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/messages/MessageTypes.h" #include "rtps/utils/Log.h" #include "ucdr/microcdr.h" +#include using rtps::SEDPAgent; @@ -45,12 +46,6 @@ using rtps::SEDPAgent; #endif void SEDPAgent::init(Participant &part, const BuiltInEndpoints &endpoints) { - // TODO move - if (!createMutex(&m_mutex)) { - SEDP_LOG("SEDPAgent failed to create mutex\n"); - return; - } - m_part = ∂ m_endpoints = endpoints; if (m_endpoints.sedpPubReader != nullptr) { @@ -87,7 +82,7 @@ void SEDPAgent::jumppadSubscriptionReader( } void SEDPAgent::handlePublisherReaderMessage(const ReaderCacheChange &change) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); #if SEDP_VERBOSE SEDP_LOG("New publisher\n"); #endif @@ -156,7 +151,7 @@ void SEDPAgent::removeUnmatchedEntity(const Guid_t &guid) { void SEDPAgent::removeUnmatchedEntitiesOfParticipant( const GuidPrefix_t &guidPrefix) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); auto isElementToRemove = [&](const TopicDataCompressed &topicData) { return topicData.endpointGuid.prefix == guidPrefix; }; @@ -222,7 +217,7 @@ void SEDPAgent::handlePublisherReaderMessage(const TopicData &writerData, void SEDPAgent::handleSubscriptionReaderMessage( const ReaderCacheChange &change) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); #if SEDP_VERBOSE SEDP_LOG("New subscriber\n"); #endif @@ -348,7 +343,7 @@ bool SEDPAgent::addWriter(Writer &writer) { return true; // No need to announce builtin endpoints } - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); // Check unmatched writers for this new reader tryMatchUnmatchedEndpoints(); @@ -442,7 +437,7 @@ void SEDPAgent::jumppadTakeProxyOfDisposedWriter(const Writer *writer, } bool SEDPAgent::deleteReader(Reader *reader) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); // Set cache change kind in SEDP endpoint to DISPOSED if (!disposeEndpointInSEDPHistory(reader, m_endpoints.sedpSubWriter)) { return false; @@ -461,7 +456,7 @@ bool SEDPAgent::deleteReader(Reader *reader) { } bool SEDPAgent::deleteWriter(Writer *writer) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); // Set cache change kind in SEDP endpoint to DISPOSED if (!disposeEndpointInSEDPHistory(writer, m_endpoints.sedpPubWriter)) { return false; @@ -491,7 +486,7 @@ bool SEDPAgent::addReader(Reader &reader) { return true; // No need to announce builtin endpoints } - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); // Check unmatched writers for this new reader tryMatchUnmatchedEndpoints(); diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp index 21cb24f7e..39f26dca9 100644 --- a/components/rtps_embedded/src/discovery/SPDPAgent.cpp +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -32,6 +32,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/utils/udpUtils.h" #include +#include #include using rtps::SPDPAgent; @@ -39,10 +40,6 @@ using rtps::SMElement::BuildInEndpointSet; using rtps::SMElement::ParameterId; void SPDPAgent::init(Participant &participant, BuiltInEndpoints &endpoints) { - if (!createMutex(&m_mutex)) { - SPDP_LOG("Could not alloc mutex"); - return; - } mp_participant = &participant; m_buildInEndpoints = endpoints; m_buildInEndpoints.spdpReader->registerCallback(receiveCallback, this); @@ -113,7 +110,7 @@ void SPDPAgent::handleSPDPPackage(const ReaderCacheChange &cacheChange) { return; } - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); if (cacheChange.size > m_inputBuffer.size()) { SPDP_LOG("Input buffer to small\n"); return; diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps_embedded/src/entities/Domain.cpp index 9e85592bb..8aacb4100 100644 --- a/components/rtps_embedded/src/entities/Domain.cpp +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -26,6 +26,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/utils/Log.h" #include "rtps/utils/udpUtils.h" #include +#include #if defined(ESP_PLATFORM) #include "esp_mac.h" @@ -50,7 +51,6 @@ Domain::Domain(const platform::transport::Ip4AddressBytes &localIpAddress) m_transport(&m_defaultTransport), m_localIpAddress(localIpAddress) { initializeTransport(); - createMutex(&m_mutex); } Domain::Domain(platform::transport::ITransport &transport, @@ -60,7 +60,6 @@ Domain::Domain(platform::transport::ITransport &transport, m_transport(&transport), m_localIpAddress(localIpAddress) { initializeTransport(); - createMutex(&m_mutex); } void Domain::initializeTransport() { @@ -260,7 +259,7 @@ void Domain::registerMulticastPort(FullLengthLocator mcastLocator) { rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, const char *typeName, bool reliable) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); if (reliable) { for (unsigned int i = 0; i < m_statefulReaders.size(); i++) { if (m_statefulReaders[i].isInitialized()) { @@ -305,7 +304,7 @@ rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, const char *typeName, bool reliable) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); if (reliable) { for (unsigned int i = 0; i < m_statefulWriters.size(); i++) { if (m_statefulWriters[i].isInitialized()) { @@ -350,7 +349,7 @@ rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, bool reliable, bool enforceUnicast) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); StatelessWriter *statelessWriter = getNextUnusedEndpoint( m_statelessWriters); @@ -419,7 +418,7 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, platform::transport::Ip4AddressBytes mcastaddress) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); StatelessReader *statelessReader = getNextUnusedEndpoint( m_statelessReaders); @@ -499,7 +498,7 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, } bool rtps::Domain::deleteReader(Participant &part, Reader *reader) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); if(reader == nullptr || !reader->isInitialized()){ return false; } @@ -512,7 +511,7 @@ bool rtps::Domain::deleteReader(Participant &part, Reader *reader) { } bool rtps::Domain::deleteWriter(Participant &part, Writer *writer) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); if(writer == nullptr || !writer->isInitialized()){ return false; } diff --git a/components/rtps_embedded/src/entities/Participant.cpp b/components/rtps_embedded/src/entities/Participant.cpp index fe64c31d1..dcb44b427 100644 --- a/components/rtps_embedded/src/entities/Participant.cpp +++ b/components/rtps_embedded/src/entities/Participant.cpp @@ -26,8 +26,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/Reader.h" #include "rtps/entities/Writer.h" #include "rtps/messages/MessageReceiver.h" -#include "rtps/utils/Lock.h" #include "rtps/utils/Log.h" +#include #if PARTICIPANT_VERBOSE && RTPS_GLOBAL_VERBOSE #define PARTICIPANT_LOG(...) \ @@ -44,20 +44,11 @@ using rtps::Participant; Participant::Participant() : m_guidPrefix(GUIDPREFIX_UNKNOWN), m_participantId(PARTICIPANT_ID_INVALID), - m_receiver(this) { - if (!createMutex(&m_mutex)) { - std::terminate(); - } -} + m_receiver(this) {} Participant::Participant(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId) : m_guidPrefix(guidPrefix), m_participantId(participantId), - m_receiver(this) { - if (!createMutex(&m_mutex)) { - while (1) - ; - } -} + m_receiver(this) {} Participant::~Participant() { m_spdpAgent.stop(); } @@ -115,7 +106,7 @@ bool Participant::registerOnNewSubscriberMatchedCallback( } rtps::Writer *Participant::addWriter(Writer *pWriter) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_writers.size(); i++) { if (m_writers[i] == nullptr) { m_writers[i] = pWriter; @@ -129,7 +120,7 @@ rtps::Writer *Participant::addWriter(Writer *pWriter) { } bool Participant::isWritersFull() { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_writers.size(); i++) { if (m_writers[i] == nullptr) { return false; @@ -140,7 +131,7 @@ bool Participant::isWritersFull() { } rtps::Reader *Participant::addReader(Reader *pReader) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_readers.size(); i++) { if (m_readers[i] == nullptr) { m_readers[i] = pReader; @@ -155,7 +146,7 @@ rtps::Reader *Participant::addReader(Reader *pReader) { } bool Participant::deleteReader(Reader *reader) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_readers.size(); i++) { if (m_readers[i]->getSEDPSequenceNumber() == reader->getSEDPSequenceNumber()) { @@ -170,7 +161,7 @@ bool Participant::deleteReader(Reader *reader) { } bool Participant::deleteWriter(Writer *writer) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_writers.size(); i++) { if (m_writers[i]->getSEDPSequenceNumber() == writer->getSEDPSequenceNumber()) { @@ -185,7 +176,7 @@ bool Participant::deleteWriter(Writer *writer) { } bool Participant::isReadersFull() { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_readers.size(); i++) { if (m_readers[i] == nullptr) { return false; @@ -196,7 +187,7 @@ bool Participant::isReadersFull() { } rtps::Writer *Participant::getWriter(EntityId_t id) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (uint8_t i = 0; i < m_writers.size(); ++i) { if (m_writers[i] == nullptr) { continue; @@ -209,7 +200,7 @@ rtps::Writer *Participant::getWriter(EntityId_t id) { } rtps::Reader *Participant::getReader(EntityId_t id) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (uint8_t i = 0; i < m_readers.size(); ++i) { if (m_readers[i] == nullptr) { continue; @@ -222,7 +213,7 @@ rtps::Reader *Participant::getReader(EntityId_t id) { } rtps::Reader *Participant::getReaderByWriterId(const Guid_t &guid) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (uint8_t i = 0; i < m_readers.size(); ++i) { if (m_readers[i] == nullptr) { continue; @@ -235,7 +226,7 @@ rtps::Reader *Participant::getReaderByWriterId(const Guid_t &guid) { } rtps::Writer *Participant::getMatchingWriter(const TopicData &readerTopicData) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (uint8_t i = 0; i < m_writers.size(); ++i) { if (m_writers[i] == nullptr) { continue; @@ -251,7 +242,7 @@ rtps::Writer *Participant::getMatchingWriter(const TopicData &readerTopicData) { } rtps::Reader *Participant::getMatchingReader(const TopicData &writerTopicData) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (uint8_t i = 0; i < m_readers.size(); ++i) { if (m_readers[i] == nullptr) { continue; @@ -268,7 +259,7 @@ rtps::Reader *Participant::getMatchingReader(const TopicData &writerTopicData) { rtps::Writer * Participant::getMatchingWriter(const TopicDataCompressed &readerTopicData) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (uint8_t i = 0; i < m_writers.size(); ++i) { if (m_writers[i] == nullptr) { continue; @@ -285,7 +276,7 @@ Participant::getMatchingWriter(const TopicDataCompressed &readerTopicData) { rtps::Reader * Participant::getMatchingReader(const TopicDataCompressed &writerTopicData) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (uint8_t i = 0; i < m_readers.size(); ++i) { if (m_readers[i] == nullptr) { continue; @@ -302,12 +293,12 @@ Participant::getMatchingReader(const TopicDataCompressed &writerTopicData) { bool Participant::addNewRemoteParticipant( const ParticipantProxyData &remotePart) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); return m_remoteParticipants.add(remotePart); } bool Participant::removeRemoteParticipant(const GuidPrefix_t &prefix) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); auto isElementToRemove = [&](const ParticipantProxyData &proxy) { return proxy.m_guid.prefix == prefix; }; @@ -320,7 +311,7 @@ bool Participant::removeRemoteParticipant(const GuidPrefix_t &prefix) { } void Participant::removeAllProxiesOfParticipant(const GuidPrefix_t &prefix) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_readers.size(); i++) { if (m_readers[i] == nullptr) { continue; @@ -337,7 +328,7 @@ void Participant::removeAllProxiesOfParticipant(const GuidPrefix_t &prefix) { } void Participant::removeProxyFromAllEndpoints(const Guid_t &guid) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_writers.size(); i++) { if (m_writers[i] == nullptr) { continue; @@ -365,7 +356,7 @@ void Participant::removeProxyFromAllEndpoints(const Guid_t &guid) { const rtps::ParticipantProxyData * Participant::findRemoteParticipant(const GuidPrefix_t &prefix) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); auto isElementToFind = [&](const ParticipantProxyData &proxy) { return proxy.m_guid.prefix == prefix; }; @@ -377,7 +368,7 @@ Participant::findRemoteParticipant(const GuidPrefix_t &prefix) { void Participant::refreshRemoteParticipantLiveliness( const GuidPrefix_t &prefix) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); auto isElementToFind = [&](const ParticipantProxyData &proxy) { return proxy.m_guid.prefix == prefix; }; @@ -393,7 +384,7 @@ void Participant::refreshRemoteParticipantLiveliness( bool Participant::hasReaderWithMulticastLocator( const std::array &address) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (uint8_t i = 0; i < m_readers.size(); i++) { if (m_readers[i] == nullptr) { continue; @@ -407,15 +398,15 @@ bool Participant::hasReaderWithMulticastLocator( } uint32_t Participant::getRemoteParticipantCount() { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); return m_remoteParticipants.getNumElements(); } rtps::MessageReceiver *Participant::getMessageReceiver() { return &m_receiver; } bool Participant::checkAndResetHeartbeats() { - Lock lock1{m_mutex}; - Lock lock2{m_spdpAgent.m_mutex}; + std::lock_guard lock1(m_mutex); + std::lock_guard lock2(m_spdpAgent.m_mutex); PARTICIPANT_LOG("Have %u remote participants", (unsigned int)m_remoteParticipants.getNumElements()); PARTICIPANT_LOG( @@ -533,7 +524,7 @@ void Participant::printInfo() { rtps::SPDPAgent &Participant::getSPDPAgent() { return m_spdpAgent; } void Participant::addBuiltInEndpoints(BuiltInEndpoints &endpoints) { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); m_hasBuilInEndpoints = true; m_spdpAgent.init(*this, endpoints); m_sedpAgent.init(*this, endpoints); diff --git a/components/rtps_embedded/src/entities/Reader.cpp b/components/rtps_embedded/src/entities/Reader.cpp index 9d22a8e55..41da9909d 100644 --- a/components/rtps_embedded/src/entities/Reader.cpp +++ b/components/rtps_embedded/src/entities/Reader.cpp @@ -1,16 +1,16 @@ #include #include #include -#include #include #include +#include using namespace rtps; Reader::Reader() { m_callbacks.fill({nullptr, nullptr, 0}); } void Reader::executeCallbacks(const ReaderCacheChange &cacheChange) { - Lock lock{m_callback_mutex}; + std::lock_guard lock(m_callback_mutex); for (unsigned int i = 0; i < m_callbacks.size(); i++) { if (m_callbacks[i].function != nullptr) { m_callbacks[i].function(m_callbacks[i].arg, cacheChange); @@ -19,26 +19,12 @@ void Reader::executeCallbacks(const ReaderCacheChange &cacheChange) { } bool Reader::initMutex() { - if (m_proxies_mutex == nullptr) { - if (!createMutex(&m_proxies_mutex)) { - SFR_LOG("StatefulReader: Failed to create mutex.\n"); - return false; - } - } - - if (m_callback_mutex == nullptr) { - if (!createMutex(&m_callback_mutex)) { - SFR_LOG("StatefulReader: Failed to create mutex.\n"); - return false; - } - } - return true; } void Reader::reset() { - Lock lock1{m_proxies_mutex}; - Lock lock2{m_callback_mutex}; + std::lock_guard lock1(m_proxies_mutex); + std::lock_guard lock2(m_callback_mutex); m_proxies.clear(); for (unsigned int i = 0; i < m_callbacks.size(); i++) { @@ -71,7 +57,7 @@ WriterProxy *Reader::getProxy(Guid_t guid) { Reader::callbackIdentifier_t Reader::registerCallback(Reader::callbackFunction_t cb, void *arg) { - Lock lock{m_callback_mutex}; + std::lock_guard lock(m_callback_mutex); if (m_callback_count == m_callbacks.size() || cb == nullptr) { return false; } @@ -92,7 +78,7 @@ Reader::registerCallback(Reader::callbackFunction_t cb, void *arg) { uint32_t Reader::getProxiesCount() { return m_proxies.getNumElements(); } bool Reader::removeCallback(Reader::callbackIdentifier_t identifier) { - Lock lock{m_callback_mutex}; + std::lock_guard lock(m_callback_mutex); for (unsigned int i = 0; i < m_callbacks.size(); i++) { if (m_callbacks[i].identifier == identifier) { m_callbacks[i].function = nullptr; @@ -108,7 +94,7 @@ bool Reader::removeCallback(Reader::callbackIdentifier_t identifier) { uint8_t Reader::getNumCallbacks() { return m_callback_count; } void Reader::removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix) { - Lock lock{m_proxies_mutex}; + std::lock_guard lock(m_proxies_mutex); auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid.prefix == guidPrefix; }; @@ -120,7 +106,7 @@ void Reader::removeAllProxiesOfParticipant(const GuidPrefix_t &guidPrefix) { } bool Reader::removeProxy(const Guid_t &guid) { - Lock lock{m_proxies_mutex}; + std::lock_guard lock(m_proxies_mutex); auto isElementToRemove = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid == guid; }; @@ -132,7 +118,7 @@ bool Reader::removeProxy(const Guid_t &guid) { } bool Reader::addNewMatchedWriter(const WriterProxy &newProxy) { - Lock lock{m_proxies_mutex}; + std::lock_guard lock(m_proxies_mutex); #if (SFR_VERBOSE || SLR_VERBOSE) && RTPS_GLOBAL_VERBOSE SFR_LOG("New writer added with id: "); printGuid(newProxy.remoteWriterGuid); @@ -152,7 +138,7 @@ int rtps::Reader::dumpAllProxies(dumpProxyCallback target, void *arg) { if (target == nullptr) { return 0; } - Lock lock{m_proxies_mutex}; + std::lock_guard lock(m_proxies_mutex); int dump_count = 0; for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { target(this, *it, arg); diff --git a/components/rtps_embedded/src/entities/StatelessReader.cpp b/components/rtps_embedded/src/entities/StatelessReader.cpp index 1fde550d6..ec4825d42 100644 --- a/components/rtps_embedded/src/entities/StatelessReader.cpp +++ b/components/rtps_embedded/src/entities/StatelessReader.cpp @@ -23,7 +23,6 @@ Author: i11 - Embedded Software, RWTH Aachen University */ #include "rtps/entities/StatelessReader.h" -#include "rtps/utils/Lock.h" #include "rtps/utils/Log.h" using rtps::StatelessReader; diff --git a/components/rtps_embedded/src/entities/Writer.cpp b/components/rtps_embedded/src/entities/Writer.cpp index aff59e0ba..e52d07a81 100644 --- a/components/rtps_embedded/src/entities/Writer.cpp +++ b/components/rtps_embedded/src/entities/Writer.cpp @@ -4,6 +4,7 @@ #include #include #include +#include using namespace rtps; @@ -13,7 +14,7 @@ bool rtps::Writer::addNewMatchedReader(const ReaderProxy &newProxy) { SFW_LOG("New reader added with id: "); printGuid(newProxy.remoteReaderGuid); #endif - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); bool success = m_proxies.add(newProxy); if (!m_enforceUnicast) { manageSendOptions(); @@ -23,7 +24,7 @@ bool rtps::Writer::addNewMatchedReader(const ReaderProxy &newProxy) { bool rtps::Writer::removeProxy(const Guid_t &guid) { INIT_GUARD() - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); auto isElementToRemove = [&](const ReaderProxy &proxy) { return proxy.remoteReaderGuid == guid; }; @@ -37,7 +38,7 @@ bool rtps::Writer::removeProxy(const Guid_t &guid) { } uint32_t rtps::Writer::getProxiesCount() { - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); return m_proxies.getNumElements(); } @@ -59,7 +60,7 @@ const rtps::CacheChange *rtps::Writer::newChange(ChangeKind_t kind, void rtps::Writer::manageSendOptions() { INIT_GUARD(); - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); for (auto &proxy : m_proxies) { if (proxy.remoteMulticastLocator.kind == LocatorKind_t::LOCATOR_KIND_INVALID) { @@ -98,7 +99,7 @@ void rtps::Writer::manageSendOptions() { void rtps::Writer::removeAllProxiesOfParticipant( const GuidPrefix_t &guidPrefix) { INIT_GUARD(); - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); auto isElementToRemove = [&](const ReaderProxy &proxy) { return proxy.remoteReaderGuid.prefix == guidPrefix; }; @@ -138,7 +139,7 @@ int rtps::Writer::dumpAllProxies(dumpProxyCallback target, void *arg) { if (target == nullptr) { return 0; } - Lock lock{m_mutex}; + std::lock_guard lock(m_mutex); int dump_count = 0; for (auto it = m_proxies.begin(); it != m_proxies.end(); ++it, ++dump_count) { target(this, *it, arg); diff --git a/components/rtps_embedded/src/platform/sync.cpp b/components/rtps_embedded/src/platform/sync.cpp deleted file mode 100644 index 768fe6e3a..000000000 --- a/components/rtps_embedded/src/platform/sync.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "rtps/platform/sync.h" - -#include - -namespace rtps { -namespace platform { -namespace sync { - -bool createRecursiveMutex(RecursiveMutexHandle *mutex) { - if (mutex == nullptr) { - return false; - } - - *mutex = new (std::nothrow) std::recursive_mutex(); - return *mutex != nullptr; -} - -bool lockRecursive(RecursiveMutexHandle mutex) { - if (mutex == nullptr) { - return false; - } - - mutex->lock(); - return true; -} - -bool unlockRecursive(RecursiveMutexHandle mutex) { - if (mutex == nullptr) { - return false; - } - - mutex->unlock(); - return true; -} - -} // namespace sync -} // namespace platform -} // namespace rtps diff --git a/components/rtps_embedded/src/utils/Lock.cpp b/components/rtps_embedded/src/utils/Lock.cpp deleted file mode 100644 index f5199a308..000000000 --- a/components/rtps_embedded/src/utils/Lock.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "rtps/utils/Lock.h" - -#include - -namespace rtps { - -bool createMutex(platform::sync::RecursiveMutexHandle *mutex) { - if (mutex == nullptr) { - assert(false && "Mutex pointer is null"); - return false; - } - - if (platform::sync::createRecursiveMutex(mutex)) { - return true; - } else { - assert(false && "Mutex creation failed"); - return false; - } -} - -} // namespace rtps From c3c7062200213f85b39162a1d81dcba7083651ae Mon Sep 17 00:00:00 2001 From: Guo Date: Tue, 14 Jul 2026 15:40:42 -0500 Subject: [PATCH 09/17] code clean up --- .../rtps_embedded/example/main/main.cpp | 4 +- .../rtps_embedded/include/rtps/ThreadPool.h | 4 +- .../include/rtps/common/Locator.h | 26 +++++----- .../rtps_embedded/include/rtps/common/types.h | 10 +++- .../rtps/communication/EsppTransport.h | 18 +++---- .../include/rtps/entities/Domain.h | 14 ++--- .../include/rtps/entities/Participant.h | 6 +-- .../include/rtps/entities/StatefulReader.h | 5 +- .../include/rtps/entities/StatefulWriter.h | 6 ++- .../include/rtps/entities/StatelessWriter.h | 6 ++- .../include/rtps/messages/MessageTypes.h | 3 +- .../include/rtps/platform/bootstrap.h | 51 ------------------- .../include/rtps/platform/platform_types.h | 29 ----------- .../include/rtps/platform/transport.h | 36 ------------- .../rtps_embedded/include/rtps/utils/hash.h | 2 + components/rtps_embedded/src/ThreadPool.cpp | 4 +- .../src/communication/EsppTransport.cpp | 14 ++--- .../rtps_embedded/src/entities/Domain.cpp | 9 ++-- .../src/entities/Participant.cpp | 2 +- .../src/messages/MessageTypes.cpp | 13 +++-- 20 files changed, 81 insertions(+), 181 deletions(-) delete mode 100644 components/rtps_embedded/include/rtps/platform/bootstrap.h delete mode 100644 components/rtps_embedded/include/rtps/platform/platform_types.h delete mode 100644 components/rtps_embedded/include/rtps/platform/transport.h diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp index 5850b585c..939d5dfd7 100644 --- a/components/rtps_embedded/example/main/main.cpp +++ b/components/rtps_embedded/example/main/main.cpp @@ -93,7 +93,7 @@ void publisher_task(void *arg) { extern "C" void embedded_rtps_start(const char *node_name, const char *pub_topic, const char *sub_topic, - const rtps::platform::transport::Ip4AddressBytes &local_ip) { + const rtps::Ip4AddressBytes &local_ip) { if (s_started) { return; } @@ -169,7 +169,7 @@ extern "C" void app_main(void) } logger.info("WiFi connected, local IP {}", ip_address); - rtps::platform::transport::Ip4AddressBytes local_ip{0, 0, 0, 0}; + rtps::Ip4AddressBytes local_ip{0, 0, 0, 0}; if (std::sscanf(ip_address.c_str(), "%hhu.%hhu.%hhu.%hhu", &local_ip[0], &local_ip[1], &local_ip[2], &local_ip[3]) != 4) { logger.error("Failed to parse local IP {}", ip_address); diff --git a/components/rtps_embedded/include/rtps/ThreadPool.h b/components/rtps_embedded/include/rtps/ThreadPool.h index 8ca6dcc2b..9b6ad648f 100644 --- a/components/rtps_embedded/include/rtps/ThreadPool.h +++ b/components/rtps_embedded/include/rtps/ThreadPool.h @@ -26,8 +26,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #define RTPS_THREADPOOL_H #include "rtps/communication/PacketInfo.h" +#include "rtps/common/types.h" #include "rtps/config.h" -#include "rtps/platform/transport.h" #include "rtps/storages/ThreadSafeCircularBuffer.h" #include @@ -64,7 +64,7 @@ class ThreadPool { static void onDatagram( void *arg, const uint8_t *data, std::size_t size, Ip4Port_t localPort, Ip4Port_t remotePort, - const platform::transport::Ip4AddressBytes &remoteAddress); + const Ip4AddressBytes &remoteAddress); bool addBuiltinPort(const Ip4Port_t &port); diff --git a/components/rtps_embedded/include/rtps/common/Locator.h b/components/rtps_embedded/include/rtps/common/Locator.h index f2b3b023f..701208fb2 100644 --- a/components/rtps_embedded/include/rtps/common/Locator.h +++ b/components/rtps_embedded/include/rtps/common/Locator.h @@ -26,7 +26,6 @@ Author: i11 - Embedded Software, RWTH Aachen University #define RTPS_LOCATOR_T_H #include "rtps/common/types.h" -#include "rtps/platform/platform_types.h" #include "rtps/utils/udpUtils.h" #include "ucdr/microcdr.h" @@ -35,7 +34,7 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { inline bool isSameSubnetAddress( - const platform::Ip4Address &addr, + const std::array &addr, const std::array &local) { return addr[0] == local[0] && addr[1] == local[1] && addr[2] == local[2]; } @@ -93,25 +92,25 @@ struct FullLengthLocator { } } - platform::Ip4Address getIp4Address() const { - return transformIP4ToU32(address[12], address[13], address[14], - address[15]); + std::array getIp4Address() const { + return getIp4AddressBytes(); } std::array getIp4AddressBytes() const { return {address[12], address[13], address[14], address[15]}; } - bool isSameAddress(const platform::Ip4Address &ipAddress) { - return getIp4Address() == ipAddress; + bool isSameAddress(const std::array &ipAddress) { + return getIp4AddressBytes() == ipAddress; } inline bool isSameSubnet(const std::array &localIp) const { - return isSameSubnetAddress(getIp4Address(), localIp); + return isSameSubnetAddress(getIp4AddressBytes(), localIp); } inline bool isMulticastAddress() const { - return rtps::isMulticastAddress(getIp4Address()); + const auto ip = getIp4AddressBytes(); + return ip[0] >= 224 && ip[0] <= 239; } inline uint32_t getLocatorPort() { return static_cast(port); } @@ -171,8 +170,8 @@ struct LocatorIPv4 { kind = locator.kind; } - platform::Ip4Address getIp4Address() const { - return transformIP4ToU32(address[0], address[1], address[2], address[3]); + std::array getIp4Address() const { + return getIp4AddressBytes(); } std::array getIp4AddressBytes() const { return address; } @@ -182,11 +181,12 @@ struct LocatorIPv4 { bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; } inline bool isSameSubnet(const std::array &localIp) const { - return isSameSubnetAddress(getIp4Address(), localIp); + return isSameSubnetAddress(getIp4AddressBytes(), localIp); } inline bool isMulticastAddress() const { - return rtps::isMulticastAddress(getIp4Address()); + const auto ip = getIp4AddressBytes(); + return ip[0] >= 224 && ip[0] <= 239; } }; diff --git a/components/rtps_embedded/include/rtps/common/types.h b/components/rtps_embedded/include/rtps/common/types.h index 1a1563cb4..069f56aba 100644 --- a/components/rtps_embedded/include/rtps/common/types.h +++ b/components/rtps_embedded/include/rtps/common/types.h @@ -25,9 +25,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_TYPES_H #define RTPS_TYPES_H -#include "rtps/platform/platform_types.h" - #include +#include #include #include #include @@ -252,6 +251,13 @@ struct InstanceHandle_t { // TODO struct ParticipantMessageData { // TODO }; +using Ip4AddressBytes = std::array; + +using ReceiveCallback = void (*)(void *arg, const uint8_t *data, + std::size_t size, Ip4Port_t localPort, + Ip4Port_t remotePort, + const Ip4AddressBytes &remoteAddress); + /* Default Values */ const EntityId_t ENTITYID_UNKNOWN{}; const EntityId_t ENTITYID_BUILD_IN_PARTICIPANT = { diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.h b/components/rtps_embedded/include/rtps/communication/EsppTransport.h index 03af218c7..4cdf8439b 100644 --- a/components/rtps_embedded/include/rtps/communication/EsppTransport.h +++ b/components/rtps_embedded/include/rtps/communication/EsppTransport.h @@ -25,9 +25,10 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_ESPPTRANSPORT_H #define RTPS_ESPPTRANSPORT_H +#include "rtps/common/types.h" #include "rtps/config.h" -#include "rtps/platform/transport.h" #include "udp_socket.hpp" +#include "rtps/communication/PacketInfo.h" #include #include @@ -37,17 +38,16 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { -class EsppTransport : public platform::transport::ITransport { +class EsppTransport { public: - using RxCallback = platform::transport::ReceiveCallback; + using RxCallback = ReceiveCallback; EsppTransport(RxCallback callback, void *args); - ~EsppTransport() override = default; + ~EsppTransport() = default; - bool ensureReceivePort(Ip4Port_t receivePort) override; - bool joinMultiCastGroup( - const platform::transport::Ip4AddressBytes &addr) const override; - void sendPacket(PacketInfo &info) override; + bool ensureReceivePort(Ip4Port_t receivePort); + bool joinMultiCastGroup(const Ip4AddressBytes &addr) const; + void sendPacket(PacketInfo &info); private: struct Channel { @@ -63,7 +63,7 @@ class EsppTransport : public platform::transport::ITransport { void onReceive(Ip4Port_t receivePort, std::vector &data, const espp::Socket::Info &sender) const; - static std::string ip4ToString(const platform::transport::Ip4AddressBytes &addr); + static std::string ip4ToString(const Ip4AddressBytes &addr); RxCallback m_rxCallback{nullptr}; void *m_callbackArgs{nullptr}; diff --git a/components/rtps_embedded/include/rtps/entities/Domain.h b/components/rtps_embedded/include/rtps/entities/Domain.h index 9ad885741..6f59b9a63 100644 --- a/components/rtps_embedded/include/rtps/entities/Domain.h +++ b/components/rtps_embedded/include/rtps/entities/Domain.h @@ -33,16 +33,16 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/StatefulWriter.h" #include "rtps/entities/StatelessReader.h" #include "rtps/entities/StatelessWriter.h" -#include "rtps/platform/transport.h" +#include "rtps/common/types.h" #include #include namespace rtps { class Domain { public: - explicit Domain(const platform::transport::Ip4AddressBytes &localIpAddress); - Domain(platform::transport::ITransport &transport, - const platform::transport::Ip4AddressBytes &localIpAddress); + explicit Domain(const Ip4AddressBytes &localIpAddress); + Domain(EsppTransport &transport, + const Ip4AddressBytes &localIpAddress); ~Domain(); bool completeInit(); @@ -54,7 +54,7 @@ class Domain { bool enforceUnicast = false); Reader *createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, - platform::transport::Ip4AddressBytes mcastaddress = + Ip4AddressBytes mcastaddress = {0, 0, 0, 0}); Writer *writerExists(Participant &part, const char *topicName, @@ -72,9 +72,9 @@ class Domain { ThreadPool m_threadPool; using DefaultTransport = EsppTransport; DefaultTransport m_defaultTransport; - platform::transport::ITransport *m_transport = nullptr; + EsppTransport *m_transport = nullptr; std::array m_participants; - platform::transport::Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; + Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; const uint8_t PARTICIPANT_START_ID = 0; ParticipantId_t m_nextParticipantId = PARTICIPANT_START_ID; diff --git a/components/rtps_embedded/include/rtps/entities/Participant.h b/components/rtps_embedded/include/rtps/entities/Participant.h index 7a8a0994d..b58002422 100644 --- a/components/rtps_embedded/include/rtps/entities/Participant.h +++ b/components/rtps_embedded/include/rtps/entities/Participant.h @@ -30,7 +30,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/discovery/SEDPAgent.h" #include "rtps/discovery/SPDPAgent.h" #include "rtps/messages/MessageReceiver.h" -#include "rtps/platform/transport.h" +#include "rtps/common/types.h" #include #include @@ -44,7 +44,7 @@ class Participant { public: GuidPrefix_t m_guidPrefix; ParticipantId_t m_participantId; - platform::transport::Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; + Ip4AddressBytes m_localIpAddress{{0, 0, 0, 0}}; Participant(); explicit Participant(const GuidPrefix_t &guidPrefix, @@ -62,7 +62,7 @@ class Participant { void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId); void reuse(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, - const platform::transport::Ip4AddressBytes &localIpAddress); + const Ip4AddressBytes &localIpAddress); std::array getNextUserEntityKey(); diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.h b/components/rtps_embedded/include/rtps/entities/StatefulReader.h index 511cdaf42..9d324a0cd 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.h +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.h @@ -28,11 +28,12 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/communication/PacketInfo.h" #include "rtps/config.h" #include "rtps/entities/Reader.h" -#include "rtps/platform/transport.h" +#include "rtps/common/types.h" #include "rtps/entities/WriterProxy.h" #include "rtps/storages/MemoryPool.h" namespace rtps { +class EsppTransport; struct SubmessageHeartbeat; template class StatefulReaderT final : public Reader { @@ -53,7 +54,7 @@ template class StatefulReaderT final : public Reader { NetworkDriver *m_transport; }; -using StatefulReader = StatefulReaderT; +using StatefulReader = StatefulReaderT; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h index 2ffd6fbe6..8b0337a38 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h @@ -27,7 +27,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/ReaderProxy.h" #include "rtps/entities/Writer.h" -#include "rtps/platform/transport.h" +#include "rtps/common/types.h" #include "rtps/storages/HistoryCacheWithDeletion.h" #include "rtps/storages/MemoryPool.h" #include "task.hpp" @@ -36,6 +36,8 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { +class EsppTransport; + template class StatefulWriterT final : public Writer { public: ~StatefulWriterT() override; @@ -86,7 +88,7 @@ template class StatefulWriterT final : public Writer { const SequenceNumber_t &nextValid); }; -using StatefulWriter = StatefulWriterT; +using StatefulWriter = StatefulWriterT; } // namespace rtps #include "StatefulWriter.tpp" diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h index 701385db3..8ad690c4d 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h @@ -28,12 +28,14 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/common/types.h" #include "rtps/config.h" #include "rtps/entities/Writer.h" -#include "rtps/platform/transport.h" +#include "rtps/common/types.h" #include "rtps/storages/MemoryPool.h" #include "rtps/storages/SimpleHistoryCache.h" namespace rtps { +class EsppTransport; + template class StatelessWriterT : public Writer { public: ~StatelessWriterT() override; @@ -57,7 +59,7 @@ template class StatelessWriterT : public Writer { SimpleHistoryCache m_history; }; -using StatelessWriter = StatelessWriterT; +using StatelessWriter = StatelessWriterT; } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.h b/components/rtps_embedded/include/rtps/messages/MessageTypes.h index 471a1da4d..d6952f781 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageTypes.h +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.h @@ -28,6 +28,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/common/types.h" #include +#include namespace rtps { @@ -434,7 +435,7 @@ bool deserializeMessage(const MessageProcessingInfo &info, bool deserializeMessage(const MessageProcessingInfo &info, SubmessageGap &msg); void deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, - size_t num_bitfields); + std::size_t num_bitfields); } // namespace rtps diff --git a/components/rtps_embedded/include/rtps/platform/bootstrap.h b/components/rtps_embedded/include/rtps/platform/bootstrap.h deleted file mode 100644 index 15cd7cbee..000000000 --- a/components/rtps_embedded/include/rtps/platform/bootstrap.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef RTPS_PLATFORM_BOOTSTRAP_H -#define RTPS_PLATFORM_BOOTSTRAP_H - -#include -#include - -namespace rtps { -namespace platform { -namespace bootstrap { - -struct InitSemaphoreHandle { - std::mutex mutex; - std::condition_variable cv; - bool signaled{false}; -}; - -inline bool createInitSemaphore(InitSemaphoreHandle *sem) { - if (sem == nullptr) { - return false; - } - std::lock_guard lock(sem->mutex); - sem->signaled = false; - return true; -} - -inline void signalInitSemaphore(InitSemaphoreHandle *sem) { - if (sem != nullptr) { - { - std::lock_guard lock(sem->mutex); - sem->signaled = true; - } - sem->cv.notify_one(); - } -} - -inline void waitInitSemaphore(InitSemaphoreHandle *sem) { - if (sem != nullptr) { - std::unique_lock lock(sem->mutex); - sem->cv.wait(lock, [sem]() { return sem->signaled; }); - } -} - -inline void freeInitSemaphore(InitSemaphoreHandle *sem) { - (void)sem; -} - -} // namespace bootstrap -} // namespace platform -} // namespace rtps - -#endif // RTPS_PLATFORM_BOOTSTRAP_H diff --git a/components/rtps_embedded/include/rtps/platform/platform_types.h b/components/rtps_embedded/include/rtps/platform/platform_types.h deleted file mode 100644 index a7ce239e7..000000000 --- a/components/rtps_embedded/include/rtps/platform/platform_types.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef RTPS_PLATFORM_TYPES_H -#define RTPS_PLATFORM_TYPES_H - -#include -#include -#include -#ifdef __cplusplus -#include -#endif - -namespace rtps { -namespace platform { - -using Ip4Address = std::array; -using SemaphoreHandle = void *; -using ThreadHandle = void *; -#ifdef __cplusplus -using Mutex = std::recursive_mutex; -#endif - -inline bool tryGetDefaultIp4AddressBytes(std::array &ip) { - (void)ip; - return false; -} - -} // namespace platform -} // namespace rtps - -#endif // RTPS_PLATFORM_TYPES_H diff --git a/components/rtps_embedded/include/rtps/platform/transport.h b/components/rtps_embedded/include/rtps/platform/transport.h deleted file mode 100644 index 361c70576..000000000 --- a/components/rtps_embedded/include/rtps/platform/transport.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef RTPS_PLATFORM_TRANSPORT_H -#define RTPS_PLATFORM_TRANSPORT_H - -#include -#include - -#include "rtps/common/types.h" -#include "rtps/platform/platform_types.h" - -namespace rtps { -struct PacketInfo; - -namespace platform { -namespace transport { - -using Ip4AddressBytes = std::array; - -using ReceiveCallback = void (*)(void *arg, const uint8_t *data, - std::size_t size, Ip4Port_t localPort, - Ip4Port_t remotePort, - const Ip4AddressBytes &remoteAddress); - -class ITransport { -public: - virtual ~ITransport() = default; - - virtual bool ensureReceivePort(Ip4Port_t receivePort) = 0; - virtual bool joinMultiCastGroup(const Ip4AddressBytes &addr) const = 0; - virtual void sendPacket(PacketInfo &info) = 0; -}; - -} // namespace transport -} // namespace platform -} // namespace rtps - -#endif // RTPS_PLATFORM_TRANSPORT_H diff --git a/components/rtps_embedded/include/rtps/utils/hash.h b/components/rtps_embedded/include/rtps/utils/hash.h index 3c868e527..e1f79b805 100644 --- a/components/rtps_embedded/include/rtps/utils/hash.h +++ b/components/rtps_embedded/include/rtps/utils/hash.h @@ -25,6 +25,8 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_HASH_H #define RTPS_HASH_H +#include + namespace rtps { inline size_t hashCharArray(const char *p, size_t s) { size_t result = 0; diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp index 1a557cae9..3ead47ef8 100644 --- a/components/rtps_embedded/src/ThreadPool.cpp +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -249,7 +249,7 @@ void ThreadPool::doWriterWork() { void ThreadPool::onDatagram( void *arg, const uint8_t *data, std::size_t size, Ip4Port_t localPort, Ip4Port_t remotePort, - const platform::transport::Ip4AddressBytes &remoteAddress) { + const Ip4AddressBytes &remoteAddress) { auto &pool = *static_cast(arg); PacketInfo packet; @@ -280,8 +280,6 @@ void ThreadPool::scheduleReaderWork() { } void ThreadPool::doReaderWork() { - uint32_t metatraffic = 0; - uint32_t usertraffic = 0; while (m_running) { PacketInfo packet_user; auto isUserWorkToDo = m_incomingUserTraffic.moveFirstInto(packet_user); diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps_embedded/src/communication/EsppTransport.cpp index aab8fdf0e..d969c4182 100644 --- a/components/rtps_embedded/src/communication/EsppTransport.cpp +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -37,8 +37,8 @@ using rtps::EsppTransport; namespace { bool parseIp4Address(const std::string &address, - rtps::platform::transport::Ip4AddressBytes &out) { - rtps::platform::transport::Ip4AddressBytes parsed{0, 0, 0, 0}; + rtps::Ip4AddressBytes &out) { + rtps::Ip4AddressBytes parsed{0, 0, 0, 0}; const char *cursor = address.c_str(); for (std::size_t i = 0; i < parsed.size(); ++i) { @@ -63,7 +63,7 @@ bool parseIp4Address(const std::string &address, return true; } -bool isMulticastAddress(const rtps::platform::transport::Ip4AddressBytes &addr) { +bool isMulticastAddress(const rtps::Ip4AddressBytes &addr) { return addr[0] >= 224 && addr[0] <= 239; } @@ -91,7 +91,7 @@ const EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) const { } std::string EsppTransport::ip4ToString( - const platform::transport::Ip4AddressBytes &addr) { + const Ip4AddressBytes &addr) { return std::to_string(addr[0]) + "." + std::to_string(addr[1]) + "." + std::to_string(addr[2]) + "." + std::to_string(addr[3]); } @@ -158,7 +158,7 @@ void EsppTransport::onReceive(Ip4Port_t receivePort, std::vector &data, return; } - platform::transport::Ip4AddressBytes remoteAddress{0, 0, 0, 0}; + Ip4AddressBytes remoteAddress{0, 0, 0, 0}; (void)parseIp4Address(sender.address, remoteAddress); m_rxCallback(m_callbackArgs, data.data(), data.size(), receivePort, @@ -178,7 +178,7 @@ bool EsppTransport::ensureReceivePort(Ip4Port_t receivePort) { } bool EsppTransport::joinMultiCastGroup( - const platform::transport::Ip4AddressBytes &addr) const { + const Ip4AddressBytes &addr) const { std::lock_guard lock(m_mutex); const std::string group = ip4ToString(addr); @@ -217,7 +217,7 @@ void EsppTransport::sendPacket(PacketInfo &info) { } espp::UdpSocket::SendConfig send_config; - const platform::transport::Ip4AddressBytes destination = info.destAddr; + const Ip4AddressBytes destination = info.destAddr; send_config.ip_address = ip4ToString(destination); send_config.port = info.destPort; send_config.is_multicast_endpoint = isMulticastAddress(info.destAddr); diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps_embedded/src/entities/Domain.cpp index 8aacb4100..f3c11553b 100644 --- a/components/rtps_embedded/src/entities/Domain.cpp +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -45,7 +45,7 @@ Author: i11 - Embedded Software, RWTH Aachen University using rtps::Domain; -Domain::Domain(const platform::transport::Ip4AddressBytes &localIpAddress) +Domain::Domain(const rtps::Ip4AddressBytes &localIpAddress) : m_threadPool(receiveJumppad, this), m_defaultTransport(ThreadPool::onDatagram, &m_threadPool), m_transport(&m_defaultTransport), @@ -53,8 +53,8 @@ Domain::Domain(const platform::transport::Ip4AddressBytes &localIpAddress) initializeTransport(); } -Domain::Domain(platform::transport::ITransport &transport, - const platform::transport::Ip4AddressBytes &localIpAddress) +Domain::Domain(rtps::EsppTransport &transport, + const rtps::Ip4AddressBytes &localIpAddress) : m_threadPool(receiveJumppad, this), m_defaultTransport(ThreadPool::onDatagram, &m_threadPool), m_transport(&transport), @@ -416,8 +416,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, rtps::Reader *Domain::createReader(Participant &part, const char *topicName, const char *typeName, bool reliable, - platform::transport::Ip4AddressBytes - mcastaddress) { + rtps::Ip4AddressBytes mcastaddress) { std::lock_guard lock(m_mutex); StatelessReader *statelessReader = getNextUnusedEndpoint( diff --git a/components/rtps_embedded/src/entities/Participant.cpp b/components/rtps_embedded/src/entities/Participant.cpp index dcb44b427..aa6fa1c5b 100644 --- a/components/rtps_embedded/src/entities/Participant.cpp +++ b/components/rtps_embedded/src/entities/Participant.cpp @@ -62,7 +62,7 @@ void Participant::reuse(const GuidPrefix_t &guidPrefix, void Participant::reuse( const GuidPrefix_t &guidPrefix, ParticipantId_t participantId, - const platform::transport::Ip4AddressBytes &localIpAddress) { + const Ip4AddressBytes &localIpAddress) { m_guidPrefix = guidPrefix; m_participantId = participantId; m_localIpAddress = localIpAddress; diff --git a/components/rtps_embedded/src/messages/MessageTypes.cpp b/components/rtps_embedded/src/messages/MessageTypes.cpp index bf56695d5..5301f03fc 100644 --- a/components/rtps_embedded/src/messages/MessageTypes.cpp +++ b/components/rtps_embedded/src/messages/MessageTypes.cpp @@ -23,6 +23,8 @@ Author: i11 - Embedded Software, RWTH Aachen University */ #include "rtps/messages/MessageTypes.h" + +#include #include #include @@ -132,7 +134,7 @@ bool rtps::deserializeMessage(const MessageProcessingInfo &info, } void rtps::deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, - size_t num_bitfields) { + std::size_t num_bitfields) { doCopyAndMoveOn(reinterpret_cast(&set.base.high), position, sizeof(SequenceNumber_t::high)); @@ -144,7 +146,8 @@ void rtps::deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, // Ensure that we copy not more bits than our sequence number set can hold if (set.numBits != 0) { // equal to size = std::min(SNS_NUM_BYTES, num_bitfields) - size_t size = num_bitfields > SNS_NUM_BYTES ? SNS_NUM_BYTES : num_bitfields; + std::size_t size = + num_bitfields > SNS_NUM_BYTES ? SNS_NUM_BYTES : num_bitfields; doCopyAndMoveOn(reinterpret_cast(set.bitMap.data()), position, size); position += (num_bitfields - size); @@ -173,7 +176,8 @@ bool rtps::deserializeMessage(const MessageProcessingInfo &info, msg.writerId.entityKey.size()); msg.writerId.entityKind = static_cast(*currentPos++); - size_t num_bitfields = msg.header.octetsToNextHeader - 4 - 4 - 8 - 4 - 4; + std::size_t num_bitfields = + msg.header.octetsToNextHeader - 4 - 4 - 8 - 4 - 4; deserializeSNS(currentPos, msg.readerSNState, num_bitfields); doCopyAndMoveOn(reinterpret_cast(&msg.count.value), currentPos, @@ -209,7 +213,8 @@ bool rtps::deserializeMessage(const MessageProcessingInfo &info, doCopyAndMoveOn(reinterpret_cast(&msg.gapStart.low), currentPos, sizeof(msg.gapStart.low)); - size_t num_bitfields = msg.header.octetsToNextHeader - 4 - 4 - 8 - 8 - 4; + std::size_t num_bitfields = + msg.header.octetsToNextHeader - 4 - 4 - 8 - 8 - 4; deserializeSNS(currentPos, msg.gapList, num_bitfields); return true; From a0edd0a61897c6e3213c18600af42f49fc8ace6c Mon Sep 17 00:00:00 2001 From: Guo Date: Tue, 14 Jul 2026 16:44:21 -0500 Subject: [PATCH 10/17] update to use base component --- .../rtps_embedded/include/rtps/ThreadPool.h | 3 +- .../rtps/communication/EsppTransport.h | 3 +- .../rtps/discovery/ParticipantProxyData.h | 54 ++++++---- .../include/rtps/discovery/SEDPAgent.h | 6 +- .../include/rtps/discovery/SPDPAgent.h | 13 +-- .../include/rtps/entities/Domain.h | 7 +- .../include/rtps/entities/Participant.h | 3 +- .../include/rtps/entities/Reader.h | 3 +- .../include/rtps/entities/StatefulReader.tpp | 35 ++---- .../include/rtps/entities/StatefulWriter.tpp | 39 +++---- .../include/rtps/entities/StatelessWriter.tpp | 21 ++-- .../include/rtps/entities/Writer.h | 7 +- .../include/rtps/messages/MessageReceiver.h | 3 +- .../include/rtps/utils/printutils.h | 2 + components/rtps_embedded/src/ThreadPool.cpp | 18 ++-- .../src/communication/EsppTransport.cpp | 18 +++- .../src/discovery/ParticipantProxyData.cpp | 83 ++++++++------ .../rtps_embedded/src/discovery/SEDPAgent.cpp | 50 ++++----- .../rtps_embedded/src/discovery/SPDPAgent.cpp | 17 ++- .../rtps_embedded/src/discovery/TopicData.cpp | 22 +++- .../rtps_embedded/src/entities/Domain.cpp | 61 +++++------ .../src/entities/Participant.cpp | 102 +++++++++--------- .../rtps_embedded/src/entities/Reader.cpp | 6 +- .../src/entities/StatelessReader.cpp | 1 - .../rtps_embedded/src/entities/Writer.cpp | 3 + .../src/messages/MessageReceiver.cpp | 34 +++--- 26 files changed, 319 insertions(+), 295 deletions(-) diff --git a/components/rtps_embedded/include/rtps/ThreadPool.h b/components/rtps_embedded/include/rtps/ThreadPool.h index 9b6ad648f..d936eaf95 100644 --- a/components/rtps_embedded/include/rtps/ThreadPool.h +++ b/components/rtps_embedded/include/rtps/ThreadPool.h @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_THREADPOOL_H #define RTPS_THREADPOOL_H +#include "base_component.hpp" #include "rtps/communication/PacketInfo.h" #include "rtps/common/types.h" #include "rtps/config.h" @@ -46,7 +47,7 @@ class ThreadPool; namespace rtps { -class ThreadPool { +class ThreadPool : public espp::BaseComponent { public: using receiveJumppad_fp = void (*)(void *callee, const PacketInfo &packet); diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.h b/components/rtps_embedded/include/rtps/communication/EsppTransport.h index 4cdf8439b..c8bca8993 100644 --- a/components/rtps_embedded/include/rtps/communication/EsppTransport.h +++ b/components/rtps_embedded/include/rtps/communication/EsppTransport.h @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_ESPPTRANSPORT_H #define RTPS_ESPPTRANSPORT_H +#include "base_component.hpp" #include "rtps/common/types.h" #include "rtps/config.h" #include "udp_socket.hpp" @@ -38,7 +39,7 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { -class EsppTransport { +class EsppTransport : public espp::BaseComponent { public: using RxCallback = ReceiveCallback; diff --git a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h index b9eb11235..1f27697ea 100644 --- a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h +++ b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h @@ -25,11 +25,14 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_PARTICIPANTPROXYDATA_H #define RTPS_PARTICIPANTPROXYDATA_H +#include "base_component.hpp" #include "rtps/config.h" #include "rtps/messages/MessageTypes.h" #include "ucdr/microcdr.h" #include +#include #include +#include #include namespace rtps { @@ -37,18 +40,22 @@ namespace rtps { class Participant; using SMElement::ParameterId; -typedef uint32_t BuiltinEndpointSet_t; +using BuiltinEndpointSet_t = uint32_t; -class ParticipantProxyData { +class ParticipantProxyData : public espp::BaseComponent { public: - ParticipantProxyData() { onAliveSignal(); } + ParticipantProxyData() + : espp::BaseComponent("RtpsParticipantProxy", + espp::Logger::Verbosity::WARN) { + onAliveSignal(); + } ParticipantProxyData(Guid_t guid); ProtocolVersion_t m_protocolVersion = PROTOCOLVERSION; Guid_t m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; VendorId_t m_vendorId = VENDOR_UNKNOWN; bool m_expectsInlineQos = false; - BuiltinEndpointSet_t m_availableBuiltInEndpoints; + BuiltinEndpointSet_t m_availableBuiltInEndpoints{0}; std::array m_metatrafficUnicastLocatorList; std::array @@ -59,21 +66,22 @@ class ParticipantProxyData { m_defaultMulticastLocatorList; Count_t m_manualLivelinessCount{1}; Duration_t m_leaseDuration = Config::SPDP_DEFAULT_REMOTE_LEASE_DURATION; - std::chrono::time_point m_lastLivelinessReceivedTimestamp; + std::chrono::time_point + m_lastLivelinessReceivedTimestamp; void reset(); bool readFromUcdrBuffer(ucdrBuffer &buffer, Participant *participant); - inline bool hasParticipantWriter(); - inline bool hasParticipantReader(); - inline bool hasPublicationWriter(); - inline bool hasPublicationReader(); - inline bool hasSubscriptionWriter(); - inline bool hasSubscriptionReader(); + inline bool hasParticipantWriter() const; + inline bool hasParticipantReader() const; + inline bool hasPublicationWriter() const; + inline bool hasPublicationReader() const; + inline bool hasSubscriptionWriter() const; + inline bool hasSubscriptionReader() const; inline void onAliveSignal(); - inline bool isAlive(); - inline uint32_t getAliveSignalAgeInMilliseconds(); + inline bool isAlive() const; + inline uint32_t getAliveSignalAgeInMilliseconds() const; private: bool readLocatorIntoList( @@ -107,32 +115,32 @@ class ParticipantProxyData { }; // Needs to be in header because they are marked with inline -bool ParticipantProxyData::hasParticipantWriter() { +bool ParticipantProxyData::hasParticipantWriter() const { return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER) == 1; } -bool ParticipantProxyData::hasParticipantReader() { +bool ParticipantProxyData::hasParticipantReader() const { return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR) != 0; } -bool ParticipantProxyData::hasPublicationWriter() { +bool ParticipantProxyData::hasPublicationWriter() const { return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_PUBLICATION_ANNOUNCER) != 0; } -bool ParticipantProxyData::hasPublicationReader() { +bool ParticipantProxyData::hasPublicationReader() const { return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_PUBLICATION_DETECTOR) != 0; } -bool ParticipantProxyData::hasSubscriptionWriter() { +bool ParticipantProxyData::hasSubscriptionWriter() const { return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_ANNOUNCER) != 0; } -bool ParticipantProxyData::hasSubscriptionReader() { +bool ParticipantProxyData::hasSubscriptionReader() const { return (m_availableBuiltInEndpoints & DISC_BUILTIN_ENDPOINT_SUBSCRIPTION_DETECTOR) != 0; } @@ -141,17 +149,17 @@ void ParticipantProxyData::onAliveSignal() { m_lastLivelinessReceivedTimestamp = std::chrono::steady_clock::now(); } -uint32_t ParticipantProxyData::getAliveSignalAgeInMilliseconds() { +uint32_t ParticipantProxyData::getAliveSignalAgeInMilliseconds() const { auto now = std::chrono::steady_clock::now(); - auto duration = - std::chrono::duration_cast(now - m_lastLivelinessReceivedTimestamp); + auto duration = std::chrono::duration_cast( + now - m_lastLivelinessReceivedTimestamp); return static_cast(duration.count()); } /* * Returns true if last heartbeat within lease duration, else false */ -bool ParticipantProxyData::isAlive() { +bool ParticipantProxyData::isAlive() const { uint32_t lease_in_ms = m_leaseDuration.seconds * 1000 + m_leaseDuration.fraction * 1e-6; diff --git a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h index f7e651070..0b2618c8b 100644 --- a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h +++ b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_SEDPAGENT_H #define RTPS_SEDPAGENT_H +#include "base_component.hpp" #include "rtps/discovery/BuiltInEndpoints.h" #include "rtps/config.h" #include "rtps/discovery/TopicData.h" @@ -39,8 +40,9 @@ class ReaderCacheChange; class Writer; class Reader; -class SEDPAgent { +class SEDPAgent : public espp::BaseComponent { public: + SEDPAgent(); void init(Participant &part, const BuiltInEndpoints &endpoints); bool addWriter(Writer &writer); bool addReader(Reader &reader); @@ -64,7 +66,7 @@ class SEDPAgent { const ReaderCacheChange &change); private: - Participant *m_part; + Participant *m_part = nullptr; std::recursive_mutex m_mutex; uint8_t m_buffer[600]; // TODO check size, currently changed from 300 to 600 // (FastDDS gives too many options) diff --git a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h index fc0ac56ba..b75028642 100644 --- a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h +++ b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_SPDP_H #define RTPS_SPDP_H +#include "base_component.hpp" #include "rtps/common/types.h" #include "rtps/config.h" #include "rtps/discovery/BuiltInEndpoints.h" @@ -38,14 +39,9 @@ Author: i11 - Embedded Software, RWTH Aachen University #if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" -#define SPDP_LOG(...) \ - if (true) { \ - printf("[SPDP] "); \ - printf(__VA_ARGS__); \ - printf("\r\n"); \ - } +#define SPDP_LOG(...) logger_.warn(__VA_ARGS__) #else -#define SPDP_LOG(...) // +#define SPDP_LOG(...) do { } while (0) #endif namespace rtps { @@ -54,8 +50,9 @@ class Writer; class Reader; class ReaderCacheChange; -class SPDPAgent { +class SPDPAgent : public espp::BaseComponent { public: + SPDPAgent(); void init(Participant &participant, BuiltInEndpoints &endpoints); void start(); void stop(); diff --git a/components/rtps_embedded/include/rtps/entities/Domain.h b/components/rtps_embedded/include/rtps/entities/Domain.h index 6f59b9a63..93bf0bbcd 100644 --- a/components/rtps_embedded/include/rtps/entities/Domain.h +++ b/components/rtps_embedded/include/rtps/entities/Domain.h @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_DOMAIN_H #define RTPS_DOMAIN_H +#include "base_component.hpp" #include "rtps/ThreadPool.h" #include "rtps/communication/EsppTransport.h" #include "rtps/config.h" @@ -38,11 +39,11 @@ Author: i11 - Embedded Software, RWTH Aachen University #include namespace rtps { -class Domain { +class Domain : public espp::BaseComponent { public: - explicit Domain(const Ip4AddressBytes &localIpAddress); + explicit Domain(const Ip4AddressBytes &localIpAddress); Domain(EsppTransport &transport, - const Ip4AddressBytes &localIpAddress); + const Ip4AddressBytes &localIpAddress); ~Domain(); bool completeInit(); diff --git a/components/rtps_embedded/include/rtps/entities/Participant.h b/components/rtps_embedded/include/rtps/entities/Participant.h index b58002422..5427aa8c4 100644 --- a/components/rtps_embedded/include/rtps/entities/Participant.h +++ b/components/rtps_embedded/include/rtps/entities/Participant.h @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_PARTICIPANT_H #define RTPS_PARTICIPANT_H +#include "base_component.hpp" #include "rtps/common/types.h" #include "rtps/config.h" #include "rtps/discovery/SEDPAgent.h" @@ -40,7 +41,7 @@ namespace rtps { class Writer; class Reader; -class Participant { +class Participant : public espp::BaseComponent { public: GuidPrefix_t m_guidPrefix; ParticipantId_t m_participantId; diff --git a/components/rtps_embedded/include/rtps/entities/Reader.h b/components/rtps_embedded/include/rtps/entities/Reader.h index 506e8e569..0716f5dcd 100644 --- a/components/rtps_embedded/include/rtps/entities/Reader.h +++ b/components/rtps_embedded/include/rtps/entities/Reader.h @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_READER_H #define RTPS_READER_H +#include "base_component.hpp" #include "rtps/common/types.h" #include "rtps/config.h" #include "rtps/discovery/TopicData.h" @@ -78,7 +79,7 @@ class ReaderCacheChange { typedef void (*ddsReaderCallback_fp)(void *callee, const ReaderCacheChange &cacheChange); -class Reader { +class Reader : public espp::BaseComponent { public: using callbackFunction_t = void (*)(void *, const ReaderCacheChange &); using callbackIdentifier_t = uint32_t; diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp index a05f151d6..cc2dc3507 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -31,14 +31,9 @@ Author: i11 - Embedded Software, RWTH Aachen University #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" -#define SFR_LOG(...) \ - if (true) { \ - printf("[StatefulReader %s] ", &m_attributes.topicName[0]); \ - printf(__VA_ARGS__); \ - printf("\r\n"); \ - } +#define SFR_LOG(...) logger_.warn(__VA_ARGS__) #else -#define SFR_LOG(...) // +#define SFR_LOG(...) do { } while (0) #endif using rtps::StatefulReaderT; @@ -71,7 +66,7 @@ void StatefulReaderT::newChange( for (auto &proxy : m_proxies) { if (proxy.remoteWriterGuid == cacheChange.writerGuid) { if (proxy.expectedSN == cacheChange.sn) { - SFR_LOG("Delivering SN %u.%u | ! GUID %u %u %u %u \r\n", + SFR_LOG("Delivering SN {}.{} | GUID {} {} {} {}", (int)cacheChange.sn.high, (int)cacheChange.sn.low, cacheChange.writerGuid.prefix.id[0], cacheChange.writerGuid.prefix.id[1], @@ -79,13 +74,13 @@ void StatefulReaderT::newChange( cacheChange.writerGuid.prefix.id[3]); executeCallbacks(cacheChange); ++proxy.expectedSN; - SFR_LOG("Done processing SN %u.%u\r\n", (int)cacheChange.sn.high, + SFR_LOG("Done processing SN {}.{}", (int)cacheChange.sn.high, (int)cacheChange.sn.low); return; } else { Diagnostics::StatefulReader::sfr_unexpected_sn++; SFR_LOG( - "Unexpected SN %u.%u != %u.%u, dropping! GUID %u %u %u %u | \r\n", + "Unexpected SN {}.{} != {}.{}, dropping! GUID {} {} {} {}", (int)proxy.expectedSN.high, (int)proxy.expectedSN.low, (int)cacheChange.sn.high, (int)cacheChange.sn.low, cacheChange.writerGuid.prefix.id[0], @@ -101,9 +96,7 @@ template bool StatefulReaderT::addNewMatchedWriter( const WriterProxy &newProxy) { #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE - SFR_LOG("New writer added with id: "); - printGuid(newProxy.remoteWriterGuid); - SFR_LOG("\n"); + SFR_LOG("New writer added"); #endif return m_proxies.add(newProxy); } @@ -115,7 +108,7 @@ bool StatefulReaderT::onNewGapMessage( if (!m_is_initialized_) { return false; } - SFR_LOG("Processing gap message %d.%u %d.%u", (int)msg.gapStart.high, + SFR_LOG("Processing gap message {}.{} {}.{}", (int)msg.gapStart.high, (unsigned int)msg.gapStart.low, (int)msg.gapList.base.high, (unsigned int)msg.gapList.base.low); @@ -127,10 +120,7 @@ bool StatefulReaderT::onNewGapMessage( if (writer == nullptr) { #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE - SFR_LOG("Ignore GAP. Couldn't find a matching " - "writer with id:"); - printEntityId(msg.writerId); - SFR_LOG("\n"); + SFR_LOG("Ignore GAP. Couldn't find a matching writer"); #endif return false; } @@ -236,10 +226,7 @@ bool StatefulReaderT::onNewHeartbeat( if (writer == nullptr) { #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE - SFR_LOG("Ignore heartbeat. Couldn't find a matching " - "writer with id:"); - printEntityId(msg.writerId); - SFR_LOG("\n"); + SFR_LOG("Ignore heartbeat. Couldn't find a matching writer"); #endif return false; } @@ -260,7 +247,7 @@ bool StatefulReaderT::onNewHeartbeat( missing_sns, writer->getNextAckNackCount(), final_flag); - SFR_LOG("Sending acknack base %u bits %u .\n", (int)missing_sns.base.low, + SFR_LOG("Sending acknack base {} bits {}.", (int)missing_sns.base.low, (int)missing_sns.numBits); info.payload = std::move(payload.bytes); if (info.payload.empty()) { @@ -293,7 +280,7 @@ bool StatefulReaderT::sendPreemptiveAckNack( payload, writer.remoteWriterGuid.entityId, m_attributes.endpointGuid.entityId, number_set, Count_t{1}, false); - SFR_LOG("Sending preemptive acknack.\n"); + SFR_LOG("Sending preemptive acknack."); info.payload = std::move(payload.bytes); if (info.payload.empty()) { return false; diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp index b77b3c587..ee14095a3 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -37,14 +37,9 @@ using rtps::StatefulWriterT; #if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" -#define SFW_LOG(...) \ - if (true) { \ - printf("[Stateful Writer %s] ", this->m_attributes.topicName); \ - printf(__VA_ARGS__); \ - printf("\r\n"); \ - } +#define SFW_LOG(...) logger_.warn(__VA_ARGS__) #else -#define SFW_LOG(...) // +#define SFW_LOG(...) do { } while (0) #endif template @@ -81,7 +76,7 @@ bool StatefulWriterT::init(TopicData attributes, m_hbCount = {1}; if (!m_disposeWithDelay.init()) { - SFW_LOG("Failed to initialize delayed dispose buffer mutex.\n"); + SFW_LOG("Failed to initialize delayed dispose buffer mutex."); return false; } @@ -149,7 +144,7 @@ const rtps::CacheChange *StatefulWriterT::newChange( m_nextSequenceNumberToSend = newMin; // Make sure we have the correct sn to send } - SFW_LOG("History full! Dropping changes %s.\r\n", this->m_attributes.topicName); + SFW_LOG("History full! Dropping changes {}.", this->m_attributes.topicName); } auto *result = @@ -158,7 +153,7 @@ const rtps::CacheChange *StatefulWriterT::newChange( mp_threadPool->addWorkload(this); } - SFW_LOG("Adding new data.\n"); + SFW_LOG("Adding new data."); return result; } @@ -178,11 +173,11 @@ template void StatefulWriterT::progress() { } } - SFW_LOG("Sending data with SN %u.%u", (int)m_nextSequenceNumberToSend.low, + SFW_LOG("Sending data with SN {}.{}", (int)m_nextSequenceNumberToSend.low, (int)m_nextSequenceNumberToSend.high); if (next->disposeAfterWrite) { - SFW_LOG("Dispose after write msg sent to %u proxies\r\n", (int)i); + SFW_LOG("Dispose after write msg sent to {} proxies", (int)i); } /* @@ -199,7 +194,7 @@ template void StatefulWriterT::progress() { SFW_LOG("Failed to enqueue dispose after write!"); m_history.dropChange(next->sequenceNumber); } else { - SFW_LOG("Delayed dispose scheduled for sn %u %u\r\n", + SFW_LOG("Delayed dispose scheduled for sn {} {}", (int)next->sequenceNumber.high, (int)next->sequenceNumber.low); } } @@ -208,7 +203,7 @@ template void StatefulWriterT::progress() { sendHeartBeat(); } else { - SFW_LOG("Couldn't get a CacheChange with SN (%li,%lu)\n", + SFW_LOG("Couldn't get a CacheChange with SN ({},{})", m_nextSequenceNumberToSend.high, m_nextSequenceNumberToSend.low); } } @@ -285,7 +280,7 @@ void StatefulWriterT::onNewAckNack( return; } - SFW_LOG("Received non-preemptive acknack with %lu bits set.\r\n", + SFW_LOG("Received non-preemptive acknack with {} bits set.", msg.readerSNState.numBits); for (uint32_t i = 0; i < msg.readerSNState.numBits && nextSN <= m_history.getLastUsedSequenceNumber(); @@ -293,17 +288,17 @@ void StatefulWriterT::onNewAckNack( if (msg.readerSNState.isSet(i)) { - SFW_LOG("Looking for change %lu | Bit %lu", nextSN.low, i); + SFW_LOG("Looking for change {} | Bit {}", nextSN.low, i); const rtps::CacheChange *cache = m_history.getChangeBySN(nextSN); // We still have the cache, send DATA if (cache != nullptr) { if (cache->disposeAfterWrite) { - SFW_LOG("SERVING FROM DISPOSE AFTER WRITE CACHE\r\n"); + SFW_LOG("Serving from dispose-after-write cache"); } sendData(*reader, cache); } else { - SFW_LOG("> Change not found, search for next valid SN %lu \r\n", + SFW_LOG("> Change not found, search for next valid SN {}", nextSN.low); // Cache not found, look for next valid SN rtps::SequenceNumber_t gapBegin = nextSN; @@ -459,7 +454,7 @@ void StatefulWriterT::sendHeartBeatLoop() { // Temporarily increase HB frequency if there are unconfirmed remote changes if (unconfirmed_changes) { - SFW_LOG("HB SPEEDUP!\r\n"); + SFW_LOG("HB speedup"); std::this_thread::sleep_for( std::chrono::milliseconds(Config::SF_WRITER_HB_PERIOD_MS / 4)); } else { @@ -492,7 +487,7 @@ void StatefulWriterT::dropDisposeAfterWriteChanges() { auto age = std::chrono::steady_clock::now() - change->sentTime; if (age > std::chrono::milliseconds(4000)) { m_history.dropChange(change->sequenceNumber); - SFW_LOG("Removing SN %u %u for good\r\n", + SFW_LOG("Removing SN {} {} for good", static_cast(oldest_retained.low), static_cast(oldest_retained.high)); SequenceNumber_t tmp; @@ -510,7 +505,7 @@ void StatefulWriterT::sendHeartBeat() { INIT_GUARD() if (m_proxies.isEmpty() || !m_is_initialized_) { - SFW_LOG("Skipping heartbeat. No proxies.\n"); + SFW_LOG("Skipping heartbeat. No proxies."); return; } @@ -555,7 +550,7 @@ void StatefulWriterT::sendHeartBeat() { } } - SFW_LOG("Sending HB with SN range [%lu.%lu;%lu.%lu]", firstSN.low, firstSN.high, + SFW_LOG("Sending HB with SN range [{}.{};{}.{}]", firstSN.low, firstSN.high, lastSN.low, lastSN.high); MessageFactory::addHeartbeat( diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp index c670ffcb0..c2e98cb47 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp @@ -38,14 +38,9 @@ using rtps::StatelessWriterT; #if SLW_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" -#define SLW_LOG(...) \ - if (true) { \ - printf("[StatelessWriter %s] ", &this->m_attributes.topicName[0]); \ - printf(__VA_ARGS__); \ - printf("\n"); \ - } +#define SLW_LOG(...) logger_.warn(__VA_ARGS__) #else -#define SLW_LOG(...) // +#define SLW_LOG(...) do { } while (0) #endif template @@ -104,7 +99,7 @@ const CacheChange *StatelessWriterT::newChange( m_nextSequenceNumberToSend = newMin; // Make sure we have the correct sn to send } - SLW_LOG("History is full, dropping oldest %s\r\n", this->m_attributes.topicName); + SLW_LOG("History is full, dropping oldest {}", this->m_attributes.topicName); } auto *result = m_history.addChange(data, size); @@ -112,7 +107,7 @@ const CacheChange *StatelessWriterT::newChange( mp_threadPool->addWorkload(this); } - SLW_LOG("Adding new data.\n"); + SLW_LOG("Adding new data."); return result; } @@ -149,12 +144,12 @@ void StatelessWriterT::progress() { // after adjusting values. if (m_proxies.getNumElements() == 0) { - SLW_LOG("No Proxy!\n"); + SLW_LOG("No proxy!"); } for (const auto &proxy : m_proxies) { - SLW_LOG("Progess.\n"); + SLW_LOG("Progress."); // Do nothing, if someone else sends for me... (Multicast) if (proxy.useMulticast || !proxy.suppressUnicast || m_enforceUnicast) { PacketInfo info; @@ -175,7 +170,7 @@ void StatelessWriterT::progress() { m_nextSequenceNumberToSend.low); return; } else { - SLW_LOG("Sending change with SN (%li,%li)\n", + SLW_LOG("Sending change with SN ({},{})", m_nextSequenceNumberToSend.high, m_nextSequenceNumberToSend.low); } @@ -207,7 +202,7 @@ void StatelessWriterT::progress() { info.destAddr = proxy.remoteLocator.getIp4AddressBytes(); info.destPort = (Ip4Port_t)proxy.remoteLocator.port; } - SLW_LOG("Sending to %u.%u.%u.%u:%d\n", info.destAddr[0], info.destAddr[1], + SLW_LOG("Sending to {}.{}.{}.{}:{}", info.destAddr[0], info.destAddr[1], info.destAddr[2], info.destAddr[3], info.destPort); if (info.payload.empty()) { continue; diff --git a/components/rtps_embedded/include/rtps/entities/Writer.h b/components/rtps_embedded/include/rtps/entities/Writer.h index 76c895ca8..37afa3f21 100644 --- a/components/rtps_embedded/include/rtps/entities/Writer.h +++ b/components/rtps_embedded/include/rtps/entities/Writer.h @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_WRITER_H #define RTPS_WRITER_H +#include "base_component.hpp" #include "rtps/ThreadPool.h" #include "rtps/discovery/TopicData.h" #include "rtps/entities/ReaderProxy.h" @@ -41,8 +42,9 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifdef COMPILE_INIT_GUARD #define INIT_GUARD() \ if (!m_is_initialized_) { \ + logger_.error("Using uninitialized endpoint"); \ while (1) { \ - printf("using uninitalized endpoint\n;"); \ + } \ } \ } #else @@ -51,7 +53,7 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { -class Writer { +class Writer : public espp::BaseComponent { public: TopicData m_attributes; virtual bool addNewMatchedReader(const ReaderProxy &newProxy); @@ -84,6 +86,7 @@ class Writer { bool isBuiltinEndpoint(); protected: + Writer(); SequenceNumber_t m_sedp_sequence_number; std::recursive_mutex m_mutex; diff --git a/components/rtps_embedded/include/rtps/messages/MessageReceiver.h b/components/rtps_embedded/include/rtps/messages/MessageReceiver.h index a4a62109f..be821e404 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageReceiver.h +++ b/components/rtps_embedded/include/rtps/messages/MessageReceiver.h @@ -25,6 +25,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_MESSAGERECEIVER_H #define RTPS_MESSAGERECEIVER_H +#include "base_component.hpp" #include "rtps/common/types.h" #include "rtps/config.h" #include "rtps/discovery/BuiltInEndpoints.h" @@ -36,7 +37,7 @@ class Writer; class Participant; class MessageProcessingInfo; -class MessageReceiver { +class MessageReceiver : public espp::BaseComponent { public: GuidPrefix_t sourceGuidPrefix = GUIDPREFIX_UNKNOWN; ProtocolVersion_t sourceVersion = PROTOCOLVERSION; diff --git a/components/rtps_embedded/include/rtps/utils/printutils.h b/components/rtps_embedded/include/rtps/utils/printutils.h index 4d4d1bfa7..3fc599344 100644 --- a/components/rtps_embedded/include/rtps/utils/printutils.h +++ b/components/rtps_embedded/include/rtps/utils/printutils.h @@ -12,6 +12,8 @@ inline void printEntityId(rtps::EntityId_t id) { printf("%x", (int)byte); } printf("%x", static_cast(id.entityKind)); + printf("\n"); + } inline void printGuidPrefix(rtps::GuidPrefix_t prefix) { diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp index 3ead47ef8..234d42a35 100644 --- a/components/rtps_embedded/src/ThreadPool.cpp +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -35,18 +35,14 @@ using rtps::ThreadPool; #if THREAD_POOL_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" -#define THREAD_POOL_LOG(...) \ - if (true) { \ - printf("[ThreadPool] "); \ - printf(__VA_ARGS__); \ - printf("\r\n"); \ - } +#define THREAD_POOL_LOG(...) logger_.warn(__VA_ARGS__) #else -#define THREAD_POOL_LOG(...) // +#define THREAD_POOL_LOG(...) do { } while (0) #endif ThreadPool::ThreadPool(receiveJumppad_fp receiveCallback, void *callee) - : m_receiveJumppad(receiveCallback), m_callee(callee) { + : espp::BaseComponent("RtpsThreadPool", espp::Logger::Verbosity::WARN), + m_receiveJumppad(receiveCallback), m_callee(callee) { if (!m_outgoingMetaTraffic.init() || !m_outgoingUserTraffic.init() || !m_incomingMetaTraffic.init() || !m_incomingUserTraffic.init()) { @@ -201,7 +197,7 @@ bool ThreadPool::addNewPacket(PacketInfo &&packet) { res = m_incomingUserTraffic.moveElementIntoBuffer(std::move(packet)); } if (!res) { - THREAD_POOL_LOG("failed to enqueue packet for port %u", + THREAD_POOL_LOG("failed to enqueue packet for port {}", static_cast(packet.destPort)); return false; } @@ -262,7 +258,7 @@ void ThreadPool::onDatagram( } if (!pool.addNewPacket(std::move(packet))) { - THREAD_POOL_LOG("ThreadPool: dropped packet\n"); + pool.logger_.warn("ThreadPool: dropped packet"); if (pool.isBuiltinPort(remotePort)) { rtps::Diagnostics::ThreadPool::dropped_incoming_packets_metatraffic++; } else { @@ -298,7 +294,7 @@ void ThreadPool::doReaderWork() { if (isUserWorkToDo || isMetaWorkToDo) { continue; } - THREAD_POOL_LOG("ReaderWorker | User = %u, Meta = %u\r\n", + THREAD_POOL_LOG("ReaderWorker | User = {}, Meta = {}", static_cast(Diagnostics::ThreadPool::processed_incoming_usertraffic), static_cast(Diagnostics::ThreadPool::processed_incoming_metatraffic)); updateDiagnostics(); diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps_embedded/src/communication/EsppTransport.cpp index d969c4182..28874d508 100644 --- a/components/rtps_embedded/src/communication/EsppTransport.cpp +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -24,7 +24,6 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/communication/EsppTransport.h" -#include "rtps/communication/PacketInfo.h" #include "task.hpp" #include @@ -70,7 +69,8 @@ bool isMulticastAddress(const rtps::Ip4AddressBytes &addr) { } // namespace EsppTransport::EsppTransport(RxCallback callback, void *args) - : m_rxCallback(callback), m_callbackArgs(args) {} + : espp::BaseComponent("RtpsTransport", espp::Logger::Verbosity::WARN), + m_rxCallback(callback), m_callbackArgs(args) {} EsppTransport::Channel *EsppTransport::findChannel(Ip4Port_t port) { for (auto &channel : m_channels) { @@ -98,6 +98,7 @@ std::string EsppTransport::ip4ToString( bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { if (!channel.socket) { + logger_.error("startReceiver called with null socket on port {}", receivePort); return false; } @@ -117,7 +118,11 @@ bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { return std::nullopt; }; - return channel.socket->start_receiving(task_config, receive_config); + const bool started = channel.socket->start_receiving(task_config, receive_config); + if (!started) { + logger_.error("Failed to start UDP receiver on port {}", receivePort); + } + return started; } EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort) { @@ -130,6 +135,7 @@ EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort) { socket_config.log_level = espp::Logger::Verbosity::WARN; channel.socket = std::make_unique(socket_config); if (!channel.socket || !channel.socket->is_valid()) { + logger_.error("Failed to create valid UDP socket for port {}", receivePort); channel.socket.reset(); return nullptr; } @@ -159,7 +165,10 @@ void EsppTransport::onReceive(Ip4Port_t receivePort, std::vector &data, } Ip4AddressBytes remoteAddress{0, 0, 0, 0}; - (void)parseIp4Address(sender.address, remoteAddress); + if (!parseIp4Address(sender.address, remoteAddress)) { + logger_.warn("Could not parse sender IPv4 address '{}', using 0.0.0.0", + sender.address); + } m_rxCallback(m_callbackArgs, data.data(), data.size(), receivePort, static_cast(sender.port), remoteAddress); @@ -209,6 +218,7 @@ void EsppTransport::sendPacket(PacketInfo &info) { } if (channel == nullptr || !channel->socket) { + logger_.error("No UDP channel available for source port {}", info.srcPort); return; } diff --git a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp index 9e2e3222b..7b833cf35 100644 --- a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp +++ b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp @@ -22,13 +22,21 @@ This file is part of embeddedRTPS. Author: i11 - Embedded Software, RWTH Aachen University */ -#include "rtps/discovery/SPDPAgent.h" +#include "rtps/discovery/ParticipantProxyData.h" #include "rtps/entities/Participant.h" #include "rtps/utils/Log.h" using rtps::ParticipantProxyData; -ParticipantProxyData::ParticipantProxyData(Guid_t guid) : m_guid(guid) {} +#if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE +#define PPD_LOG(...) logger_.warn(__VA_ARGS__) +#else +#define PPD_LOG(...) do { } while (0) +#endif + +ParticipantProxyData::ParticipantProxyData(Guid_t guid) + : espp::BaseComponent("RtpsParticipantProxy", espp::Logger::Verbosity::WARN), + m_guid(guid) {} void ParticipantProxyData::reset() { m_guid = Guid_t{GUIDPREFIX_UNKNOWN, ENTITYID_UNKNOWN}; @@ -48,24 +56,26 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, reset(); SMElement::ParameterId pid; uint16_t length; - SPDP_LOG("Start deserializing ParticipantProxyData\n"); - SPDP_LOG("Buffer has %u bytes remaining\n", ucdr_buffer_remaining(&buffer)); - SPDP_LOG("first 20 bytes of data: "); + PPD_LOG("Start deserializing ParticipantProxyData"); + PPD_LOG("Buffer has {} bytes remaining", ucdr_buffer_remaining(&buffer)); + PPD_LOG("first 20 bytes of data:"); for (int i = 0; i < 20 && i < ucdr_buffer_remaining(&buffer); ++i) { - SPDP_LOG("%02x ", buffer.iterator[i]); + PPD_LOG("{:02x}", buffer.iterator[i]); } - SPDP_LOG("\n"); - SPDP_LOG("before start, buff length: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); + PPD_LOG("before start, buff length: {}. last data size: {}", + ucdr_buffer_length(&buffer), buffer.last_data_size); while (ucdr_buffer_remaining(&buffer) >= 4) { ucdr_deserialize_uint16_t(&buffer, reinterpret_cast(&pid)); - SPDP_LOG("buff length after get id: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); + PPD_LOG("buff length after get id: {}. last data size: {}", + ucdr_buffer_length(&buffer), buffer.last_data_size); ucdr_deserialize_uint16_t(&buffer, &length); - SPDP_LOG("buff length after get length: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); - SPDP_LOG("Deserializing parameter with id %u and length %u\n", - static_cast(pid), length); + PPD_LOG("buff length after get length: {}. last data size: {}", + ucdr_buffer_length(&buffer), buffer.last_data_size); + PPD_LOG("Deserializing parameter with id {} and length {}", + static_cast(pid), length); if (ucdr_buffer_remaining(&buffer) < length) { - SPDP_LOG("Not enough data left in buffer to read parameter with id %u and length %u\n", + PPD_LOG("Not enough data left in buffer to read parameter with id {} and length {}", static_cast(pid), length); return false; } @@ -79,24 +89,26 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, case ParameterId::PID_PROTOCOL_VERSION: { ucdr_deserialize_uint8_t(&buffer, &m_protocolVersion.major); if (m_protocolVersion.major < PROTOCOLVERSION.major) { - SPDP_LOG("Unsupported protocol version: %u.%u\n", m_protocolVersion.major, + PPD_LOG("Unsupported protocol version: {}.{}", m_protocolVersion.major, m_protocolVersion.minor); return false; } else { ucdr_deserialize_uint8_t(&buffer, &m_protocolVersion.minor); } - SPDP_LOG("Protocol version: %u.%u\n", m_protocolVersion.major, - m_protocolVersion.minor); - SPDP_LOG("buff length: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); + PPD_LOG("Protocol version: {}.{}", m_protocolVersion.major, + m_protocolVersion.minor); + PPD_LOG("buff length: {}. last data size: {}", + ucdr_buffer_length(&buffer), buffer.last_data_size); break; } case ParameterId::PID_VENDORID: { ucdr_deserialize_array_uint8_t(&buffer, m_vendorId.vendorId.data(), m_vendorId.vendorId.size()); - SPDP_LOG(" vendor id struct size: %d\n", m_vendorId.vendorId.size()); - SPDP_LOG("Vendor ID: %u %u\n", m_vendorId.vendorId[0], - m_vendorId.vendorId[1]); - SPDP_LOG("buff length: %d. last data size: %d, offset: %d", ucdr_buffer_length(&buffer), buffer.last_data_size, buffer.offset); + PPD_LOG("vendor id struct size: {}", m_vendorId.vendorId.size()); + PPD_LOG("Vendor ID: {} {}", m_vendorId.vendorId[0], + m_vendorId.vendorId[1]); + PPD_LOG("buff length: {}. last data size: {}", + ucdr_buffer_length(&buffer), buffer.last_data_size); break; } @@ -112,35 +124,35 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, ucdr_deserialize_uint8_t( &buffer, reinterpret_cast(&m_guid.entityId.entityKind)); if (participant->findRemoteParticipant(m_guid.prefix)) { - SPDP_LOG("stopping deserialization early, participant is known\n"); + PPD_LOG("stopping deserialization early, participant is known"); return true; } break; } case ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR: { if (!readLocatorIntoList(buffer, m_metatrafficMulticastLocatorList)) { - SPDP_LOG("Failed to read metatraffic multicast locator\n"); + PPD_LOG("Failed to read metatraffic multicast locator"); return false; } break; } case ParameterId::PID_METATRAFFIC_UNICAST_LOCATOR: { if (!readLocatorIntoList(buffer, m_metatrafficUnicastLocatorList)) { - SPDP_LOG("Failed to read metatraffic unicast locator\n"); + PPD_LOG("Failed to read metatraffic unicast locator"); return false; } break; } case ParameterId::PID_DEFAULT_UNICAST_LOCATOR: { if (!readLocatorIntoList(buffer, m_defaultUnicastLocatorList)) { - SPDP_LOG("Failed to read default unicast locator\n"); + PPD_LOG("Failed to read default unicast locator"); return false; } break; } case ParameterId::PID_DEFAULT_MULTICAST_LOCATOR: { if (!readLocatorIntoList(buffer, m_defaultMulticastLocatorList)) { - SPDP_LOG("Failed to read default multicast locator\n"); + PPD_LOG("Failed to read default multicast locator"); return false; } break; @@ -185,15 +197,15 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, // should not return false for unknown parameter, just skip it, otherwise we might miss some important information if the remote participant is using some vendor specific parameters that we do not know about. // TODO: GUO: need read out the data for the length of the parameter, otherwise the buffer will be in wrong state and the following parameters cannot be read correctly. For now just skip the data by moving the iterator forward, but we might want to actually read out the data and store it for future use if needed, especially for some vendor specific parameters that we do not know about. // buffer.iterator += length; - ucdr_advance_buffer(&buffer, length); + buffer.iterator += length; + buffer.last_data_size = 1; break; } } // Parameter lists are 4-byte aligned uint32_t alignment = ucdr_buffer_alignment(&buffer, 4); - SPDP_LOG("Alignment for next parameter: %lu\n", alignment); - ucdr_advance_buffer(&buffer, alignment); - // buffer.iterator += alignment; - // buffer.last_data_size = 4; + PPD_LOG("Alignment for next parameter: {}", alignment); + buffer.iterator += alignment; + buffer.last_data_size = 4; } return true; } @@ -208,12 +220,12 @@ bool ParticipantProxyData::readLocatorIntoList( bool ret = full_length_locator.readFromUcdrBuffer(buffer); if (ret && full_length_locator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { proxy_locator = LocatorIPv4(full_length_locator); - SPDP_LOG("Adding locator: %u %u %u %u", + PPD_LOG("Adding locator: {} {} {} {}", (int)proxy_locator.address[0], (int)proxy_locator.address[1], (int)proxy_locator.address[2], (int)proxy_locator.address[3]); return true; } else { - SPDP_LOG("Ignoring locator: %u %u %u %u", + PPD_LOG("Ignoring locator: {} {} {} {}", (int)full_length_locator.address[12], (int)full_length_locator.address[13], (int)full_length_locator.address[14], @@ -224,11 +236,12 @@ bool ParticipantProxyData::readLocatorIntoList( valid_locators++; if (valid_locators == Config::SPDP_MAX_NUM_LOCATORS) { buffer.iterator += sizeof(FullLengthLocator); - SPDP_LOG("Max number of valid locators exceed, ignoring this locator " - "as we have at least one valid locator\n"); + PPD_LOG("Max number of valid locators exceeded, ignoring this locator as we have at least one valid locator"); return true; } } } return false; } + +#undef PPD_LOG diff --git a/components/rtps_embedded/src/discovery/SEDPAgent.cpp b/components/rtps_embedded/src/discovery/SEDPAgent.cpp index a5c0a01ec..07c0a26ad 100644 --- a/components/rtps_embedded/src/discovery/SEDPAgent.cpp +++ b/components/rtps_embedded/src/discovery/SEDPAgent.cpp @@ -35,16 +35,14 @@ Author: i11 - Embedded Software, RWTH Aachen University using rtps::SEDPAgent; #if SEDP_VERBOSE && RTPS_GLOBAL_VERBOSE -#define SEDP_LOG(...) \ - if (true) { \ - printf("[SEDP] "); \ - printf(__VA_ARGS__); \ - printf("\r\n"); \ - } +#define SEDP_LOG(...) logger_.warn(__VA_ARGS__) #else -#define SEDP_LOG(...) // +#define SEDP_LOG(...) do { } while (0) #endif +SEDPAgent::SEDPAgent() + : espp::BaseComponent("RtpsSEDP", espp::Logger::Verbosity::WARN) {} + void SEDPAgent::init(Participant &part, const BuiltInEndpoints &endpoints) { m_part = ∂ m_endpoints = endpoints; @@ -84,12 +82,12 @@ void SEDPAgent::jumppadSubscriptionReader( void SEDPAgent::handlePublisherReaderMessage(const ReaderCacheChange &change) { std::lock_guard lock(m_mutex); #if SEDP_VERBOSE - SEDP_LOG("New publisher\n"); + SEDP_LOG("New publisher"); #endif if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { #if SEDP_VERBOSE - SEDP_LOG("EDPAgent: Buffer too small.\n"); + SEDP_LOG("EDPAgent: Buffer too small."); #endif return; } @@ -114,11 +112,11 @@ void SEDPAgent::addUnmatchedRemoteWriter( const TopicDataCompressed &writerData) { if (m_unmatchedRemoteWriters.isFull()) { #if SEDP_VERBOSE - SEDP_LOG("List of unmatched remote writers is full.\n"); + SEDP_LOG("List of unmatched remote writers is full."); #endif return; } - SEDP_LOG("Adding unmatched remote writer %zx %zx.\n", writerData.topicHash, + SEDP_LOG("Adding unmatched remote writer {:x} {:x}.", writerData.topicHash, writerData.typeHash); m_unmatchedRemoteWriters.add(writerData); } @@ -127,11 +125,11 @@ void SEDPAgent::addUnmatchedRemoteReader( const TopicDataCompressed &readerData) { if (m_unmatchedRemoteReaders.isFull()) { #if SEDP_VERBOSE - SEDP_LOG("List of unmatched remote readers is full.\n"); + SEDP_LOG("List of unmatched remote readers is full."); #endif return; } - SEDP_LOG("Adding unmatched remote reader %zx %zx.\n", readerData.topicHash, + SEDP_LOG("Adding unmatched remote reader {:x} {:x}.", readerData.topicHash, readerData.typeHash); m_unmatchedRemoteReaders.add(readerData); } @@ -186,12 +184,12 @@ void SEDPAgent::handlePublisherReaderMessage(const TopicData &writerData, } #if SEDP_VERBOSE - SEDP_LOG("PUB T/D %s/%s", writerData.topicName, writerData.typeName); + SEDP_LOG("PUB T/D {}/{}", writerData.topicName, writerData.typeName); #endif Reader *reader = m_part->getMatchingReader(writerData); if (reader == nullptr) { #if SEDP_VERBOSE - SEDP_LOG("SEDPAgent: Couldn't find reader for new Publisher[%s, %s] \n", + SEDP_LOG("SEDPAgent: Couldn't find reader for new Publisher[{}, {}]", writerData.topicName, writerData.typeName); #endif addUnmatchedRemoteWriter(writerData); @@ -199,13 +197,11 @@ void SEDPAgent::handlePublisherReaderMessage(const TopicData &writerData, } // TODO check policies #if SEDP_VERBOSE - SEDP_LOG("Found a new "); if (writerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { - SEDP_LOG("reliable "); + SEDP_LOG("Found a new reliable publisher"); } else { - SEDP_LOG("best-effort "); + SEDP_LOG("Found a new best-effort publisher"); } - SEDP_LOG("publisher\n"); #endif reader->addNewMatchedWriter( WriterProxy{writerData.endpointGuid, writerData.unicastLocator, @@ -219,7 +215,7 @@ void SEDPAgent::handleSubscriptionReaderMessage( const ReaderCacheChange &change) { std::lock_guard lock(m_mutex); #if SEDP_VERBOSE - SEDP_LOG("New subscriber\n"); + SEDP_LOG("New subscriber"); #endif if (!change.copyInto(m_buffer, sizeof(m_buffer) / sizeof(m_buffer[0]))) { @@ -239,7 +235,7 @@ void SEDPAgent::handleSubscriptionReaderMessage( void SEDPAgent::handleRemoteEndpointDeletion(const TopicData &topic, const ReaderCacheChange &change) { - SEDP_LOG("Endpoint deletion message SN %u.%u GUID %u %u %u %u \r\n", + SEDP_LOG("Endpoint deletion message SN {}.{} GUID {} {} {} {}", (int)change.sn.high, (int)change.sn.low, change.writerGuid.prefix.id[0], change.writerGuid.prefix.id[1], change.writerGuid.prefix.id[2], change.writerGuid.prefix.id[3]); @@ -271,11 +267,11 @@ void SEDPAgent::handleSubscriptionReaderMessage( Writer *writer = m_part->getMatchingWriter(readerData); #if SEDP_VERBOSE - SEDP_LOG("SUB T/D %s/%s", readerData.topicName, readerData.typeName); + SEDP_LOG("SUB T/D {}/{}", readerData.topicName, readerData.typeName); #endif if (writer == nullptr) { #if SEDP_VERBOSE - SEDP_LOG("SEDPAgent: Couldn't find writer for new subscriber[%s, %s]\n", + SEDP_LOG("SEDPAgent: Couldn't find writer for new subscriber[{}, {}]", readerData.topicName, readerData.typeName); #endif addUnmatchedRemoteReader(readerData); @@ -284,13 +280,11 @@ void SEDPAgent::handleSubscriptionReaderMessage( // TODO check policies #if SEDP_VERBOSE - SEDP_LOG("Found a new "); if (readerData.reliabilityKind == ReliabilityKind_t::RELIABLE) { - SEDP_LOG("reliable "); + SEDP_LOG("Found a new reliable subscriber"); } else { - SEDP_LOG("best-effort "); + SEDP_LOG("Found a new best-effort subscriber"); } - SEDP_LOG("Subscriber\n"); #endif if (readerData.multicastLocator.kind == rtps::LocatorKind_t::LOCATOR_KIND_UDPv4) { @@ -407,7 +401,7 @@ bool SEDPAgent::announceEndpointDeletion(A *local_endpoint, auto ret = sedp_endpoint->newChange(ChangeKind_t::ALIVE, m_buffer, ucdr_buffer_length(µbuffer), true, true); - SEDP_LOG("Annoucing endpoint delete, SN = %u.%u\r\n", + SEDP_LOG("Announcing endpoint delete, SN = {}.{}", (int)ret->sequenceNumber.low, (int)ret->sequenceNumber.high); return (ret != nullptr); } diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp index 39f26dca9..380a9e83c 100644 --- a/components/rtps_embedded/src/discovery/SPDPAgent.cpp +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -39,6 +39,9 @@ using rtps::SPDPAgent; using rtps::SMElement::BuildInEndpointSet; using rtps::SMElement::ParameterId; +SPDPAgent::SPDPAgent() + : espp::BaseComponent("RtpsSPDP", espp::Logger::Verbosity::WARN) {} + void SPDPAgent::init(Participant &participant, BuiltInEndpoints &endpoints) { mp_participant = &participant; m_buildInEndpoints = endpoints; @@ -106,13 +109,13 @@ void SPDPAgent::receiveCallback(void *callee, void SPDPAgent::handleSPDPPackage(const ReaderCacheChange &cacheChange) { if (!initialized) { - SPDP_LOG("Callback called without initialization\n"); + SPDP_LOG("Callback called without initialization"); return; } std::lock_guard lock(m_mutex); if (cacheChange.size > m_inputBuffer.size()) { - SPDP_LOG("Input buffer to small\n"); + SPDP_LOG("Input buffer too small"); return; } @@ -120,7 +123,7 @@ void SPDPAgent::handleSPDPPackage(const ReaderCacheChange &cacheChange) { if (!cacheChange.copyInto(m_inputBuffer.data(), m_inputBuffer.size())) { return; } - SPDP_LOG("SPDPPackage size: %d", cacheChange.size); + SPDP_LOG("SPDPPackage size: {}", cacheChange.size); ucdrBuffer buffer; ucdr_init_buffer(&buffer, m_inputBuffer.data(), m_inputBuffer.size()); @@ -133,7 +136,7 @@ void SPDPAgent::handleSPDPPackage(const ReaderCacheChange &cacheChange) { // TODO In case we store the history we can free the history mutex here processProxyData(); } else { - SPDP_LOG("ParticipantProxyData deserializtaion failed\n"); + SPDP_LOG("ParticipantProxyData deserialization failed"); } } else { // TODO RemoveParticipant @@ -160,7 +163,11 @@ void SPDPAgent::processProxyData() { return; // Our own packet } - SPDP_LOG("Message from GUID = %u %u %u %u", m_proxyDataBuffer.m_guid.prefix.id[4], m_proxyDataBuffer.m_guid.prefix.id[5], m_proxyDataBuffer.m_guid.prefix.id[6], m_proxyDataBuffer.m_guid.prefix.id[7]); + SPDP_LOG("Message from GUID = {} {} {} {}", + m_proxyDataBuffer.m_guid.prefix.id[4], + m_proxyDataBuffer.m_guid.prefix.id[5], + m_proxyDataBuffer.m_guid.prefix.id[6], + m_proxyDataBuffer.m_guid.prefix.id[7]); const rtps::ParticipantProxyData *remote_part; remote_part = mp_participant->findRemoteParticipant(m_proxyDataBuffer.m_guid.prefix); diff --git a/components/rtps_embedded/src/discovery/TopicData.cpp b/components/rtps_embedded/src/discovery/TopicData.cpp index 9f0ad73db..5f55702b9 100644 --- a/components/rtps_embedded/src/discovery/TopicData.cpp +++ b/components/rtps_embedded/src/discovery/TopicData.cpp @@ -23,6 +23,7 @@ Author: i11 - Embedded Software, RWTH Aachen University */ #include "rtps/discovery/TopicData.h" #include "rtps/messages/MessageTypes.h" +#include "logger.hpp" #include #include @@ -30,6 +31,11 @@ using rtps::TopicData; using rtps::TopicDataCompressed; using rtps::SMElement::ParameterId; +namespace { +espp::Logger s_topic_data_logger( + {.tag = "RtpsTopicData", .level = espp::Logger::Verbosity::WARN}); +} + bool TopicData::isDisposedFlagSet() const { return statusInfoValid && ((statusInfo & 0b1)); } @@ -51,7 +57,7 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { while (ucdr_buffer_remaining(&buffer) >= 4) { if (ucdr_buffer_has_error(&buffer)) { - printf("FAILED TO DESERIALIZE TOPIC DATA\n"); + s_topic_data_logger.error("FAILED TO DESERIALIZE TOPIC DATA"); return false; // while (1) { // printf("FAILED TO DESERIALIZE TOPIC DATA\n"); @@ -101,12 +107,20 @@ bool TopicData::readFromUcdrBuffer(ucdrBuffer &buffer) { // unavailable (e.g. early startup) to avoid keeping placeholder defaults. if (uLoc.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { unicastLocator = uLoc; - printf ("Received unicast locator: %d.%d.%d.%d:%ld\n", uLoc.address[12], uLoc.address[13], uLoc.address[14], uLoc.address[15], uLoc.port); + const auto a0 = static_cast(uLoc.address[12]); + const auto a1 = static_cast(uLoc.address[13]); + const auto a2 = static_cast(uLoc.address[14]); + const auto a3 = static_cast(uLoc.address[15]); + const auto port = static_cast(uLoc.port); + s_topic_data_logger.warn( + "Received unicast locator: {}.{}.{}.{}:{}", a0, a1, a2, a3, + port); } else { // print warning and the invalid locator for debugging - printf("Warning: Received invalid unicast locator with kind %d\n", - static_cast(uLoc.kind)); + s_topic_data_logger.warn( + "Warning: Received invalid unicast locator with kind {}", + static_cast(uLoc.kind)); } break; case ParameterId::PID_MULTICAST_LOCATOR: diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps_embedded/src/entities/Domain.cpp index f3c11553b..641849b44 100644 --- a/components/rtps_embedded/src/entities/Domain.cpp +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -33,20 +33,16 @@ Author: i11 - Embedded Software, RWTH Aachen University #endif #if DOMAIN_VERBOSE && RTPS_GLOBAL_VERBOSE -#define DOMAIN_LOG(...) \ - if (true) { \ - printf("[Domain] "); \ - printf(__VA_ARGS__); \ - printf("\n"); \ - } +#define DOMAIN_LOG(...) logger_.warn(__VA_ARGS__) #else -#define DOMAIN_LOG(...) // +#define DOMAIN_LOG(...) do { } while (0) #endif using rtps::Domain; Domain::Domain(const rtps::Ip4AddressBytes &localIpAddress) - : m_threadPool(receiveJumppad, this), + : espp::BaseComponent("RtpsDomain", espp::Logger::Verbosity::WARN), + m_threadPool(receiveJumppad, this), m_defaultTransport(ThreadPool::onDatagram, &m_threadPool), m_transport(&m_defaultTransport), m_localIpAddress(localIpAddress) { @@ -55,7 +51,8 @@ Domain::Domain(const rtps::Ip4AddressBytes &localIpAddress) Domain::Domain(rtps::EsppTransport &transport, const rtps::Ip4AddressBytes &localIpAddress) - : m_threadPool(receiveJumppad, this), + : espp::BaseComponent("RtpsDomain", espp::Logger::Verbosity::WARN), + m_threadPool(receiveJumppad, this), m_defaultTransport(ThreadPool::onDatagram, &m_threadPool), m_transport(&transport), m_localIpAddress(localIpAddress) { @@ -75,7 +72,7 @@ bool Domain::completeInit() { m_initComplete = m_threadPool.startThreads(); if (!m_initComplete) { - DOMAIN_LOG("Failed starting threads\n"); + DOMAIN_LOG("Failed starting threads"); } for (auto i = 0; i < m_nextParticipantId; i++) { @@ -93,7 +90,7 @@ void Domain::receiveJumppad(void *callee, const PacketInfo &packet) { void Domain::receiveCallback(const PacketInfo &packet) { if (packet.payload.empty()) { - DOMAIN_LOG("Dropping packet without payload\n"); + DOMAIN_LOG("Dropping packet without payload"); return; } @@ -102,7 +99,7 @@ void Domain::receiveCallback(const PacketInfo &packet) { if (isMetaMultiCastPort(packet.destPort)) { // Pass to all - DOMAIN_LOG("Domain: Multicast to port %u\n", packet.destPort); + DOMAIN_LOG("Domain: Multicast to port {}", packet.destPort); for (auto i = 0; i < m_nextParticipantId - PARTICIPANT_START_ID; ++i) { m_participants[i].newMessage(payload, payload_size); } @@ -110,11 +107,11 @@ void Domain::receiveCallback(const PacketInfo &packet) { } else if (isUserMultiCastPort(packet.destPort)) { // Pass to Participant with assigned Multicast Adress (Port ist everytime // the same) - DOMAIN_LOG("Domain: Got user multicast message on port %u\n", + DOMAIN_LOG("Domain: Got user multicast message on port {}", packet.destPort); for (auto i = 0; i < m_nextParticipantId - PARTICIPANT_START_ID; ++i) { if (m_participants[i].hasReaderWithMulticastLocator(packet.destAddr)) { - DOMAIN_LOG("Domain: Forward Multicast only to Participant: %u\n", i); + DOMAIN_LOG("Domain: Forward Multicast only to Participant: {}", i); m_participants[i].newMessage(payload, payload_size); } } @@ -123,17 +120,17 @@ void Domain::receiveCallback(const PacketInfo &packet) { ParticipantId_t id = getParticipantIdFromUnicastPort( packet.destPort, isUserPort(packet.destPort)); if (id != PARTICIPANT_ID_INVALID) { - DOMAIN_LOG("Domain: Got unicast message on port %u\n", packet.destPort); + DOMAIN_LOG("Domain: Got unicast message on port {}", packet.destPort); if (id < m_nextParticipantId && id >= PARTICIPANT_START_ID) { // added extra check to avoid segfault // (id below START_ID) m_participants[id - PARTICIPANT_START_ID].newMessage( payload, payload_size); } else { - DOMAIN_LOG("Domain: Participant id too high or unplausible.\n"); + DOMAIN_LOG("Domain: Participant id too high or unplausible."); } } else { - DOMAIN_LOG("Domain: Got message to port %u: no matching participant\n", + DOMAIN_LOG("Domain: Got message to port {}: no matching participant", packet.destPort); } } @@ -141,7 +138,7 @@ void Domain::receiveCallback(const PacketInfo &packet) { rtps::Participant *Domain::createParticipant() { - DOMAIN_LOG("Domain: Creating new participant.\n"); + DOMAIN_LOG("Domain: Creating new participant."); auto nextSlot = static_cast(m_nextParticipantId - PARTICIPANT_START_ID); @@ -273,7 +270,7 @@ rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, continue; } - DOMAIN_LOG("StatefulReader exists already [%s, %s]\n", topicName, + DOMAIN_LOG("StatefulReader exists already [{}, {}]", topicName, typeName); return &m_statefulReaders[i]; @@ -292,7 +289,7 @@ rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, continue; } - DOMAIN_LOG("StatelessReader exists [%s, %s]\n", topicName, typeName); + DOMAIN_LOG("StatelessReader exists [{}, {}]", topicName, typeName); return &m_statelessReaders[i]; } @@ -318,7 +315,7 @@ rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, continue; } - DOMAIN_LOG("StatefulWriter exists [%s, %s]\n", topicName, typeName); + DOMAIN_LOG("StatefulWriter exists [{}, {}]", topicName, typeName); return &m_statefulWriters[i]; } @@ -336,7 +333,7 @@ rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, continue; } - DOMAIN_LOG("StatelessWriter exists [%s, %s]\n", topicName, typeName); + DOMAIN_LOG("StatelessWriter exists [{}, {}]", topicName, typeName); return &m_statelessWriters[i]; } @@ -361,7 +358,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, if ((reliable && statefulWriter == nullptr) || (!reliable && statelessWriter == nullptr) || part.isWritersFull()) { - DOMAIN_LOG("No Writer created. Max Number of Writers reached.\n"); + DOMAIN_LOG("No Writer created. Max Number of Writers reached."); return nullptr; } @@ -383,14 +380,14 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, getUserUnicastLocator(part.m_participantId, m_localIpAddress); attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; - DOMAIN_LOG("Creating writer[%s, %s]\n", topicName, typeName); + DOMAIN_LOG("Creating writer[{}, {}]", topicName, typeName); if (reliable) { attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; if (!statefulWriter->init(attributes, TopicKind_t::NO_KEY, &m_threadPool, *m_transport, enforceUnicast)) { - DOMAIN_LOG("StatefulWriter init failed.\n"); + DOMAIN_LOG("StatefulWriter init failed."); return nullptr; } @@ -403,7 +400,7 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, if (!statelessWriter->init(attributes, TopicKind_t::NO_KEY, &m_threadPool, *m_transport, enforceUnicast)) { - DOMAIN_LOG("StatelessWriter init failed.\n"); + DOMAIN_LOG("StatelessWriter init failed."); return nullptr; } @@ -428,7 +425,7 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, if ((reliable && statefulReader == nullptr) || (!reliable && statelessReader == nullptr) || part.isReadersFull()) { - DOMAIN_LOG("No Reader created. Max Number of Readers reached.\n"); + DOMAIN_LOG("No Reader created. Max Number of Readers reached."); return nullptr; } @@ -460,16 +457,16 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, attributes.multicastLocator.address[15]}); registerMulticastPort(attributes.multicastLocator); - DOMAIN_LOG("Multicast enabled!\n"); + DOMAIN_LOG("Multicast enabled!"); } else { - DOMAIN_LOG("This is not a Multicastaddress!\n"); + DOMAIN_LOG("This is not a Multicastaddress!"); } } attributes.durabilityKind = DurabilityKind_t::VOLATILE; - DOMAIN_LOG("Creating reader[%s, %s]\n", topicName, typeName); + DOMAIN_LOG("Creating reader[{}, {}]", topicName, typeName); if (reliable) { @@ -478,7 +475,7 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, statefulReader->init(attributes, *m_transport); if (!part.addReader(statefulReader)) { - DOMAIN_LOG("Failed to add reader to participant.\n"); + DOMAIN_LOG("Failed to add reader to participant."); return nullptr; } @@ -524,7 +521,7 @@ bool rtps::Domain::deleteWriter(Participant &part, Writer *writer) { void rtps::Domain::printInfo() { for (unsigned int i = 0; i < m_participants.size(); i++) { - DOMAIN_LOG("Participant %u\r\n", i); + DOMAIN_LOG("Participant {}", i); m_participants[i].printInfo(); } } diff --git a/components/rtps_embedded/src/entities/Participant.cpp b/components/rtps_embedded/src/entities/Participant.cpp index aa6fa1c5b..80e2f9462 100644 --- a/components/rtps_embedded/src/entities/Participant.cpp +++ b/components/rtps_embedded/src/entities/Participant.cpp @@ -30,24 +30,21 @@ Author: i11 - Embedded Software, RWTH Aachen University #include #if PARTICIPANT_VERBOSE && RTPS_GLOBAL_VERBOSE -#define PARTICIPANT_LOG(...) \ - if (true) { \ - printf("[Participant] "); \ - printf(__VA_ARGS__); \ - printf("\r\n"); \ - } +#define PARTICIPANT_LOG(...) logger_.warn(__VA_ARGS__) #else -#define PARTICIPANT_LOG(...) // +#define PARTICIPANT_LOG(...) do { } while (0) #endif using rtps::Participant; Participant::Participant() - : m_guidPrefix(GUIDPREFIX_UNKNOWN), m_participantId(PARTICIPANT_ID_INVALID), + : espp::BaseComponent("RtpsParticipant", espp::Logger::Verbosity::WARN), + m_guidPrefix(GUIDPREFIX_UNKNOWN), m_participantId(PARTICIPANT_ID_INVALID), m_receiver(this) {} Participant::Participant(const GuidPrefix_t &guidPrefix, ParticipantId_t participantId) - : m_guidPrefix(guidPrefix), m_participantId(participantId), + : espp::BaseComponent("RtpsParticipant", espp::Logger::Verbosity::WARN), + m_guidPrefix(guidPrefix), m_participantId(participantId), m_receiver(this) {} Participant::~Participant() { m_spdpAgent.stop(); } @@ -334,7 +331,7 @@ void Participant::removeProxyFromAllEndpoints(const Guid_t &guid) { continue; } if (m_writers[i]->removeProxy(guid)) { - PARTICIPANT_LOG("Removing proxy for writer [%s, %s], proxies left = %u\n", + PARTICIPANT_LOG("Removing proxy for writer [{}, {}], proxies left = {}", m_writers[i]->m_attributes.topicName, m_writers[i]->m_attributes.typeName, (int)m_writers[i]->getProxiesCount()); @@ -346,7 +343,7 @@ void Participant::removeProxyFromAllEndpoints(const Guid_t &guid) { continue; } if (m_readers[i]->removeProxy(guid)) { - PARTICIPANT_LOG("Removing proxy for reader [%s, %s], proxies left = %u\n", + PARTICIPANT_LOG("Removing proxy for reader [{}, {}], proxies left = {}", m_readers[i]->m_attributes.topicName, m_readers[i]->m_attributes.typeName, (int)m_readers[i]->getProxiesCount()); @@ -407,14 +404,14 @@ rtps::MessageReceiver *Participant::getMessageReceiver() { return &m_receiver; } bool Participant::checkAndResetHeartbeats() { std::lock_guard lock1(m_mutex); std::lock_guard lock2(m_spdpAgent.m_mutex); - PARTICIPANT_LOG("Have %u remote participants", + PARTICIPANT_LOG("Have {} remote participants", (unsigned int)m_remoteParticipants.getNumElements()); PARTICIPANT_LOG( - "Unmatched remote writers/readers, %u / %u", + "Unmatched remote writers/readers, {} / {}", static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters()), static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); for (auto &remote : m_remoteParticipants) { - PARTICIPANT_LOG("Remote GUID = %u %u %u %u | Age = %u [ms]", + PARTICIPANT_LOG("Remote GUID = {} {} {} {} | Age = {} [ms]", remote.m_guid.prefix.id[4], remote.m_guid.prefix.id[5], remote.m_guid.prefix.id[6], remote.m_guid.prefix.id[7], (unsigned int)remote.getAliveSignalAgeInMilliseconds() ); if (remote.isAlive()) { continue; @@ -439,20 +436,21 @@ void Participant::printInfo() { #ifdef PARTICIPANT_PRINTINFO_LONG if (m_readers[i]->m_attributes.endpointGuid.entityId == ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER) { - printf("Reader %u: SPDP BUILTIN READER | Remote Proxies = %u \r\n ", - i, static_cast(m_readers[i]->getProxiesCount())); + PARTICIPANT_LOG("Reader {}: SPDP BUILTIN READER | Remote Proxies = {}", + i, + static_cast(m_readers[i]->getProxiesCount())); } if (m_readers[i]->m_attributes.endpointGuid.entityId == ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER) { - printf( - "Reader %u: SEDP PUBLICATION READER | Remote Proxies = %u \r\n ", - i, static_cast(m_readers[i]->getProxiesCount())); + PARTICIPANT_LOG( + "Reader {}: SEDP PUBLICATION READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); } if (m_readers[i]->m_attributes.endpointGuid.entityId == ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER) { - printf( - "Reader %u: SEDP SUBSCRIPTION READER | Remote Proxies = %u \r\n", - i, static_cast(m_readers[i]->getProxiesCount())); + PARTICIPANT_LOG( + "Reader {}: SEDP SUBSCRIPTION READER | Remote Proxies = {}", i, + static_cast(m_readers[i]->getProxiesCount())); } #endif continue; @@ -461,12 +459,13 @@ void Participant::printInfo() { max_reader_proxies = std::max(max_reader_proxies, m_readers[i]->getProxiesCount()); #ifdef PARTICIPANT_PRINTINFO_LONG - printf("Reader %u: Topic = %s | Type = %s | Remote Proxies = %u | SEDP " - "SN = %u \r\n ", - i, m_readers[i]->m_attributes.topicName, - m_readers[i]->m_attributes.typeName, - static_cast(m_readers[i]->getProxiesCount()), - static_cast(m_readers[i]->getSEDPSequenceNumber().low)); + PARTICIPANT_LOG( + "Reader {}: Topic = {} | Type = {} | Remote Proxies = {} | SEDP " + "SN = {}", + i, m_readers[i]->m_attributes.topicName, + m_readers[i]->m_attributes.typeName, + static_cast(m_readers[i]->getProxiesCount()), + static_cast(m_readers[i]->getSEDPSequenceNumber().low)); #endif } } @@ -478,20 +477,20 @@ void Participant::printInfo() { #ifdef PARTICIPANT_PRINTINFO_LONG if (m_writers[i]->m_attributes.endpointGuid.entityId == ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER) { - printf("Writer %u: SPDP WRITER | Remote Proxies = %u \r\n ", i, - static_cast(m_writers[i]->getProxiesCount())); + PARTICIPANT_LOG("Writer {}: SPDP WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); } if (m_writers[i]->m_attributes.endpointGuid.entityId == ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER) { - printf( - "Writer %u: SEDP PUBLICATION WRITER | Remote Proxies = %u \r\n ", - i, static_cast(m_writers[i]->getProxiesCount())); + PARTICIPANT_LOG( + "Writer {}: SEDP PUBLICATION WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); } if (m_writers[i]->m_attributes.endpointGuid.entityId == ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER) { - printf( - "Writer %u: SEDP SUBSCRIPTION WRITER | Remote Proxies = %u \r\n ", - i, static_cast(m_writers[i]->getProxiesCount())); + PARTICIPANT_LOG( + "Writer {}: SEDP SUBSCRIPTION WRITER | Remote Proxies = {}", i, + static_cast(m_writers[i]->getProxiesCount())); } #endif continue; @@ -501,24 +500,25 @@ void Participant::printInfo() { max_writer_proxies = std::max(max_writer_proxies, m_writers[i]->getProxiesCount()); #ifdef PARTICIPANT_PRINTINFO_LONG - printf("Writer %u: Topic = %s | Type = %s | Remote Proxies = %u | SEDP " - "SN = %u \r\n ", - i, m_writers[i]->m_attributes.topicName, - m_writers[i]->m_attributes.typeName, - static_cast(m_writers[i]->getProxiesCount()), - static_cast(m_writers[i]->getSEDPSequenceNumber().low)); + PARTICIPANT_LOG( + "Writer {}: Topic = {} | Type = {} | Remote Proxies = {} | SEDP " + "SN = {}", + i, m_writers[i]->m_attributes.topicName, + m_writers[i]->m_attributes.typeName, + static_cast(m_writers[i]->getProxiesCount()), + static_cast(m_writers[i]->getSEDPSequenceNumber().low)); #endif } } - printf("Max Writer Proxies %lu \r\n ", max_writer_proxies); - printf("Max Reader Proxies %lu \r\n ", max_reader_proxies); - printf("Unmatched Remote Readers = %u\r\n", - static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); - printf("Unmatched Remote Writers = %u \r\n ", - static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters())); - printf("Remote Participants = %u \r\n ", - static_cast(m_remoteParticipants.getNumElements())); + PARTICIPANT_LOG("Max Writer Proxies {}", max_writer_proxies); + PARTICIPANT_LOG("Max Reader Proxies {}", max_reader_proxies); + PARTICIPANT_LOG("Unmatched Remote Readers = {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + PARTICIPANT_LOG("Unmatched Remote Writers = {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters())); + PARTICIPANT_LOG("Remote Participants = {}", + static_cast(m_remoteParticipants.getNumElements())); } rtps::SPDPAgent &Participant::getSPDPAgent() { return m_spdpAgent; } @@ -540,6 +540,6 @@ void Participant::addBuiltInEndpoints(BuiltInEndpoints &endpoints) { void Participant::newMessage(const uint8_t *data, DataSize_t size) { if (!m_receiver.processMessage(data, size)) { - PARTICIPANT_LOG("MESSAGE PROCESSING FAILE \r\n"); + PARTICIPANT_LOG("MESSAGE PROCESSING FAILED"); } } diff --git a/components/rtps_embedded/src/entities/Reader.cpp b/components/rtps_embedded/src/entities/Reader.cpp index 41da9909d..1c7d64a28 100644 --- a/components/rtps_embedded/src/entities/Reader.cpp +++ b/components/rtps_embedded/src/entities/Reader.cpp @@ -7,7 +7,10 @@ using namespace rtps; -Reader::Reader() { m_callbacks.fill({nullptr, nullptr, 0}); } +Reader::Reader() + : espp::BaseComponent("RtpsReader", espp::Logger::Verbosity::WARN) { + m_callbacks.fill({nullptr, nullptr, 0}); +} void Reader::executeCallbacks(const ReaderCacheChange &cacheChange) { std::lock_guard lock(m_callback_mutex); @@ -122,7 +125,6 @@ bool Reader::addNewMatchedWriter(const WriterProxy &newProxy) { #if (SFR_VERBOSE || SLR_VERBOSE) && RTPS_GLOBAL_VERBOSE SFR_LOG("New writer added with id: "); printGuid(newProxy.remoteWriterGuid); - SFR_LOG("\n"); #endif return m_proxies.add(newProxy); } diff --git a/components/rtps_embedded/src/entities/StatelessReader.cpp b/components/rtps_embedded/src/entities/StatelessReader.cpp index ec4825d42..394640b78 100644 --- a/components/rtps_embedded/src/entities/StatelessReader.cpp +++ b/components/rtps_embedded/src/entities/StatelessReader.cpp @@ -61,7 +61,6 @@ bool StatelessReader::addNewMatchedWriter(const WriterProxy &newProxy) { #if (SLR_VERBOSE && RTPS_GLOBAL_VERBOSE) SLR_LOG("Adding WriterProxy"); printGuid(newProxy.remoteWriterGuid); - printf("\n"); #endif return m_proxies.add(newProxy); } diff --git a/components/rtps_embedded/src/entities/Writer.cpp b/components/rtps_embedded/src/entities/Writer.cpp index e52d07a81..43a5e3838 100644 --- a/components/rtps_embedded/src/entities/Writer.cpp +++ b/components/rtps_embedded/src/entities/Writer.cpp @@ -8,6 +8,9 @@ using namespace rtps; +Writer::Writer() + : espp::BaseComponent("RtpsWriter", espp::Logger::Verbosity::WARN) {} + bool rtps::Writer::addNewMatchedReader(const ReaderProxy &newProxy) { INIT_GUARD(); #if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE diff --git a/components/rtps_embedded/src/messages/MessageReceiver.cpp b/components/rtps_embedded/src/messages/MessageReceiver.cpp index 589f2e7e9..ddaffb471 100644 --- a/components/rtps_embedded/src/messages/MessageReceiver.cpp +++ b/components/rtps_embedded/src/messages/MessageReceiver.cpp @@ -34,17 +34,14 @@ using rtps::MessageReceiver; #if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" -#define RECV_LOG(...) \ - if (true) { \ - printf("[RECV] "); \ - printf(__VA_ARGS__); \ - printf("\n"); \ - } +#define RECV_LOG(...) logger_.warn(__VA_ARGS__) #else -#define RECV_LOG(...) // +#define RECV_LOG(...) do { } while (0) #endif -MessageReceiver::MessageReceiver(Participant *part) : mp_part(part) {} +MessageReceiver::MessageReceiver(Participant *part) + : espp::BaseComponent("RtpsMessageReceiver", espp::Logger::Verbosity::WARN), + mp_part(part) {} void MessageReceiver::resetState() { sourceGuidPrefix = GUIDPREFIX_UNKNOWN; @@ -78,7 +75,7 @@ bool MessageReceiver::processHeader(MessageProcessingInfo &msgInfo) { } if (header.guidPrefix.id == mp_part->m_guidPrefix.id) { - RECV_LOG("[MessageReceiver]: Received own message.\n"); + RECV_LOG("[MessageReceiver]: Received own message."); return false; // Don't process our own packet } @@ -101,31 +98,31 @@ bool MessageReceiver::processSubmessage(MessageProcessingInfo &msgInfo, switch (submsgHeader.submessageId) { case SubmessageKind::ACKNACK: - RECV_LOG("Processing AckNack submessage\n"); + RECV_LOG("Processing AckNack submessage"); success = processAckNackSubmessage(msgInfo); break; case SubmessageKind::DATA: - RECV_LOG("Processing Data submessage\n"); + RECV_LOG("Processing Data submessage"); success = processDataSubmessage(msgInfo, submsgHeader); break; case SubmessageKind::HEARTBEAT: - RECV_LOG("Processing Heartbeat submessage\n"); + RECV_LOG("Processing Heartbeat submessage"); success = processHeartbeatSubmessage(msgInfo); break; case SubmessageKind::INFO_DST: - RECV_LOG("Info_DST submessage not relevant.\n"); + RECV_LOG("Info_DST submessage not relevant."); success = true; // Not relevant break; case SubmessageKind::GAP: - RECV_LOG("Processing GAP submessage\n"); + RECV_LOG("Processing GAP submessage"); success = processGapSubmessage(msgInfo); break; case SubmessageKind::INFO_TS: - RECV_LOG("Info_TS submessage not relevant.\n"); + RECV_LOG("Info_TS submessage not relevant."); success = true; // Not relevant now break; default: - RECV_LOG("Submessage of type %u currently not supported. Skipping..\n", + RECV_LOG("Submessage of type {} currently not supported. Skipping..", static_cast(submsgHeader.submessageId)); success = false; } @@ -148,14 +145,13 @@ bool MessageReceiver::processDataSubmessage( SubmessageData::getRawSize() + SubmessageHeader::getRawSize(); - RECV_LOG("Received data message size %u", (int)size); + RECV_LOG("Received data message size {}", static_cast(size)); Reader *reader; if (dataSubmsg.readerId == ENTITYID_UNKNOWN) { #if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE RECV_LOG("Received ENTITYID_UNKNOWN readerID, searching for writer ID = "); printGuid(Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); - printf("\n"); #endif reader = mp_part->getReaderByWriterId( Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); @@ -170,7 +166,6 @@ bool MessageReceiver::processDataSubmessage( if (reader_by_writer == nullptr && reader != nullptr) { RECV_LOG("FOUND By READER ID, NOT BY WRITER ID ="); printGuid(Guid_t{sourceGuidPrefix, dataSubmsg.writerId}); - printf("\n"); } #endif } @@ -183,7 +178,6 @@ bool MessageReceiver::processDataSubmessage( #if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE RECV_LOG("Couldn't find a reader with id: "); printEntityId(dataSubmsg.readerId); - printf("\n"); #endif } From 173818402d5e69216cc1ec978cfeeb07ff57dabc Mon Sep 17 00:00:00 2001 From: Guo Date: Thu, 16 Jul 2026 10:20:26 -0500 Subject: [PATCH 11/17] remove micro_CDR --- .gitmodules | 3 --- components/rtps_embedded/thirdparty/Micro-CDR | 1 - 2 files changed, 4 deletions(-) delete mode 160000 components/rtps_embedded/thirdparty/Micro-CDR diff --git a/.gitmodules b/.gitmodules index c4061a1ab..7817faf37 100644 --- a/.gitmodules +++ b/.gitmodules @@ -37,6 +37,3 @@ [submodule "components/gfps_service/detail/nearby"] path = components/gfps_service/detail/nearby url = https://github.com/esp-cpp/nearby -[submodule "components/rtps_embedded/thirdparty/Micro-CDR"] - path = components/rtps_embedded/thirdparty/Micro-CDR - url = git@github.com:eProsima/Micro-CDR.git diff --git a/components/rtps_embedded/thirdparty/Micro-CDR b/components/rtps_embedded/thirdparty/Micro-CDR deleted file mode 160000 index 99672492c..000000000 --- a/components/rtps_embedded/thirdparty/Micro-CDR +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 99672492c5ef9fc378a8835b0bce9b2f7fa41306 From c069ef65439c57c771c93175148bc4fc43b15e29 Mon Sep 17 00:00:00 2001 From: Guo Date: Thu, 16 Jul 2026 10:23:47 -0500 Subject: [PATCH 12/17] add the forked micro-CDR --- .gitmodules | 3 +++ components/rtps_embedded/thirdparty/Micro-CDR | 1 + 2 files changed, 4 insertions(+) create mode 160000 components/rtps_embedded/thirdparty/Micro-CDR diff --git a/.gitmodules b/.gitmodules index 7817faf37..d08d15741 100644 --- a/.gitmodules +++ b/.gitmodules @@ -37,3 +37,6 @@ [submodule "components/gfps_service/detail/nearby"] path = components/gfps_service/detail/nearby url = https://github.com/esp-cpp/nearby +[submodule "components/rtps_embedded/thirdparty/Micro-CDR"] + path = components/rtps_embedded/thirdparty/Micro-CDR + url = git@github.com:esp-cpp/Micro-CDR.git diff --git a/components/rtps_embedded/thirdparty/Micro-CDR b/components/rtps_embedded/thirdparty/Micro-CDR new file mode 160000 index 000000000..99672492c --- /dev/null +++ b/components/rtps_embedded/thirdparty/Micro-CDR @@ -0,0 +1 @@ +Subproject commit 99672492c5ef9fc378a8835b0bce9b2f7fa41306 From ed2e8e5074d5b059d4a95a4d6475e1aa7eca4949 Mon Sep 17 00:00:00 2001 From: Guo Date: Thu, 16 Jul 2026 10:28:30 -0500 Subject: [PATCH 13/17] update micro cdr config --- components/rtps_embedded/thirdparty/Micro-CDR | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/rtps_embedded/thirdparty/Micro-CDR b/components/rtps_embedded/thirdparty/Micro-CDR index 99672492c..550f87de8 160000 --- a/components/rtps_embedded/thirdparty/Micro-CDR +++ b/components/rtps_embedded/thirdparty/Micro-CDR @@ -1 +1 @@ -Subproject commit 99672492c5ef9fc378a8835b0bce9b2f7fa41306 +Subproject commit 550f87de804edf2ad6682e81868dadb6a375b9e0 From 291ddd6706fddb55f86e968cac575d75e1440723 Mon Sep 17 00:00:00 2001 From: Guo Date: Fri, 17 Jul 2026 12:19:33 -0500 Subject: [PATCH 14/17] add pc test; fix heartbeat ping-pang bug --- .../include/rtps/common/Locator.h | 14 +- .../include/rtps/config_desktop.h | 24 ++-- .../rtps_embedded/include/rtps/config_esp32.h | 6 +- .../include/rtps/entities/StatefulReader.tpp | 10 ++ .../include/rtps/entities/StatefulWriter.tpp | 29 +++- .../include/rtps/entities/StatelessWriter.tpp | 2 + .../include/rtps/messages/MessageTypes.h | 14 +- .../rtps_embedded/include/rtps/utils/Log.h | 2 +- .../src/discovery/ParticipantProxyData.cpp | 37 +++-- .../rtps_embedded/src/discovery/SPDPAgent.cpp | 2 +- pc/CMakeLists.txt | 36 ++++- pc/tests/rtps_embedded_publisher.cpp | 80 +++++++++++ pc/tests/rtps_embedded_subscriber.cpp | 133 ++++++++++++++++++ 13 files changed, 354 insertions(+), 35 deletions(-) create mode 100644 pc/tests/rtps_embedded_publisher.cpp create mode 100644 pc/tests/rtps_embedded_subscriber.cpp diff --git a/components/rtps_embedded/include/rtps/common/Locator.h b/components/rtps_embedded/include/rtps/common/Locator.h index 701208fb2..e18714f5c 100644 --- a/components/rtps_embedded/include/rtps/common/Locator.h +++ b/components/rtps_embedded/include/rtps/common/Locator.h @@ -33,6 +33,13 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { +#if defined(_MSC_VER) +#define RTPS_EMBEDDED_PACKED +#pragma pack(push, 1) +#else +#define RTPS_EMBEDDED_PACKED __attribute__((packed)) +#endif + inline bool isSameSubnetAddress( const std::array &addr, const std::array &local) { @@ -115,7 +122,7 @@ struct FullLengthLocator { inline uint32_t getLocatorPort() { return static_cast(port); } -} __attribute__((packed)); +} RTPS_EMBEDDED_PACKED; inline FullLengthLocator getBuiltInUnicastLocator(ParticipantId_t participantId, @@ -192,4 +199,9 @@ struct LocatorIPv4 { } // namespace rtps +#if defined(_MSC_VER) +#pragma pack(pop) +#endif +#undef RTPS_EMBEDDED_PACKED + #endif // RTPS_LOCATOR_T_H diff --git a/components/rtps_embedded/include/rtps/config_desktop.h b/components/rtps_embedded/include/rtps/config_desktop.h index 0d011e7e6..f7b8f5e21 100644 --- a/components/rtps_embedded/include/rtps/config_desktop.h +++ b/components/rtps_embedded/include/rtps/config_desktop.h @@ -22,8 +22,8 @@ This file is part of embeddedRTPS. Author: i11 - Embedded Software, RWTH Aachen University */ -#ifndef RTPS_CONFIG_H -#define RTPS_CONFIG_H +#ifndef RTPS_CONFIG_DESKTOP_H +#define RTPS_CONFIG_DESKTOP_H #include "rtps/common/types.h" @@ -34,7 +34,7 @@ namespace rtps { namespace Config { const VendorId_t VENDOR_ID = {13, 37}; const std::array IP_ADDRESS = { - 192, 168, 1, 2}; // Needs to be set in lwipcfg.h too. + 192, 168, 4, 1}; // Needs to be set in lwipcfg.h too. const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 9}; const uint8_t DOMAIN_ID = 0; // 230 possible with UDP @@ -57,18 +57,21 @@ const uint8_t NUM_READER_PROXIES_PER_WRITER = 3; const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 100; const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 10; -const uint8_t HISTORY_SIZE = 10; +const uint8_t MAX_NUM_READER_CALLBACKS = 5; -const uint8_t MAX_TYPENAME_LENGTH = 20; -const uint8_t MAX_TOPICNAME_LENGTH = 20; +const uint8_t HISTORY_SIZE_STATELESS = 2; +const uint8_t HISTORY_SIZE_STATEFUL = 10; + +const uint8_t MAX_TYPENAME_LENGTH = 64; +const uint8_t MAX_TOPICNAME_LENGTH = 64; const int HEARTBEAT_STACKSIZE = 1200; // byte const int THREAD_POOL_WRITER_STACKSIZE = 1100; // byte const int THREAD_POOL_READER_STACKSIZE = 1600; // byte const uint16_t SPDP_WRITER_STACKSIZE = 550; // byte -const uint16_t SF_WRITER_HB_PERIOD_MS = 500; -const uint16_t SPDP_RESEND_PERIOD_MS = 10000; +const uint16_t SF_WRITER_HB_PERIOD_MS = 2000; +const uint16_t SPDP_RESEND_PERIOD_MS = 1000; const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = 2; // skip x SPDP rounds before checking liveliness const uint8_t SPDP_WRITER_PRIO = 3; @@ -87,7 +90,8 @@ const int THREAD_POOL_NUM_WRITERS = 2; const int THREAD_POOL_NUM_READERS = 2; const int THREAD_POOL_WRITER_PRIO = 3; const int THREAD_POOL_READER_PRIO = 3; -const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH = 10; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC = 10; +const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_METATRAFFIC = 10; constexpr int OVERALL_HEAP_SIZE = THREAD_POOL_NUM_WRITERS * THREAD_POOL_WRITER_STACKSIZE + @@ -97,4 +101,4 @@ constexpr int OVERALL_HEAP_SIZE = } // namespace Config } // namespace rtps -#endif // RTPS_CONFIG_H +#endif // RTPS_CONFIG_DESKTOP_H diff --git a/components/rtps_embedded/include/rtps/config_esp32.h b/components/rtps_embedded/include/rtps/config_esp32.h index 80e1323d1..161cc057a 100644 --- a/components/rtps_embedded/include/rtps/config_esp32.h +++ b/components/rtps_embedded/include/rtps/config_esp32.h @@ -66,7 +66,7 @@ const int THREAD_POOL_READER_STACKSIZE = 4096; // byte const uint16_t SPDP_WRITER_STACKSIZE = 4096; // byte const uint16_t SF_WRITER_HB_PERIOD_MS = 4000; -const uint16_t SPDP_RESEND_PERIOD_MS = 250; +const uint16_t SPDP_RESEND_PERIOD_MS = 1000; const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = 2; // skip x SPDP rounds before checking liveliness const uint8_t SPDP_WRITER_PRIO = 5; @@ -83,8 +83,8 @@ const Duration_t SPDP_LEASE_DURATION = {5, 0}; const int MAX_NUM_UDP_CONNECTIONS = 10; -const int THREAD_POOL_NUM_WRITERS = 1; -const int THREAD_POOL_NUM_READERS = 1; +const int THREAD_POOL_NUM_WRITERS = 2; +const int THREAD_POOL_NUM_READERS = 2; const int THREAD_POOL_WRITER_PRIO = 5; const int THREAD_POOL_READER_PRIO = 5; const int THREAD_POOL_WORKLOAD_QUEUE_LENGTH_USERTRAFFIC = 60; diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp index cc2dc3507..cb1dec25a 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -37,6 +37,16 @@ Author: i11 - Embedded Software, RWTH Aachen University #endif using rtps::StatefulReaderT; +using rtps::GuidPrefix_t; +using rtps::Guid_t; +using rtps::PacketInfo; +using rtps::ReaderCacheChange; +using rtps::SequenceNumberSet; +using rtps::SequenceNumber_t; +using rtps::SubmessageGap; +using rtps::SubmessageHeartbeat; +using rtps::TopicData; +using rtps::WriterProxy; template StatefulReaderT::~StatefulReaderT() {} diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp index ee14095a3..9de2c76f6 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -34,6 +34,11 @@ Author: i11 - Embedded Software, RWTH Aachen University #include using rtps::StatefulWriterT; +using rtps::CacheChange; +using rtps::GuidPrefix_t; +using rtps::ReaderProxy; +using rtps::SequenceNumber_t; +using rtps::SubmessageAckNack; #if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" @@ -200,6 +205,7 @@ template void StatefulWriterT::progress() { } ++m_nextSequenceNumberToSend; + SFW_LOG("HB from progress"); sendHeartBeat(); } else { @@ -255,14 +261,18 @@ void StatefulWriterT::onNewAckNack( // Preemptive ack nack if (nextSN.low == 0 && nextSN.high == 0) { + SFW_LOG("Received preemptive acknack, sending heartbeat."); sendHeartBeat(); return; } if (m_history.isEmpty()) { - // We have never sent anything -> heartbeat + // We have never sent anything. Do not immediately respond with another + // heartbeat here, otherwise reader/writer can get stuck in HB<->ACKNACK + // ping-pong. Periodic heartbeat still handles liveliness. if (m_history.getLastUsedSequenceNumber() == rtps::SequenceNumber_t{0, 0}) { - sendHeartBeat(); + SFW_LOG("Ignoring acknack while history is empty and no samples were sent yet."); + return; } else { // No data but we have sent something in the past -> GapStart = // readerSNState.base, NextValid = lastUsedSequenceNumber+1 @@ -442,6 +452,7 @@ template void StatefulWriterT::sendHeartBeatLoop() { m_thread_running = true; while (m_running) { + SFW_LOG("HB from loop"); sendHeartBeat(); dropDisposeAfterWriteChanges(); bool unconfirmed_changes = false; @@ -538,10 +549,24 @@ void StatefulWriterT::sendHeartBeat() { // Proxy has confirmed all sequence numbers and set final flag if ((proxy.lastAckNackSequenceNumber > lastSN) && proxy.finalFlag && proxy.ackNackCount.value > 0) { + SFW_LOG("Skipping heartbeat for proxy, all changes confirmed. lastSN {}.{}, lastAckNack {}.{}", + (int)lastSN.low, + (int)lastSN.high, + (int)proxy.lastAckNackSequenceNumber.low, + (int)proxy.lastAckNackSequenceNumber.high); continue; } } else if (m_history.getLastUsedSequenceNumber() == SequenceNumber_t{0, 0}) { + if ((proxy.lastAckNackSequenceNumber > m_history.getLastUsedSequenceNumber()) && proxy.finalFlag && + proxy.ackNackCount.value > 0) { + SFW_LOG("Skipping heartbeat for proxy, all changes confirmed. lastUsedSN {}.{}, lastAckNack {}.{}", + (int)m_history.getLastUsedSequenceNumber().low, + (int)m_history.getLastUsedSequenceNumber().high, + (int)proxy.lastAckNackSequenceNumber.low, + (int)proxy.lastAckNackSequenceNumber.high); + continue; + } firstSN = SequenceNumber_t{0, 1}; lastSN = SequenceNumber_t{0, 0}; } else { diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp index c2e98cb47..7a66755e5 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp @@ -33,8 +33,10 @@ Author: i11 - Embedded Software, RWTH Aachen University #include using rtps::CacheChange; +using rtps::GuidPrefix_t; using rtps::SequenceNumber_t; using rtps::StatelessWriterT; +using rtps::SubmessageAckNack; #if SLW_VERBOSE && RTPS_GLOBAL_VERBOSE #include "rtps/utils/printutils.h" diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.h b/components/rtps_embedded/include/rtps/messages/MessageTypes.h index d6952f781..190919002 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageTypes.h +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.h @@ -32,6 +32,13 @@ Author: i11 - Embedded Software, RWTH Aachen University namespace rtps { +#if defined(_MSC_VER) +#define RTPS_EMBEDDED_PACKED +#pragma pack(push, 1) +#else +#define RTPS_EMBEDDED_PACKED __attribute__((packed)) +#endif + namespace SMElement { // TODO endianess enum ParameterId : uint16_t { @@ -117,7 +124,7 @@ struct ParameterList_t { ParameterId pid; uint16_t length; // Values follow -} __attribute__((packed)); +} RTPS_EMBEDDED_PACKED; } // namespace SMElement enum class SubmessageKind : uint8_t { @@ -439,4 +446,9 @@ void deserializeSNS(const uint8_t *&position, SequenceNumberSet &set, } // namespace rtps +#if defined(_MSC_VER) +#pragma pack(pop) +#endif +#undef RTPS_EMBEDDED_PACKED + #endif // RTPS_MESSAGES_H diff --git a/components/rtps_embedded/include/rtps/utils/Log.h b/components/rtps_embedded/include/rtps/utils/Log.h index ae52abcb1..8d0a48f10 100644 --- a/components/rtps_embedded/include/rtps/utils/Log.h +++ b/components/rtps_embedded/include/rtps/utils/Log.h @@ -30,7 +30,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #define RTPS_GLOBAL_VERBOSE 1 -#define SFW_VERBOSE 0 +#define SFW_VERBOSE 1 #define SPDP_VERBOSE 0 #define PBUF_WRAP_VERBOSE 0 #define SEDP_VERBOSE 1 diff --git a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp index 7b833cf35..60d92a014 100644 --- a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp +++ b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp @@ -58,20 +58,20 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, uint16_t length; PPD_LOG("Start deserializing ParticipantProxyData"); PPD_LOG("Buffer has {} bytes remaining", ucdr_buffer_remaining(&buffer)); - PPD_LOG("first 20 bytes of data:"); - for (int i = 0; i < 20 && i < ucdr_buffer_remaining(&buffer); ++i) { - PPD_LOG("{:02x}", buffer.iterator[i]); - } - PPD_LOG("before start, buff length: {}. last data size: {}", - ucdr_buffer_length(&buffer), buffer.last_data_size); + // PPD_LOG("first 20 bytes of data:"); + // for (int i = 0; i < 20 && i < ucdr_buffer_remaining(&buffer); ++i) { + // PPD_LOG("{:02x}", buffer.iterator[i]); + // } + // PPD_LOG("before start, buff length: {}. last data size: {}", + // ucdr_buffer_length(&buffer), buffer.last_data_size); while (ucdr_buffer_remaining(&buffer) >= 4) { ucdr_deserialize_uint16_t(&buffer, reinterpret_cast(&pid)); - PPD_LOG("buff length after get id: {}. last data size: {}", - ucdr_buffer_length(&buffer), buffer.last_data_size); + // PPD_LOG("buff length after get id: {}. last data size: {}", + // ucdr_buffer_length(&buffer), buffer.last_data_size); ucdr_deserialize_uint16_t(&buffer, &length); - PPD_LOG("buff length after get length: {}. last data size: {}", - ucdr_buffer_length(&buffer), buffer.last_data_size); + // PPD_LOG("buff length after get length: {}. last data size: {}", + // ucdr_buffer_length(&buffer), buffer.last_data_size); PPD_LOG("Deserializing parameter with id {} and length {}", static_cast(pid), length); if (ucdr_buffer_remaining(&buffer) < length) { @@ -127,6 +127,9 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, PPD_LOG("stopping deserialization early, participant is known"); return true; } + PPD_LOG("Participant GUID: {} {} {} {} {} {} {} {} {} {}", + m_guid.prefix.id[0], m_guid.prefix.id[1], m_guid.prefix.id[2], m_guid.prefix.id[3], + m_guid.prefix.id[4], m_guid.prefix.id[5], m_guid.prefix.id[6], m_guid.prefix.id[7], m_guid.prefix.id[8], m_guid.prefix.id[9]); break; } case ParameterId::PID_METATRAFFIC_MULTICAST_LOCATOR: { @@ -197,15 +200,19 @@ bool ParticipantProxyData::readFromUcdrBuffer(ucdrBuffer &buffer, // should not return false for unknown parameter, just skip it, otherwise we might miss some important information if the remote participant is using some vendor specific parameters that we do not know about. // TODO: GUO: need read out the data for the length of the parameter, otherwise the buffer will be in wrong state and the following parameters cannot be read correctly. For now just skip the data by moving the iterator forward, but we might want to actually read out the data and store it for future use if needed, especially for some vendor specific parameters that we do not know about. // buffer.iterator += length; - buffer.iterator += length; - buffer.last_data_size = 1; + // buffer.iterator += length; + // buffer.last_data_size = 1; + ucdr_advance_buffer(&buffer, length); + break; } } // Parameter lists are 4-byte aligned uint32_t alignment = ucdr_buffer_alignment(&buffer, 4); - PPD_LOG("Alignment for next parameter: {}", alignment); - buffer.iterator += alignment; - buffer.last_data_size = 4; + PPD_LOG("Alignment for next parameter: {}", alignment); + ucdr_advance_buffer(&buffer, alignment); + + // buffer.iterator += alignment; + // buffer.last_data_size = 4; } return true; } diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp index 380a9e83c..53f493266 100644 --- a/components/rtps_embedded/src/discovery/SPDPAgent.cpp +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -260,7 +260,7 @@ bool SPDPAgent::addProxiesForBuiltInEndpoints() { *locator, true}; m_buildInEndpoints.sedpSubReader->addNewMatchedWriter(proxy); - m_buildInEndpoints.sedpPubReader->sendPreemptiveAckNack(proxy); + m_buildInEndpoints.sedpSubReader->sendPreemptiveAckNack(proxy); } return true; diff --git a/pc/CMakeLists.txt b/pc/CMakeLists.txt index 494b59bc2..b30a1cc81 100644 --- a/pc/CMakeLists.txt +++ b/pc/CMakeLists.txt @@ -4,6 +4,29 @@ set(CMAKE_CXX_STANDARD 20) include(${CMAKE_CURRENT_SOURCE_DIR}/../lib/espp.cmake) +set(RTPS_EMBEDDED_COMPONENT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../components/rtps_embedded) +set(RTPS_EMBEDDED_SOURCES + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/ThreadPool.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/communication/EsppTransport.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/discovery/ParticipantProxyData.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/discovery/SEDPAgent.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/discovery/SPDPAgent.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/discovery/TopicData.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/Domain.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/Participant.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/Reader.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/StatelessReader.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/entities/Writer.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/messages/MessageReceiver.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/messages/MessageTypes.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/src/utils/Diagnostics.cpp + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/common.c + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/types/array.c + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/types/basic.c + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/types/sequence.c + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/src/c/types/string.c +) + MACRO(GEN_TESTS curdir) # get test files FILE(GLOB tests RELATIVE ${curdir} ${curdir}/tests/*.cpp) @@ -11,7 +34,7 @@ MACRO(GEN_TESTS curdir) FOREACH(test_file ${tests}) get_filename_component(TEST_NAME ${test_file} NAME_WE) project(${TEST_NAME} - LANGUAGES CXX + LANGUAGES C CXX VERSION 1.0.0 ) # settings for Windows / MSVC @@ -20,6 +43,17 @@ MACRO(GEN_TESTS curdir) add_definitions(-D_CRT_SECURE_NO_WARNINGS) endif() add_executable(${TEST_NAME} ${test_file}) + + if(TEST_NAME MATCHES "^rtps_embedded_.*") + target_sources(${TEST_NAME} PRIVATE ${RTPS_EMBEDDED_SOURCES}) + target_sources(${TEST_NAME} PRIVATE + ${curdir}/../components/thread_pool/src/thread_pool.cpp) + target_include_directories(${TEST_NAME} PRIVATE + ${curdir}/../components/thread_pool/include + ${RTPS_EMBEDDED_COMPONENT_DIR}/include + ${RTPS_EMBEDDED_COMPONENT_DIR}/thirdparty/Micro-CDR/include) + endif() + target_include_directories(${TEST_NAME} PRIVATE ${curdir}/../lib/pc/include) target_link_directories(${TEST_NAME} PRIVATE diff --git a/pc/tests/rtps_embedded_publisher.cpp b/pc/tests/rtps_embedded_publisher.cpp new file mode 100644 index 000000000..e4a37c593 --- /dev/null +++ b/pc/tests/rtps_embedded_publisher.cpp @@ -0,0 +1,80 @@ +// Standalone embeddedRTPS publisher test for host/PC. It creates an embeddedRTPS Domain, +// participant, and writer, then periodically publishes a uint32 payload. +// +// Usage: rtps_embedded_publisher [topic] [local_ipv4] [period_ms] + +#include +#include +#include +#include + +#include "espp.hpp" +#include "rtps/entities/Domain.h" +#include "rtps_common.hpp" + +namespace { + +bool parse_ipv4(const std::string &ip, rtps::Ip4AddressBytes &out) { + unsigned int b0 = 0, b1 = 0, b2 = 0, b3 = 0; + if (std::sscanf(ip.c_str(), "%u.%u.%u.%u", &b0, &b1, &b2, &b3) != 4) { + return false; + } + if (b0 > 255 || b1 > 255 || b2 > 255 || b3 > 255) { + return false; + } + out = {static_cast(b0), static_cast(b1), static_cast(b2), + static_cast(b3)}; + return true; +} + +} // namespace + +int main(int argc, char **argv) { + espp::Logger logger({.tag = "rtps_embedded_pub", .level = espp::Logger::Verbosity::INFO}); + + const std::string topic = argc > 1 ? argv[1] : "rtps_emb_pub"; + const std::string local_ip_string = argc > 2 ? argv[2] : rtps_test::guess_local_ipv4(); + const int period_ms = argc > 3 ? std::atoi(argv[3]) : 1000; + + rtps::Ip4AddressBytes local_ip{0, 0, 0, 0}; + if (!parse_ipv4(local_ip_string, local_ip)) { + logger.error("Invalid IPv4 address '{}'", local_ip_string); + return 1; + } + + rtps::Domain domain(local_ip); + rtps::Participant *participant = domain.createParticipant(); + if (participant == nullptr) { + logger.error("Failed to create embeddedRTPS participant"); + return 1; + } + + // Keep topic/type names short because embeddedRTPS desktop config caps lengths. + rtps::Writer *writer = + domain.createWriter(*participant, topic.c_str(), "UInt32", false); + if (writer == nullptr) { + logger.error("Failed to create embeddedRTPS writer for topic '{}'", topic); + return 1; + } + + if (!domain.completeInit()) { + logger.error("Failed to start embeddedRTPS domain"); + return 1; + } + + logger.info("publishing on '{}' from {} every {}ms (Ctrl-C to stop)", topic, + local_ip_string, period_ms); + + uint32_t value = 0; + while (true) { + ++value; + const rtps::CacheChange *change = + writer->newChange(rtps::ChangeKind_t::ALIVE, + reinterpret_cast(&value), + static_cast(sizeof(value))); + logger.info("publish {} -> {}", value, change != nullptr ? "queued" : "dropped"); + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + + return 0; +} \ No newline at end of file diff --git a/pc/tests/rtps_embedded_subscriber.cpp b/pc/tests/rtps_embedded_subscriber.cpp new file mode 100644 index 000000000..fdb1d75e9 --- /dev/null +++ b/pc/tests/rtps_embedded_subscriber.cpp @@ -0,0 +1,133 @@ +// Standalone embeddedRTPS subscriber test for host/PC. It creates an embeddedRTPS Domain, +// participant, and reader, then logs received std_msgs::msg::String payloads. +// +// Usage: rtps_embedded_subscriber [topic] [local_ipv4] [run_seconds] + +#include +#include +#include +#include +#include +#include + +#include "espp.hpp" +#include "rtps/entities/Domain.h" +#include "rtps_common.hpp" + +namespace { + +struct SubscriberState { + std::atomic received_count{0}; + std::string last_message; + std::mutex mutex; +}; + +bool parse_ipv4(const std::string &ip, rtps::Ip4AddressBytes &out) { + unsigned int b0 = 0, b1 = 0, b2 = 0, b3 = 0; + if (std::sscanf(ip.c_str(), "%u.%u.%u.%u", &b0, &b1, &b2, &b3) != 4) { + return false; + } + if (b0 > 255 || b1 > 255 || b2 > 255 || b3 > 255) { + return false; + } + out = {static_cast(b0), static_cast(b1), static_cast(b2), + static_cast(b3)}; + return true; +} + +void on_sample(void *arg, const rtps::ReaderCacheChange &change) { + if (arg == nullptr) { + return; + } + auto *state = static_cast(arg); + const auto size = change.getDataSize(); + if (size == 0) { + return; + } + + std::string message(size, '\0'); + if (!change.copyInto(reinterpret_cast(message.data()), + static_cast(message.size()))) { + return; + } + + if (!message.empty() && message.back() == '\0') { + message.pop_back(); + } + + { + std::lock_guard lock(state->mutex); + state->last_message = std::move(message); + } + state->received_count++; +} + +} // namespace + +int main(int argc, char **argv) { + espp::Logger logger({.tag = "rtps_embedded_sub", .level = espp::Logger::Verbosity::INFO}); + + const std::string topic = argc > 1 ? argv[1] : "rtps_emb_pub"; + const std::string local_ip_string = argc > 2 ? argv[2] : rtps_test::guess_local_ipv4(); + const int run_seconds = argc > 3 ? std::atoi(argv[3]) : 0; + + rtps::Ip4AddressBytes local_ip{0, 0, 0, 0}; + if (!parse_ipv4(local_ip_string, local_ip)) { + logger.error("Invalid IPv4 address '{}'", local_ip_string); + return 1; + } + + rtps::Domain domain(local_ip); + rtps::Participant *participant = domain.createParticipant(); + if (participant == nullptr) { + logger.error("Failed to create embeddedRTPS participant"); + return 1; + } + + // Keep topic/type names short because embeddedRTPS desktop config caps lengths. + rtps::Reader *reader = + domain.createReader(*participant, topic.c_str(), "std_msgs::msg::String", false); + if (reader == nullptr) { + logger.error("Failed to create embeddedRTPS reader for topic '{}'", topic); + return 1; + } + + SubscriberState state; + if (reader->registerCallback(on_sample, &state) == 0) { + logger.error("Failed to register reader callback"); + return 1; + } + + if (!domain.completeInit()) { + logger.error("Failed to start embeddedRTPS domain"); + return 1; + } + + logger.info("subscribing on '{}' from {} (run_seconds={} / 0=forever)", topic, + local_ip_string, run_seconds); + + const auto start_time = std::chrono::steady_clock::now(); + while (true) { + std::string last_message; + { + std::lock_guard lock(state.mutex); + last_message = state.last_message; + } + logger.info("received {} samples, last='{}'", state.received_count.load(), + last_message); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + if (run_seconds > 0) { + const auto elapsed = + std::chrono::duration_cast(std::chrono::steady_clock::now() - + start_time) + .count(); + if (elapsed >= run_seconds) { + break; + } + } + } + + domain.stop(); + return state.received_count.load() > 0 ? 0 : 1; +} \ No newline at end of file From d9524eb2c6b4e2ce4d167c19bc475bfa37db87df Mon Sep 17 00:00:00 2001 From: Guo Date: Tue, 21 Jul 2026 10:57:44 -0500 Subject: [PATCH 15/17] test with reliable and add some log for testing --- components/rtps_embedded/include/rtps/config_esp32.h | 10 +++++----- components/rtps_embedded/src/ThreadPool.cpp | 1 + .../rtps_embedded/src/communication/EsppTransport.cpp | 8 ++++---- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/components/rtps_embedded/include/rtps/config_esp32.h b/components/rtps_embedded/include/rtps/config_esp32.h index 161cc057a..5b93ca6d2 100644 --- a/components/rtps_embedded/include/rtps/config_esp32.h +++ b/components/rtps_embedded/include/rtps/config_esp32.h @@ -41,8 +41,8 @@ const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 13}; const uint8_t DOMAIN_ID = 0; // 230 possible with UDP const uint8_t NUM_STATELESS_WRITERS = 5; const uint8_t NUM_STATELESS_READERS = 5; -const uint8_t NUM_STATEFUL_READERS = 2; -const uint8_t NUM_STATEFUL_WRITERS = 2; +const uint8_t NUM_STATEFUL_READERS = 5; +const uint8_t NUM_STATEFUL_WRITERS = 5; const uint8_t MAX_NUM_PARTICIPANTS = 1; const uint8_t NUM_WRITERS_PER_PARTICIPANT = 10; const uint8_t NUM_READERS_PER_PARTICIPANT = 10; @@ -60,13 +60,13 @@ const uint8_t HISTORY_SIZE_STATEFUL = 10; const uint8_t MAX_TYPENAME_LENGTH = 64; const uint8_t MAX_TOPICNAME_LENGTH = 64; -const int HEARTBEAT_STACKSIZE = 3072; // byte +const int HEARTBEAT_STACKSIZE = 1024*6; // byte const int THREAD_POOL_WRITER_STACKSIZE = 4096; // byte -const int THREAD_POOL_READER_STACKSIZE = 4096; // byte +const int THREAD_POOL_READER_STACKSIZE = 1024*6; // byte const uint16_t SPDP_WRITER_STACKSIZE = 4096; // byte const uint16_t SF_WRITER_HB_PERIOD_MS = 4000; -const uint16_t SPDP_RESEND_PERIOD_MS = 1000; +const uint16_t SPDP_RESEND_PERIOD_MS = 2000; const uint8_t SPDP_CYCLECOUNT_HEARTBEAT = 2; // skip x SPDP rounds before checking liveliness const uint8_t SPDP_WRITER_PRIO = 5; diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp index 234d42a35..85fd59cea 100644 --- a/components/rtps_embedded/src/ThreadPool.cpp +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -288,6 +288,7 @@ void ThreadPool::doReaderWork() { auto isMetaWorkToDo = m_incomingMetaTraffic.moveFirstInto(packet_meta); if (isMetaWorkToDo) { Diagnostics::ThreadPool::processed_incoming_metatraffic++; + THREAD_POOL_LOG("ReaderWorker | Processing meta traffic with size {}", static_cast(packet_meta.payload.size())); m_receiveJumppad(m_callee, const_cast(packet_meta)); } diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps_embedded/src/communication/EsppTransport.cpp index 28874d508..20cfe23f1 100644 --- a/components/rtps_embedded/src/communication/EsppTransport.cpp +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -104,12 +104,12 @@ bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { espp::Task::BaseConfig task_config; task_config.name = "rtps_rx_" + std::to_string(receivePort); - task_config.stack_size_bytes = Config::THREAD_POOL_READER_STACKSIZE; + task_config.stack_size_bytes = Config::THREAD_POOL_READER_STACKSIZE; // todo: should use socket config, not thread pool task_config.priority = Config::THREAD_POOL_READER_PRIO; espp::UdpSocket::ReceiveConfig receive_config; receive_config.port = receivePort; - receive_config.buffer_size = 4096; + receive_config.buffer_size = 1024*8; receive_config.on_receive_callback = [this, receivePort](std::vector &data, const espp::Socket::Info &sender) @@ -132,7 +132,7 @@ EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort) { } espp::UdpSocket::Config socket_config; - socket_config.log_level = espp::Logger::Verbosity::WARN; + socket_config.log_level = espp::Logger::Verbosity::DEBUG; channel.socket = std::make_unique(socket_config); if (!channel.socket || !channel.socket->is_valid()) { logger_.error("Failed to create valid UDP socket for port {}", receivePort); @@ -169,7 +169,7 @@ void EsppTransport::onReceive(Ip4Port_t receivePort, std::vector &data, logger_.warn("Could not parse sender IPv4 address '{}', using 0.0.0.0", sender.address); } - + logger_.warn("received {} bytes on port {}", static_cast(data.size()), receivePort); m_rxCallback(m_callbackArgs, data.data(), data.size(), receivePort, static_cast(sender.port), remoteAddress); } From 9cdb1e61ecc71dd9edf3c1380e01a6ff6818af8c Mon Sep 17 00:00:00 2001 From: Guo Date: Tue, 21 Jul 2026 11:28:49 -0500 Subject: [PATCH 16/17] fix documentation and add readme --- components/rtps_embedded/README.md | 162 ++++++++++++++++++ .../rtps_embedded/example/main/main.cpp | 136 +++++++-------- .../rtps_embedded/include/rtps/ThreadPool.h | 1 + .../include/rtps/common/Locator.h | 1 + .../rtps_embedded/include/rtps/common/types.h | 1 + .../rtps/communication/EsppTransport.h | 1 + .../include/rtps/communication/PacketInfo.h | 1 + .../rtps_embedded/include/rtps/config.h | 1 + .../include/rtps/config_desktop.h | 1 + .../rtps_embedded/include/rtps/config_esp32.h | 1 + .../include/rtps/discovery/BuiltInEndpoints.h | 1 + .../rtps/discovery/ParticipantProxyData.h | 1 + .../include/rtps/discovery/SEDPAgent.h | 1 + .../include/rtps/discovery/SPDPAgent.h | 1 + .../include/rtps/discovery/TopicData.h | 1 + .../include/rtps/entities/Domain.h | 1 + .../include/rtps/entities/Participant.h | 1 + .../include/rtps/entities/Reader.h | 1 + .../include/rtps/entities/ReaderProxy.h | 1 + .../include/rtps/entities/StatefulReader.h | 1 + .../include/rtps/entities/StatefulReader.tpp | 1 + .../include/rtps/entities/StatefulWriter.h | 1 + .../include/rtps/entities/StatefulWriter.tpp | 1 + .../include/rtps/entities/StatelessReader.h | 1 + .../include/rtps/entities/StatelessWriter.h | 1 + .../include/rtps/entities/StatelessWriter.tpp | 1 + .../include/rtps/entities/Writer.h | 1 + .../include/rtps/entities/WriterProxy.h | 1 + .../include/rtps/messages/MessageFactory.h | 1 + .../include/rtps/messages/MessageReceiver.h | 1 + .../include/rtps/messages/MessageTypes.h | 1 + components/rtps_embedded/include/rtps/rtps.h | 1 + .../include/rtps/storages/CacheChange.h | 1 + .../rtps/storages/HistoryCacheWithDeletion.h | 1 + .../include/rtps/storages/MemoryPool.h | 1 + .../include/rtps/storages/PayloadBuffer.h | 1 + .../rtps/storages/SimpleHistoryCache.h | 1 + .../rtps/storages/ThreadSafeCircularBuffer.h | 1 + .../include/rtps/utils/Diagnostics.h | 1 + .../rtps_embedded/include/rtps/utils/Log.h | 1 + .../include/rtps/utils/constants.h | 1 + .../rtps_embedded/include/rtps/utils/hash.h | 1 + .../include/rtps/utils/sysFunctions.h | 1 + .../include/rtps/utils/udpUtils.h | 1 + components/rtps_embedded/src/ThreadPool.cpp | 1 + .../src/communication/EsppTransport.cpp | 7 +- .../src/discovery/ParticipantProxyData.cpp | 1 + .../rtps_embedded/src/discovery/SEDPAgent.cpp | 1 + .../rtps_embedded/src/discovery/SPDPAgent.cpp | 1 + .../rtps_embedded/src/discovery/TopicData.cpp | 1 + .../rtps_embedded/src/entities/Domain.cpp | 1 + .../src/entities/Participant.cpp | 1 + .../src/entities/StatelessReader.cpp | 1 + .../src/messages/MessageReceiver.cpp | 1 + .../src/messages/MessageTypes.cpp | 1 + 55 files changed, 283 insertions(+), 74 deletions(-) create mode 100644 components/rtps_embedded/README.md diff --git a/components/rtps_embedded/README.md b/components/rtps_embedded/README.md new file mode 100644 index 000000000..45d161076 --- /dev/null +++ b/components/rtps_embedded/README.md @@ -0,0 +1,162 @@ +# rtps_embedded + +ESPP component that integrates the [embeddedRTPS](https://github.com/embedded-software-laboratory/embeddedRTPS) +RTPS/DDS stack into the ESPP ecosystem. +Any platform that can build ESPP — including ESP32, Linux, and desktop PCs — +can use this component to discover and exchange typed messages with ROS 2 nodes +or any other DDS participant on the same network using the standard RTPS wire +protocol. + +The original embeddedRTPS library has hard dependencies on FreeRTOS and lwIP. +`rtps_embedded` removes those dependencies by replacing all socket, task, and +synchronisation calls with ESPP's platform-agnostic `UdpSocket`, `Task`, and +`ThreadPool` primitives. When built for ESP32, ESPP uses FreeRTOS and lwIP +under the hood; on other platforms it uses the host OS equivalents — the RTPS +code itself is unchanged in either case. + +--- + +## Architecture + +``` +user code + │ + ▼ +rtps::Domain — routes packets to participants; owns discovery threads + │ + ├── rtps::Participant — groups writers and readers + │ ├── rtps::Writer — publishes CacheChange samples + │ └── rtps::Reader — delivers samples to a user callback + │ + ├── rtps::ThreadPool — espp::ThreadPool workers that drain the four + │ incoming/outgoing meta/user traffic queues + │ + └── rtps::EsppTransport — one espp::UdpSocket per open UDP port, + each with its own receive task +``` + +`EsppTransport` is the sole platform-specific adapter. It wraps ESPP's +`UdpSocket` and `Task`, which in turn map to: + +| Build target | Socket backend | Task backend | +|---|---|---| +| ESP32 | lwIP (via ESP-IDF) | FreeRTOS | +| Linux / PC | POSIX sockets | `std::thread` | + +--- + +## Quick-start + +```cpp +#include "rtps/entities/Domain.h" + +// 1. Construct the domain with the local interface IP. +rtps::Domain domain(local_ip); + +// 2. Create a participant *before* completeInit(). +rtps::Participant *part = domain.createParticipant(); + +// 3. Add user-defined writer and reader endpoints. +rtps::Writer *writer = domain.createWriter(*part, "my/topic", + "std_msgs::msg::String", false); +rtps::Reader *reader = domain.createReader(*part, "my/topic", + "std_msgs::msg::String", false); + +// 4. Register a receive callback on the reader. +reader->registerCallback( + [](void *, const rtps::ReaderCacheChange &change) { + // process change.getData() / change.copyInto(...) + }, nullptr); + +// 5. Start discovery (SPDP/SEDP) and worker threads. +domain.completeInit(); + +// 6. Publish a sample. +const char *payload = "hello"; +writer->newChange(rtps::ChangeKind_t::ALIVE, + reinterpret_cast(payload), + static_cast(strlen(payload) + 1)); +``` + +> **Note**: `createParticipant()` **must** be called before `completeInit()`. +> No new participants can be added after init is complete. + +--- + +## Configuration + +Two built-in config headers are provided. Select one by defining +`RTPS_CONFIG_HEADER`, or let `include/rtps/config.h` pick automatically based +on the build target. + +| Header | Target | +|---|---| +| [`include/rtps/config_esp32.h`](include/rtps/config_esp32.h) | ESP32 (ESP-IDF) | +| [`include/rtps/config_desktop.h`](include/rtps/config_desktop.h) | Linux / PC | + +All tunable constants follow the same layout in both files: + +| Constant | Default | Description | +|---|---|---| +| `DOMAIN_ID` | 0 | RTPS domain number (0–230 with UDP) | +| `MAX_NUM_PARTICIPANTS` | 1 | Participant pool size | +| `NUM_STATEFUL_WRITERS` | 5 | User writer endpoint pool | +| `NUM_STATEFUL_READERS` | 5 | User reader endpoint pool | +| `NUM_STATELESS_WRITERS` | 5 | Discovery writer endpoint pool | +| `NUM_STATELESS_READERS` | 5 | Discovery reader endpoint pool | +| `NUM_WRITERS_PER_PARTICIPANT` | 10 | Max writers per participant | +| `NUM_READERS_PER_PARTICIPANT` | 10 | Max readers per participant | +| `HISTORY_SIZE_STATEFUL` | 10 | Per-endpoint history depth | +| `THREAD_POOL_NUM_WRITERS` | 2 | Writer worker threads | +| `THREAD_POOL_NUM_READERS` | 2 | Reader worker threads | +| `THREAD_POOL_WRITER_STACKSIZE` | 4096 B | Writer task stack | +| `THREAD_POOL_READER_STACKSIZE` | 6144 B | Reader / UDP-receive task stack | +| `MAX_NUM_UDP_CONNECTIONS` | 10 | UDP socket pool size | +| `SPDP_RESEND_PERIOD_MS` | 2000 | Discovery announce period | +| `SF_WRITER_HB_PERIOD_MS` | 4000 | Reliable-writer heartbeat period | + +The `OVERALL_HEAP_SIZE` constant at the bottom of that file estimates the +total stack RAM consumed by all internal tasks. + +--- + +## ESPP component dependencies + +| Component | Purpose | +|---|---| +| `base_component` | ESPP base class with integrated `espp::Logger` | +| `socket` | ESPP `UdpSocket` used by `EsppTransport` | +| `task` | ESPP `Task` for per-port UDP receive loops | +| `thread_pool` | ESPP `ThreadPool` for writer/reader workers | +| `cdr` | CDR serialization helpers | + +These components abstract away all OS and network-stack details, so +`rtps_embedded` itself has no direct dependency on FreeRTOS, lwIP, or any +other platform library. + +Third-party (vendored in `thirdparty/`): + +| Library | Purpose | +|---|---| +| `Micro-CDR` | eProsima CDR (de)serialization for discovery messages | + +--- + +## Example + +See [`example/`](example/) for a two-node **initiator / responder** demo. + +The same logic runs on any ESPP-supported platform. For ESP32, flash one board +as *Initiator* and a second as *Responder* via menuconfig +(`idf.py menuconfig → RTPS Example Configuration`). The initiator periodically +publishes numbered request messages; the responder echoes each message back on +the response topic. + +Key menuconfig options (ESP32 example): + +| Option | Description | +|---|---| +| `RTPS_EXAMPLE_ROLE` | `Initiator` or `Responder` | +| `RTPS_EXAMPLE_TOPIC_PREFIX` | Shared topic prefix (e.g. `espp/rtps_example`) | +| `RTPS_EXAMPLE_PUBLISH_PERIOD_MS` | Initiator publish interval | +| `ESP_WIFI_SSID` / `ESP_WIFI_PASSWORD` | Wi-Fi credentials | diff --git a/components/rtps_embedded/example/main/main.cpp b/components/rtps_embedded/example/main/main.cpp index 939d5dfd7..4b0ebe748 100644 --- a/components/rtps_embedded/example/main/main.cpp +++ b/components/rtps_embedded/example/main/main.cpp @@ -1,6 +1,7 @@ #include #include +#include #include "esp_log.h" #include "freertos/FreeRTOS.h" @@ -14,23 +15,12 @@ using namespace std::chrono_literals; namespace { -static const char *TAG = "embedded_rtps"; +static const char *TAG = "rtps_example"; static bool s_started = false; static rtps::Domain *s_domain = nullptr; static rtps::Participant *s_participant = nullptr; static rtps::Writer *s_writer = nullptr; static rtps::Reader *s_reader = nullptr; -static char s_node_name[16] = "node"; - -bool is_dhcps_node() { - return (strcmp(s_node_name, "DHCPS") == 0 || - strcmp(s_node_name, "dhcps") == 0); -} - -bool is_dhcpc_node() { - return (strcmp(s_node_name, "DHCPC") == 0 || - strcmp(s_node_name, "dhcpc") == 0); -} bool send_text_message(const char *text) { if (text == nullptr || s_writer == nullptr) { @@ -45,91 +35,87 @@ bool send_text_message(const char *text) { return change != nullptr; } -void reader_cb(void *callee, const rtps::ReaderCacheChange &change) { - (void)callee; +void reader_cb(void * /*callee*/, const rtps::ReaderCacheChange &change) { char buffer[128] = {0}; const rtps::DataSize_t copy_len = - (change.getDataSize() < sizeof(buffer) - 1) ? change.getDataSize() - : (sizeof(buffer) - 1); + (change.getDataSize() < static_cast(sizeof(buffer) - 1)) + ? change.getDataSize() + : static_cast(sizeof(buffer) - 1); - if (copy_len == 0) { + if (copy_len == 0 || + !change.copyInto(reinterpret_cast(buffer), sizeof(buffer))) { return; } - if (!change.copyInto(reinterpret_cast(buffer), sizeof(buffer))) { - return; - } - - ESP_LOGI(TAG, "RTPS rx (%u B): %s", static_cast(copy_len), buffer); + ESP_LOGI(TAG, "rx (%u B): %s", static_cast(copy_len), buffer); - if (strcmp(buffer, "who are you? Are you PC?") == 0 && is_dhcpc_node()) { - if (send_text_message("i am dhcpc")) { - ESP_LOGI(TAG, "RTPS tx: i am dhcpc"); - } else { - ESP_LOGW(TAG, "RTPS tx dropped: i am dhcpc"); - } - } else if (is_dhcps_node()) { - ESP_LOGI(TAG, "RTPS Rx: %s", buffer); +#if CONFIG_RTPS_EXAMPLE_ROLE_RESPONDER + // Echo the message back on the publish topic. + if (!send_text_message(buffer)) { + ESP_LOGW(TAG, "tx echo dropped (history full or no matched reader)"); } +#endif } -void publisher_task(void *arg) { - (void)arg; +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR +void publisher_task(void * /*arg*/) { + uint32_t counter = 0; while (true) { - if (is_dhcps_node()) { - if (!send_text_message("who are you? Are you PC?")) { - ESP_LOGW(TAG, "RTPS tx dropped (history full or no matched reader)"); - } else { - ESP_LOGI(TAG, "RTPS tx: who are you? Are you PC?"); - } + char msg[32]; + snprintf(msg, sizeof(msg), "request %u", static_cast(counter++)); + if (!send_text_message(msg)) { + ESP_LOGW(TAG, "tx dropped (history full or no matched reader)"); + } else { + ESP_LOGI(TAG, "tx: %s", msg); } - - vTaskDelay(pdMS_TO_TICKS(2000)); + vTaskDelay(pdMS_TO_TICKS(CONFIG_RTPS_EXAMPLE_PUBLISH_PERIOD_MS)); } } +#endif } // namespace -extern "C" void embedded_rtps_start(const char *node_name, - const char *pub_topic, - const char *sub_topic, - const rtps::Ip4AddressBytes &local_ip) { +// Start the RTPS stack. Topics are derived from the configured prefix: +// initiator publishes on /request, subscribes to /response +// responder publishes on /response, subscribes to /request +extern "C" void embedded_rtps_start(const rtps::Ip4AddressBytes &local_ip) { if (s_started) { return; } - if (node_name == nullptr || pub_topic == nullptr || sub_topic == nullptr) { - ESP_LOGE(TAG, "RTPS start rejected due to invalid arguments"); - return; - } - - strncpy(s_node_name, node_name, sizeof(s_node_name) - 1); - s_node_name[sizeof(s_node_name) - 1] = '\0'; + const std::string prefix = CONFIG_RTPS_EXAMPLE_TOPIC_PREFIX; +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR + const std::string pub_topic = prefix + "/request"; + const std::string sub_topic = prefix + "/response"; +#else + const std::string pub_topic = prefix + "/response"; + const std::string sub_topic = prefix + "/request"; +#endif static rtps::Domain domain(local_ip); s_domain = &domain; + // Participants must be created before completeInit() starts the discovery + // threads; no new participants can be added after that point. s_participant = s_domain->createParticipant(); if (s_participant == nullptr) { ESP_LOGE(TAG, "Failed to create RTPS participant"); return; } - // Complete domain init first so built-in discovery endpoints and worker - // threads are fully initialized before user endpoints are added. if (!s_domain->completeInit()) { ESP_LOGE(TAG, "Failed to complete RTPS domain init"); return; } - s_writer = s_domain->createWriter(*s_participant, pub_topic, + s_writer = s_domain->createWriter(*s_participant, pub_topic.c_str(), "std_msgs::msg::String", false); if (s_writer == nullptr) { ESP_LOGE(TAG, "Failed to create RTPS writer"); return; } - s_reader = s_domain->createReader(*s_participant, sub_topic, + s_reader = s_domain->createReader(*s_participant, sub_topic.c_str(), "std_msgs::msg::String", false); if (s_reader == nullptr) { ESP_LOGE(TAG, "Failed to create RTPS reader"); @@ -141,27 +127,32 @@ extern "C" void embedded_rtps_start(const char *node_name, return; } +#if CONFIG_RTPS_EXAMPLE_ROLE_INITIATOR xTaskCreate(publisher_task, "rtps_pub", 4096, nullptr, 5, nullptr); +#endif + s_started = true; - ESP_LOGI(TAG, "EmbeddedRTPS started: pub=%s sub=%s", pub_topic, sub_topic); + ESP_LOGI(TAG, "started as '%s': pub=%s sub=%s", + CONFIG_RTPS_EXAMPLE_NODE_NAME, pub_topic.c_str(), sub_topic.c_str()); } -extern "C" void app_main(void) -{ - espp::Logger logger({.tag = "rtps_example", .level = espp::Logger::Verbosity::INFO}); +extern "C" void app_main(void) { + espp::Logger logger({.tag = TAG, .level = espp::Logger::Verbosity::INFO}); //! [rtps example] std::string ip_address; - espp::WifiSta wifi_sta({.ssid = CONFIG_ESP_WIFI_SSID, - .password = CONFIG_ESP_WIFI_PASSWORD, - .num_connect_retries = CONFIG_ESP_MAXIMUM_RETRY, - .on_connected = nullptr, - .on_disconnected = nullptr, - .on_got_ip = [&ip_address](ip_event_got_ip_t *eventdata) { - ip_address = fmt::format("{}.{}.{}.{}", IP2STR(&eventdata->ip_info.ip)); - fmt::print("got IP: {}\n", ip_address); - }}); + espp::WifiSta wifi_sta({ + .ssid = CONFIG_ESP_WIFI_SSID, + .password = CONFIG_ESP_WIFI_PASSWORD, + .num_connect_retries = CONFIG_ESP_MAXIMUM_RETRY, + .on_connected = nullptr, + .on_disconnected = nullptr, + .on_got_ip = [&ip_address](ip_event_got_ip_t *eventdata) { + ip_address = fmt::format("{}.{}.{}.{}", IP2STR(&eventdata->ip_info.ip)); + fmt::print("got IP: {}\n", ip_address); + }, + }); logger.info("Waiting for WiFi connection..."); while (!wifi_sta.is_connected()) { @@ -170,14 +161,15 @@ extern "C" void app_main(void) logger.info("WiFi connected, local IP {}", ip_address); rtps::Ip4AddressBytes local_ip{0, 0, 0, 0}; - if (std::sscanf(ip_address.c_str(), "%hhu.%hhu.%hhu.%hhu", &local_ip[0], - &local_ip[1], &local_ip[2], &local_ip[3]) != 4) { + if (std::sscanf(ip_address.c_str(), "%hhu.%hhu.%hhu.%hhu", + &local_ip[0], &local_ip[1], &local_ip[2], &local_ip[3]) != 4) { logger.error("Failed to parse local IP {}", ip_address); return; } - embedded_rtps_start("DHCPS", "rtps_embedded_pub", "rtps_embedded_sub", - local_ip); + embedded_rtps_start(local_ip); + //! [rtps example] + while (true) { vTaskDelay(pdMS_TO_TICKS(1000)); } diff --git a/components/rtps_embedded/include/rtps/ThreadPool.h b/components/rtps_embedded/include/rtps/ThreadPool.h index d936eaf95..7d8d09d6b 100644 --- a/components/rtps_embedded/include/rtps/ThreadPool.h +++ b/components/rtps_embedded/include/rtps/ThreadPool.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/common/Locator.h b/components/rtps_embedded/include/rtps/common/Locator.h index e18714f5c..fc1bdc36f 100644 --- a/components/rtps_embedded/include/rtps/common/Locator.h +++ b/components/rtps_embedded/include/rtps/common/Locator.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/common/types.h b/components/rtps_embedded/include/rtps/common/types.h index 069f56aba..f6488c79a 100644 --- a/components/rtps_embedded/include/rtps/common/types.h +++ b/components/rtps_embedded/include/rtps/common/types.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/communication/EsppTransport.h b/components/rtps_embedded/include/rtps/communication/EsppTransport.h index c8bca8993..c56628e5b 100644 --- a/components/rtps_embedded/include/rtps/communication/EsppTransport.h +++ b/components/rtps_embedded/include/rtps/communication/EsppTransport.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/communication/PacketInfo.h b/components/rtps_embedded/include/rtps/communication/PacketInfo.h index 4c5006299..b679c17fe 100644 --- a/components/rtps_embedded/include/rtps/communication/PacketInfo.h +++ b/components/rtps_embedded/include/rtps/communication/PacketInfo.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/config.h b/components/rtps_embedded/include/rtps/config.h index 43642c4a9..eac533a8c 100644 --- a/components/rtps_embedded/include/rtps/config.h +++ b/components/rtps_embedded/include/rtps/config.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/config_desktop.h b/components/rtps_embedded/include/rtps/config_desktop.h index f7b8f5e21..999b1c959 100644 --- a/components/rtps_embedded/include/rtps/config_desktop.h +++ b/components/rtps_embedded/include/rtps/config_desktop.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/config_esp32.h b/components/rtps_embedded/include/rtps/config_esp32.h index 5b93ca6d2..b20a23a70 100644 --- a/components/rtps_embedded/include/rtps/config_esp32.h +++ b/components/rtps_embedded/include/rtps/config_esp32.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h index eb3a32c86..922fc01a8 100644 --- a/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h +++ b/components/rtps_embedded/include/rtps/discovery/BuiltInEndpoints.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h index 1f27697ea..95b8cda44 100644 --- a/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h +++ b/components/rtps_embedded/include/rtps/discovery/ParticipantProxyData.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h index 0b2618c8b..d1f6b711f 100644 --- a/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h +++ b/components/rtps_embedded/include/rtps/discovery/SEDPAgent.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h index b75028642..501df7b44 100644 --- a/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h +++ b/components/rtps_embedded/include/rtps/discovery/SPDPAgent.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/discovery/TopicData.h b/components/rtps_embedded/include/rtps/discovery/TopicData.h index 24c6a2c1b..8071f108d 100644 --- a/components/rtps_embedded/include/rtps/discovery/TopicData.h +++ b/components/rtps_embedded/include/rtps/discovery/TopicData.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/Domain.h b/components/rtps_embedded/include/rtps/entities/Domain.h index 93bf0bbcd..fcfdd45fd 100644 --- a/components/rtps_embedded/include/rtps/entities/Domain.h +++ b/components/rtps_embedded/include/rtps/entities/Domain.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/Participant.h b/components/rtps_embedded/include/rtps/entities/Participant.h index 5427aa8c4..8cc36a1b0 100644 --- a/components/rtps_embedded/include/rtps/entities/Participant.h +++ b/components/rtps_embedded/include/rtps/entities/Participant.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/Reader.h b/components/rtps_embedded/include/rtps/entities/Reader.h index 0716f5dcd..33bd4d603 100644 --- a/components/rtps_embedded/include/rtps/entities/Reader.h +++ b/components/rtps_embedded/include/rtps/entities/Reader.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/ReaderProxy.h b/components/rtps_embedded/include/rtps/entities/ReaderProxy.h index fb6860836..8a49de732 100644 --- a/components/rtps_embedded/include/rtps/entities/ReaderProxy.h +++ b/components/rtps_embedded/include/rtps/entities/ReaderProxy.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.h b/components/rtps_embedded/include/rtps/entities/StatefulReader.h index 9d324a0cd..4edbac8e3 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.h +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp index cb1dec25a..3e46dc06c 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulReader.tpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h index 8b0337a38..a5ff6b204 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.h +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp index 9de2c76f6..fe370107b 100644 --- a/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatefulWriter.tpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/StatelessReader.h b/components/rtps_embedded/include/rtps/entities/StatelessReader.h index cb2ad7a40..6ab9759cf 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessReader.h +++ b/components/rtps_embedded/include/rtps/entities/StatelessReader.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h index 8ad690c4d..2cf07e692 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.h +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp index 7a66755e5..0f883d5ad 100644 --- a/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp +++ b/components/rtps_embedded/include/rtps/entities/StatelessWriter.tpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/Writer.h b/components/rtps_embedded/include/rtps/entities/Writer.h index 37afa3f21..2df11f834 100644 --- a/components/rtps_embedded/include/rtps/entities/Writer.h +++ b/components/rtps_embedded/include/rtps/entities/Writer.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/entities/WriterProxy.h b/components/rtps_embedded/include/rtps/entities/WriterProxy.h index 4db9e529d..9d7bbf249 100644 --- a/components/rtps_embedded/include/rtps/entities/WriterProxy.h +++ b/components/rtps_embedded/include/rtps/entities/WriterProxy.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/messages/MessageFactory.h b/components/rtps_embedded/include/rtps/messages/MessageFactory.h index 711bf8c98..4bb45e3b3 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageFactory.h +++ b/components/rtps_embedded/include/rtps/messages/MessageFactory.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/messages/MessageReceiver.h b/components/rtps_embedded/include/rtps/messages/MessageReceiver.h index be821e404..ca39ca54d 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageReceiver.h +++ b/components/rtps_embedded/include/rtps/messages/MessageReceiver.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/messages/MessageTypes.h b/components/rtps_embedded/include/rtps/messages/MessageTypes.h index 190919002..208f5d144 100644 --- a/components/rtps_embedded/include/rtps/messages/MessageTypes.h +++ b/components/rtps_embedded/include/rtps/messages/MessageTypes.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/rtps.h b/components/rtps_embedded/include/rtps/rtps.h index 09f31d559..4415a3377 100644 --- a/components/rtps_embedded/include/rtps/rtps.h +++ b/components/rtps_embedded/include/rtps/rtps.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/storages/CacheChange.h b/components/rtps_embedded/include/rtps/storages/CacheChange.h index a6ae4482b..a053b6d1e 100644 --- a/components/rtps_embedded/include/rtps/storages/CacheChange.h +++ b/components/rtps_embedded/include/rtps/storages/CacheChange.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h index cb1c382f4..4dc213004 100644 --- a/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h +++ b/components/rtps_embedded/include/rtps/storages/HistoryCacheWithDeletion.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/storages/MemoryPool.h b/components/rtps_embedded/include/rtps/storages/MemoryPool.h index c4bbc2c23..cb8161062 100644 --- a/components/rtps_embedded/include/rtps/storages/MemoryPool.h +++ b/components/rtps_embedded/include/rtps/storages/MemoryPool.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/storages/PayloadBuffer.h b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.h index b0abdbb0e..f2b3de123 100644 --- a/components/rtps_embedded/include/rtps/storages/PayloadBuffer.h +++ b/components/rtps_embedded/include/rtps/storages/PayloadBuffer.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h index ba6715eb3..666966c2b 100644 --- a/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h +++ b/components/rtps_embedded/include/rtps/storages/SimpleHistoryCache.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h index 47f1e2fd2..c3921c13e 100644 --- a/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h +++ b/components/rtps_embedded/include/rtps/storages/ThreadSafeCircularBuffer.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/utils/Diagnostics.h b/components/rtps_embedded/include/rtps/utils/Diagnostics.h index fbc4aad95..1cc9e9550 100644 --- a/components/rtps_embedded/include/rtps/utils/Diagnostics.h +++ b/components/rtps_embedded/include/rtps/utils/Diagnostics.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/utils/Log.h b/components/rtps_embedded/include/rtps/utils/Log.h index 8d0a48f10..dc0858f5f 100644 --- a/components/rtps_embedded/include/rtps/utils/Log.h +++ b/components/rtps_embedded/include/rtps/utils/Log.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/utils/constants.h b/components/rtps_embedded/include/rtps/utils/constants.h index 2f557a880..58b5c7c74 100644 --- a/components/rtps_embedded/include/rtps/utils/constants.h +++ b/components/rtps_embedded/include/rtps/utils/constants.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/utils/hash.h b/components/rtps_embedded/include/rtps/utils/hash.h index e1f79b805..2cd6ae5e6 100644 --- a/components/rtps_embedded/include/rtps/utils/hash.h +++ b/components/rtps_embedded/include/rtps/utils/hash.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/utils/sysFunctions.h b/components/rtps_embedded/include/rtps/utils/sysFunctions.h index b7175eca2..028568e6b 100644 --- a/components/rtps_embedded/include/rtps/utils/sysFunctions.h +++ b/components/rtps_embedded/include/rtps/utils/sysFunctions.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/include/rtps/utils/udpUtils.h b/components/rtps_embedded/include/rtps/utils/udpUtils.h index 7368c0c07..f32ba16a9 100644 --- a/components/rtps_embedded/include/rtps/utils/udpUtils.h +++ b/components/rtps_embedded/include/rtps/utils/udpUtils.h @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/ThreadPool.cpp b/components/rtps_embedded/src/ThreadPool.cpp index 85fd59cea..4f4fefe7d 100644 --- a/components/rtps_embedded/src/ThreadPool.cpp +++ b/components/rtps_embedded/src/ThreadPool.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/communication/EsppTransport.cpp b/components/rtps_embedded/src/communication/EsppTransport.cpp index 20cfe23f1..0f45e677d 100644 --- a/components/rtps_embedded/src/communication/EsppTransport.cpp +++ b/components/rtps_embedded/src/communication/EsppTransport.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 @@ -104,7 +105,9 @@ bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { espp::Task::BaseConfig task_config; task_config.name = "rtps_rx_" + std::to_string(receivePort); - task_config.stack_size_bytes = Config::THREAD_POOL_READER_STACKSIZE; // todo: should use socket config, not thread pool + // Reuse the reader-worker stack size for the UDP receive task. Both tasks + // perform similar deserialization work, so the value is a reasonable bound. + task_config.stack_size_bytes = Config::THREAD_POOL_READER_STACKSIZE; task_config.priority = Config::THREAD_POOL_READER_PRIO; espp::UdpSocket::ReceiveConfig receive_config; @@ -169,7 +172,7 @@ void EsppTransport::onReceive(Ip4Port_t receivePort, std::vector &data, logger_.warn("Could not parse sender IPv4 address '{}', using 0.0.0.0", sender.address); } - logger_.warn("received {} bytes on port {}", static_cast(data.size()), receivePort); + logger_.debug("received {} bytes on port {}", static_cast(data.size()), receivePort); m_rxCallback(m_callbackArgs, data.data(), data.size(), receivePort, static_cast(sender.port), remoteAddress); } diff --git a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp index 60d92a014..407898b67 100644 --- a/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp +++ b/components/rtps_embedded/src/discovery/ParticipantProxyData.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/discovery/SEDPAgent.cpp b/components/rtps_embedded/src/discovery/SEDPAgent.cpp index 07c0a26ad..73c9578a5 100644 --- a/components/rtps_embedded/src/discovery/SEDPAgent.cpp +++ b/components/rtps_embedded/src/discovery/SEDPAgent.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/discovery/SPDPAgent.cpp b/components/rtps_embedded/src/discovery/SPDPAgent.cpp index 53f493266..108eb5789 100644 --- a/components/rtps_embedded/src/discovery/SPDPAgent.cpp +++ b/components/rtps_embedded/src/discovery/SPDPAgent.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/discovery/TopicData.cpp b/components/rtps_embedded/src/discovery/TopicData.cpp index 5f55702b9..bb65097af 100644 --- a/components/rtps_embedded/src/discovery/TopicData.cpp +++ b/components/rtps_embedded/src/discovery/TopicData.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/entities/Domain.cpp b/components/rtps_embedded/src/entities/Domain.cpp index 641849b44..72f5b6044 100644 --- a/components/rtps_embedded/src/entities/Domain.cpp +++ b/components/rtps_embedded/src/entities/Domain.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/entities/Participant.cpp b/components/rtps_embedded/src/entities/Participant.cpp index 80e2f9462..7e62f973c 100644 --- a/components/rtps_embedded/src/entities/Participant.cpp +++ b/components/rtps_embedded/src/entities/Participant.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/entities/StatelessReader.cpp b/components/rtps_embedded/src/entities/StatelessReader.cpp index 394640b78..f2ec862e4 100644 --- a/components/rtps_embedded/src/entities/StatelessReader.cpp +++ b/components/rtps_embedded/src/entities/StatelessReader.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/messages/MessageReceiver.cpp b/components/rtps_embedded/src/messages/MessageReceiver.cpp index ddaffb471..994927513 100644 --- a/components/rtps_embedded/src/messages/MessageReceiver.cpp +++ b/components/rtps_embedded/src/messages/MessageReceiver.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 diff --git a/components/rtps_embedded/src/messages/MessageTypes.cpp b/components/rtps_embedded/src/messages/MessageTypes.cpp index 5301f03fc..0a9bfb174 100644 --- a/components/rtps_embedded/src/messages/MessageTypes.cpp +++ b/components/rtps_embedded/src/messages/MessageTypes.cpp @@ -1,6 +1,7 @@ /* The MIT License Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University +Modifications Copyright (c) 2026 ATDev 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 From ca544048634a9717f97c3dacf3de0de83e3d05dd Mon Sep 17 00:00:00 2001 From: Guo Date: Tue, 21 Jul 2026 11:36:11 -0500 Subject: [PATCH 17/17] disable logging by default --- components/rtps_embedded/include/rtps/utils/Log.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/rtps_embedded/include/rtps/utils/Log.h b/components/rtps_embedded/include/rtps/utils/Log.h index dc0858f5f..23525c4fe 100644 --- a/components/rtps_embedded/include/rtps/utils/Log.h +++ b/components/rtps_embedded/include/rtps/utils/Log.h @@ -29,7 +29,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include #include -#define RTPS_GLOBAL_VERBOSE 1 +#define RTPS_GLOBAL_VERBOSE 0 #define SFW_VERBOSE 1 #define SPDP_VERBOSE 0