Log framework override

include/rtps/discovery/SPDPAgent.h

@@ -34,13 +34,15 @@ Author: i11 - Embedded Software, RWTH Aachen University
 #include "ucdr/microcdr.h"
 
 #if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE
-#include "rtps/utils/printutils.h"
+#ifndef SPDP_LOG
+#include "rtps/utils/strutils.h"
 #define SPDP_LOG(...)                                                          \
   if (true) {                                                                  \
     printf("[SPDP] ");                                                         \
     printf(__VA_ARGS__);                                                       \
     printf("\r\n");                                                              \
   }
+#endif
 #else
 #define SPDP_LOG(...) //
 #endif

include/rtps/entities/StatefulReader.tpp

@@ -31,13 +31,15 @@ Author: i11 - Embedded Software, RWTH Aachen University
 #include "rtps/utils/Log.h"
 
 #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE
-#include "rtps/utils/printutils.h"
+#include "rtps/utils/strutils.h"
+#ifndef SFR_LOG
 #define SFR_LOG(...)                                                           \
   if (true) {                                                                  \
     printf("[StatefulReader %s] ", &m_attributes.topicName[0]);                \
     printf(__VA_ARGS__);                                                       \
     printf("\r\n");                                                            \
   }
+#endif
 #else
 #define SFR_LOG(...) //
 #endif
@@ -102,9 +104,9 @@ template <class NetworkDriver>
 bool StatefulReaderT<NetworkDriver>::addNewMatchedWriter(
     const WriterProxy &newProxy) {
 #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE
-  SFR_LOG("New writer added with id: ");
-  printGuid(newProxy.remoteWriterGuid);
-  SFR_LOG("\n");
+  char buffer[64];
+  guid2Str(newProxy.remoteWriterGuid, buffer, sizeof(buffer));
+  SFR_LOG("New writer added with id: %s", buffer);
 #endif
   return m_proxies.add(newProxy);
 }
@@ -124,12 +126,11 @@ bool StatefulReaderT<NetworkDriver>::onNewGapMessage(
   WriterProxy *writer = getProxy(writerProxyGuid);
 
   if (writer == nullptr) {
-
 #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE
+    char buffer[64];
+    entityId2Str(msg.writerId, buffer, sizeof(buffer));
     SFR_LOG("Ignore GAP. Couldn't find a matching "
-            "writer with id:");
-    printEntityId(msg.writerId);
-    SFR_LOG("\n");
+            "writer with id: %s", buffer);
 #endif
     return false;
   }
@@ -203,6 +204,7 @@ bool StatefulReaderT<NetworkDriver>::onNewGapMessage(
 	  }
 
 
+    return false;
   }
 }
 
@@ -222,12 +224,11 @@ bool StatefulReaderT<NetworkDriver>::onNewHeartbeat(
   WriterProxy *writer = getProxy(writerProxyGuid);
 
   if (writer == nullptr) {
-
 #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE
+    char buffer[64];
+    entityId2Str(msg.writerId, buffer, sizeof(buffer));
     SFR_LOG("Ignore heartbeat. Couldn't find a matching "
-            "writer with id:");
-    printEntityId(msg.writerId);
-    SFR_LOG("\n");
+            "writer with id: %s", buffer);
 #endif
     return false;
   }

include/rtps/entities/StatefulWriter.tpp

@@ -33,13 +33,15 @@ Author: i11 - Embedded Software, RWTH Aachen University
 using rtps::StatefulWriterT;
 
 #if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE
-#include "rtps/utils/printutils.h"
+#include "rtps/utils/strutils.h"
+#ifndef SFW_LOG
 #define SFW_LOG(...)                                                           \
   if (true) {                                                                  \
     printf("[Stateful Writer %s] ", this->m_attributes.topicName);             \
     printf(__VA_ARGS__);                                                       \
     printf("\r\n");                                                            \
   }
+#endif
 #else
 #define SFW_LOG(...) //
 #endif
@@ -239,9 +241,9 @@ void StatefulWriterT<NetworkDriver>::onNewAckNack(
 
   if (reader == nullptr) {
 #if SFW_VERBOSE && RTPS_GLOBAL_VERBOSE
-    SFW_LOG("No proxy found with id: ");
-    printEntityId(msg.readerId);
-    SFW_LOG(" Dropping acknack.\n");
+    char buffer[64];
+    entityId2Str(msg.readerId, buffer, sizeof(buffer));
+    SFW_LOG("No proxy found with id: %s Dropping acknack.", buffer);
 #endif
     return;
   }

include/rtps/entities/StatelessWriter.tpp

@@ -39,13 +39,15 @@ using rtps::SequenceNumber_t;
 using rtps::StatelessWriterT;
 
 #if SLW_VERBOSE && RTPS_GLOBAL_VERBOSE
-#include "rtps/utils/printutils.h"
+#ifndef SLW_LOG
+#include "rtps/utils/strutils.h"
 #define SLW_LOG(...)                                                           \
   if (true) {                                                                  \
     printf("[StatelessWriter %s] ", &this->m_attributes.topicName[0]);         \
     printf(__VA_ARGS__);                                                       \
     printf("\n");                                                              \
   }
+#endif
 #else
 #define SLW_LOG(...) //
 #endif

include/rtps/storages/ThreadSafeCircularBuffer.tpp

@@ -6,12 +6,14 @@
 #include "rtps/utils/Log.h"
 
 #if TSCB_VERBOSE && RTPS_GLOBAL_VERBOSE
+#ifndef TSCB_LOG
 #define TSCB_LOG(...)                                                          \
   if (true) {                                                                  \
     printf("[TSCircularBuffer] ");                                             \
     printf(__VA_ARGS__);                                                       \
     printf("\n");                                                              \
   }
+#endif
 #else
 #define TSCB_LOG(...) //
 #endif

include/rtps/utils/Log.h

@@ -44,4 +44,18 @@ Author: i11 - Embedded Software, RWTH Aachen University
 #define SLR_VERBOSE 0
 #define THREAD_POOL_VERBOSE 0
 
+#undef SFW_LOG
+#undef SPDP_LOG
+#undef PBUF_LOG
+#undef SEDP_LOG
+#undef RECV_LOG
+#undef PARTICIPANT_LOG
+#undef DOMAIN_LOG
+#undef UDP_DRIVER_LOG
+#undef TSCB_LOG
+#undef SLW_LOG
+#undef SFR_LOG
+#undef SLR_LOG
+#undef THREAD_POOL_LOG
+
 #endif // RTPS_LOG_H

include/rtps/utils/strutils.h

@@ -0,0 +1,40 @@
+//
+// Created by andreas on 13.01.19.
+//
+
+#ifndef RTPS_STRUTILS_H
+#define RTPS_STRUTILS_H
+
+#include "rtps/common/types.h"
+
+inline int entityId2Str(rtps::EntityId_t id, char* str, size_t size) {
+  int bytes = 0;
+  for (const auto byte : id.entityKey) {
+    bytes += snprintf(&str[bytes], size - bytes, "%x", (int)byte);
+  }
+  bytes +=
+    snprintf(
+      &str[bytes],
+      size - bytes,
+      "%x",
+      static_cast<uint8_t>(id.entityKind)
+  );
+  return bytes;
+}
+
+inline int guidPrefix2Str(rtps::GuidPrefix_t prefix, char* str, size_t size) {
+  int bytes = 0;
+  for (const auto byte : prefix.id) {
+    bytes +=  snprintf(&str[bytes], size - bytes, "%x", (int)byte);
+  }
+  return bytes;
+}
+
+inline int guid2Str(rtps::Guid_t guid, char* str, size_t size) {
+  int bytes = guidPrefix2Str(guid.prefix, str, size);
+  bytes += snprintf(&str[bytes], size - bytes, ":");
+  bytes += entityId2Str(guid.entityId, &str[bytes], size - bytes);
+  return bytes;
+}
+
+#endif // RTPS_STRUTILS_H

src/ThreadPool.cpp

@@ -34,13 +34,15 @@ Author: i11 - Embedded Software, RWTH Aachen University
 using rtps::ThreadPool;
 
 #if THREAD_POOL_VERBOSE && RTPS_GLOBAL_VERBOSE
-#include "rtps/utils/printutils.h"
+#ifndef THREAD_POOL_LOG
+#include "rtps/utils/strutils.h"
 #define THREAD_POOL_LOG(...)                                                   \
   if (true) {                                                                  \
     printf("[ThreadPool] ");                                                   \
     printf(__VA_ARGS__);                                                       \
     printf("\r\n");                                                            \
   }
+#endif
 #else
 #define THREAD_POOL_LOG(...) //
 #endif

src/communication/UdpDriver.cpp

@@ -30,16 +30,22 @@ Author: i11 - Embedded Software, RWTH Aachen University
 #include <lwip/igmp.h>
 #include <lwip/tcpip.h>
 
+#if PLATFORM_ESP32
+#include <string.h>
+#endif
+
 using rtps::UdpDriver;
 
 #if UDP_DRIVER_VERBOSE && RTPS_GLOBAL_VERBOSE
-#include "rtps/utils/printutils.h"
+#ifndef UDP_DRIVER_LOG
+#include "rtps/utils/strutils.h"
 #define UDP_DRIVER_LOG(...)                                                    \
   if (true) {                                                                  \
     printf("[UDP Driver] ");                                                   \
     printf(__VA_ARGS__);                                                       \
     printf("\r\n");                                                            \
   }
+#endif
 #else
 #define UDP_DRIVER_LOG(...) //
 #endif
@@ -83,8 +89,13 @@ UdpDriver::createUdpConnection(Ip4Port_t receivePort) {
 }
 
 bool UdpDriver::isSameSubnet(ip4_addr_t addr) {
+#if PLATFORM_ESP32
+  return (ip4_addr_netcmp(&addr, &(netif_default->ip_addr.u_addr.ip4),
+                          &(netif_default->netmask.u_addr.ip4)) != 0);
+#else
   return (ip4_addr_netcmp(&addr, &(netif_default->ip_addr),
                           &(netif_default->netmask)) != 0);
+#endif
 }
 
 bool UdpDriver::isMulticastAddress(ip4_addr_t addr) {
@@ -100,19 +111,37 @@ bool UdpDriver::joinMultiCastGroup(ip4_addr_t addr) const {
 
   {
     TcpipCoreLock lock;
+#if PLATFORM_ESP32
+    iret = igmp_joingroup(ip_2_ip4(IP_ADDR_ANY), (&addr));
+#else
     iret = igmp_joingroup(IP_ADDR_ANY, (&addr));
+#endif
   }
 
   if (iret != ERR_OK) {
-
+#if PLATFORM_ESP32
+    ip_addr_t tmpAddr;
+    memcpy((char *)&tmpAddr.u_addr.ip4, (char *)&addr.addr, sizeof(ip4_addr));
+    tmpAddr.type = IPADDR_TYPE_V4;
+    UDP_DRIVER_LOG("Failed to join IGMP multicast group %s\n",
+                   ipaddr_ntoa(&tmpAddr));
+#else
     UDP_DRIVER_LOG("Failed to join IGMP multicast group %s\n",
                    ipaddr_ntoa(&addr));
+#endif
 
     return false;
   } else {
-
+#if PLATFORM_ESP32
+    ip_addr_t tmpAddr;
+    memcpy((char *)&tmpAddr.u_addr.ip4, (char *)&addr.addr, sizeof(ip4_addr));
+    tmpAddr.type = IPADDR_TYPE_V4;
+    UDP_DRIVER_LOG("Succesfully joined  IGMP multicast group %s\n",
+                   ipaddr_ntoa(&tmpAddr));
+#else
     UDP_DRIVER_LOG("Succesfully joined  IGMP multicast group %s\n",
                    ipaddr_ntoa(&addr));
+#endif
   }
   return true;
 }
@@ -122,15 +151,27 @@ bool UdpDriver::sendPacket(const UdpConnection &conn, ip4_addr_t &destAddr,
   err_t err;
   {
     TcpipCoreLock lock;
+#if PLATFORM_ESP32
+    ip_addr_t tmpAddr;
+    memcpy((char *)&tmpAddr.u_addr.ip4, (char *)&destAddr.addr, sizeof(ip4_addr));
+    tmpAddr.type = IPADDR_TYPE_V4;
+    err = udp_sendto(conn.pcb, &buffer, &tmpAddr, destPort);
+#else
     err = udp_sendto(conn.pcb, &buffer, &destAddr, destPort);
+#endif
   }
 
   if (err != ERR_OK) {
-    ;
-
+#if PLATFORM_ESP32
+    ip_addr_t tmpAddr;
+    memcpy((char *)&tmpAddr.u_addr.ip4, (char *)&destAddr.addr, sizeof(ip4_addr));
+    tmpAddr.type = IPADDR_TYPE_V4;
+    UDP_DRIVER_LOG("UDP TRANSMIT NOT SUCCESSFUL %s:%u size: %u err: %i\n",
+                   ipaddr_ntoa(&tmpAddr), destPort, buffer.tot_len, err);
+#else
     UDP_DRIVER_LOG("UDP TRANSMIT NOT SUCCESSFUL %s:%u size: %u err: %i\n",
                    ipaddr_ntoa(&destAddr), destPort, buffer.tot_len, err);
-
+#endif
     return false;
   }
   return true;

src/discovery/SEDPAgent.cpp

@@ -34,12 +34,14 @@ Author: i11 - Embedded Software, RWTH Aachen University
 using rtps::SEDPAgent;
 
 #if SEDP_VERBOSE && RTPS_GLOBAL_VERBOSE
+#ifndef SEDP_LOG
 #define SEDP_LOG(...)                                                          \
   if (true) {                                                                  \
     printf("[SEDP] ");                                                         \
     printf(__VA_ARGS__);                                                       \
     printf("\r\n");                                                            \
   }
+#endif
 #else
 #define SEDP_LOG(...) //
 #endif

src/discovery/SPDPAgent.cpp

@@ -161,8 +161,9 @@ void SPDPAgent::processProxyData() {
     addProxiesForBuiltInEndpoints();
     m_buildInEndpoints.spdpWriter->setAllChangesToUnsent();
 #if SPDP_VERBOSE && RTPS_GLOBAL_VERBOSE
-    SPDP_LOG("Added new participant with guid: ");
-    printGuidPrefix(m_proxyDataBuffer.m_guid.prefix);
+    char buffer[64];
+    guidPrefix2Str(m_proxyDataBuffer.m_guid.prefix, buffer, sizeof(buffer));
+    SPDP_LOG("Added new participant with guid: %s", buffer);
   } else {
     SPDP_LOG("Failed to add new participant");
   }