Merge remote-tracking branch 'LSTS/fix/udp_filter'

Author: Luís Venâncio

src/Transports/UDP/ContactTable.hpp

@@ -61,18 +61,34 @@ namespace Transports
           list.push_back(itr->second);
       }
 
-      void
-      update(unsigned id, const Address& addr)
+      bool
+      addContact(unsigned id, const Address& addr)
       {
         Table::iterator itr = m_table.find(id);
 
-        if (itr == m_table.end())
+        if (itr != m_table.end())
+          return false;
+
+        std::pair<Table::iterator, bool> rv = m_table.insert(Entry(id, Contact(id, addr)));
+        if (rv.second)
         {
-          std::pair<Table::iterator, bool> rv = m_table.insert(Entry(id, Contact(id, addr)));
           itr = rv.first;
-          itr->second.setTimeout(m_tout);
+          // Set invalid timeout.
+          itr->second.setTimeout(-1);
+          return true;
         }
 
+        return false;
+      }
+
+      bool
+      update(unsigned id, const Address& addr)
+      {
+        Table::iterator itr = m_table.find(id);
+
+        if (itr == m_table.end())
+          return false;
+
         // Address has changed... update it.
         if (itr->second.getAddress() != addr)
         {
@@ -81,6 +97,8 @@ namespace Transports
         }
 
         itr->second.update();
+
+        return true;
       }
 
     private:

src/Transports/UDP/Listener.hpp

@@ -51,10 +51,11 @@ namespace Transports
     {
     public:
       Listener(Tasks::Task& task, UDPSocket& sock, LimitedComms* lcomms,
-               float contact_timeout, bool trace = false):
+               float contact_timeout, bool force = false, bool trace = false):
         m_task(task),
         m_sock(sock),
         m_trace(trace),
+        force_send(force),
         m_contacts(contact_timeout),
         m_lcomms(lcomms)
       { }
@@ -83,6 +84,14 @@ namespace Transports
         m_contacts_lock.unlock();
       }
 
+      void
+      addContact(unsigned id)
+      {
+        m_contacts_lock.lockWrite();
+        m_contacts.addContact(id, Address::Any);
+        m_contacts_lock.unlock();
+      }
+
     private:
       // Buffer capacity.
       static const int c_bfr_size = 65535;
@@ -94,6 +103,8 @@ namespace Transports
       UDPSocket& m_sock;
       // True to print incoming messages.
       bool m_trace;
+      // Flag to force sending messages.
+      bool force_send;
       // Table of contacts.
       ContactTable m_contacts;
       // Lock to serialize access to m_contacts.
@@ -133,10 +144,13 @@ namespace Transports
             }
 
             m_contacts_lock.lockWrite();
-            m_contacts.update(msg->getSource(), addr);
+            bool onTable = m_contacts.update(msg->getSource(), addr);
             m_contacts_lock.unlock();
 
-            m_task.dispatch(msg, DF_KEEP_TIME | DF_KEEP_SRC_EID);
+            if (force_send)
+              m_task.dispatch(msg, DF_KEEP_TIME | DF_KEEP_SRC_EID);
+            else if (onTable)
+              m_task.dispatch(msg, DF_KEEP_TIME | DF_KEEP_SRC_EID);
 
             if (m_trace)
               DUNE_MSG(m_task.getName(), "incoming: " + std::string(msg->getName()));

src/Transports/UDP/Task.cpp

@@ -84,6 +84,8 @@ namespace Transports
       bool only_local;
       // Optional custom service type
       std::string custom_service;
+      // Ignore the UDP source message filter
+      bool ign_filter;
     };
 
     // Internal buffer size.
@@ -184,6 +186,10 @@ namespace Transports
         .defaultValue("")
         .description("Optional custom service type (imc+udp+<Custom Service Type>), empty entry gives default service (imc+udp)");
 
+        param("Ignore Filter", m_args.ign_filter)
+        .defaultValue("false")
+        .description("Ignore the UDP source message filter.");
+
         // Allocate space for internal buffer.
         m_bfr = new uint8_t[c_bfr_size];
 
@@ -302,7 +308,7 @@ namespace Transports
 
         // Start listener thread.
         m_listener = new Listener(*this, m_sock, m_lcomms,
-                                  m_args.contact_timeout, m_args.trace_in);
+                                  m_args.contact_timeout, m_args.ign_filter, m_args.trace_in);
         m_listener->start();
 
         setEntityState(IMC::EntityState::ESTA_NORMAL, Status::CODE_ACTIVE);
@@ -384,7 +390,12 @@ namespace Transports
             return;
         }
 
+        // Check if the message is from this system.
+        if (msg->getSource() == getSystemId())
+          return;
+
         m_node_table.addNode(msg->getSource(), msg->sys_name, msg->services);
+        m_listener->addContact(msg->getSource());
         m_lcomms->setAnnounce(msg);
       }