Improved new thing processing

Arduino/ArduinoParticipant.cpp

@@ -88,7 +88,7 @@ void ParticipantUDP::Receive() {
 #endif
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
   // << remoteParticipant->port << "\n";

Arduino/ArduinoParticipant.h

@@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
   void Setup();
   void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:

EspIdf/EspIdfParticipant.cpp

@@ -130,7 +130,7 @@ void ParticipantUDP::Receive() {
 #endif  // IDF_VER
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) {
 #if defined(IDF_VER)
   // std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
   //           << remoteParticipant->port << "\n";

EspIdf/EspIdfParticipant.h

@@ -13,7 +13,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:

Participant.cpp

@@ -49,10 +49,10 @@ Participant::~Participant() {
   delete[] this->ipAddress;
 }
 
-void Participant::Update() {
+void Participant::Update(bool recurse) {
   for (Thing* thing : this->things) {
     if (thing != nullptr)
-      thing->Update(true);
+      thing->Update();
   }
 }
 
@@ -67,26 +67,26 @@ bool Participant::Send(IMessage* msg) {
 #if defined(_WIN32) || defined(_WIN64)
   Windows::ParticipantUDP* thisWindows =
       static_cast<Windows::ParticipantUDP*>(this);
-  return thisWindows->Send(this, bufferSize);
+  return thisWindows->SendTo(this, bufferSize);
 #elif defined(__unix__) || defined(__APPLE__)
   Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  return thisPosix->Send(this, bufferSize);
+  return thisPosix->SendTo(this, bufferSize);
 #elif defined(ARDUINO)
   Arduino::ParticipantUDP* thisArduino =
       static_cast<Arduino::ParticipantUDP*>(this);
-  return thisArduino->Send(this, bufferSize);
+  return thisArduino->SendTo(this, bufferSize);
 #elif defined(IDF_VER)
   EspIdf::ParticipantUDP* thisEspIdf =
       static_cast<EspIdf::ParticipantUDP*>(this);
-  return thisEspIdf->Send(remoteParticipant, bufferSize);
+  return thisEspIdf->SendTo(this, bufferSize);
 #else
   return false;
 #endif
 }
 
-Thing* Participant::Get(unsigned char thingId) {
+Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
   for (Thing* thing : this->things) {
-    if (thing->id == thingId)
+    if (thing->owner->networkId == networkId && thing->id == thingId)
       return thing;
   }
   std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
@@ -115,7 +115,7 @@ void Participant::Add(Thing* thing, bool checkId) {
     // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
     //           << this->port << "[" << (int)thing->id << "]\n";
   } else {
-    Thing* foundThing = Get(thing->id);
+    Thing* foundThing = Get(thing->owner->networkId, thing->id);
     if (foundThing == nullptr) {
 #if defined(NO_STD)
       this->things[this->thingCount++] = thing;

Participant.h

@@ -112,9 +112,10 @@ class Participant {
   std::list<Thing*> things;
 #endif
   /// @brief Find a thing managed by this participant
+  /// @param networkId The network ID of the thing
   /// @param thingId The ID of the thing
   /// @return The thing if found, nullptr when no thing has been found
-  Thing* Get(unsigned char thingId);
+  Thing* Get(unsigned char networkId, unsigned char thingId);
   /// @brief Add a new thing for this participant.
   /// @param thing The thing to add
   /// @param checkId If true, the thing.id is regenerated if it is zero
@@ -129,7 +130,7 @@ class Participant {
 
  public:
   /// @brief Update all things for this participant
-  virtual void Update();
+  virtual void Update(bool recurse = true);
 
 #pragma endregion Update
 

Participants/ParticipantUDP.cpp

@@ -8,6 +8,10 @@
 #include "Posix/PosixParticipant.h"
 #include "Windows/WindowsParticipant.h"
 
+#include "Things/DistanceSensor.h"
+#include "Things/TouchSensor.h"
+#include "Things/DifferentialDrive.h"
+
 #include <string.h>
 
 namespace RoboidControl {
@@ -83,7 +87,7 @@ void ParticipantUDP::SetupUDP(int localPort,
 // 1. receive external messages
 // 2. update the state
 // 3. send out the updated messages
-void ParticipantUDP::Update() {
+void ParticipantUDP::Update(bool recurse) {
   unsigned long currentTimeMs = Thing::GetTimeMs();
 
   if (this->isIsolated == false) {
@@ -182,7 +186,7 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
 bool ParticipantUDP::Send(IMessage* msg) {
   if (this->remoteSite != nullptr)
     return this->remoteSite->Send(msg);
-    
+
   return true;
 }
 
@@ -379,6 +383,43 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
             << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
             << (int)msg->parentId << "\n";
 #endif
+  Participant* owner = Participant::registry.Get(msg->networkId);
+  if (owner == nullptr) {
+    owner = new Participant();
+    owner->networkId = msg->networkId;
+    Participant::registry.Add(owner);
+  }
+
+  Thing* thing = owner->Get(msg->networkId, msg->thingId);
+  if (thing == nullptr) {
+    bool isRemote = (sender->networkId != owner->networkId);
+    thing = ProcessNewThing(owner, msg, isRemote);
+    thing->id = msg->thingId;
+    thing->type = msg->thingType;
+    thing->isRemote = isRemote;
+  }
+
+  if (msg->parentId != 0) {
+    thing->SetParent(owner->Get(msg->networkId, msg->parentId));
+    if (thing->GetParent() == nullptr)
+      std::cout << "Could not find parent" << std::endl;
+  } else
+    thing->SetParent(nullptr);
+}
+
+Thing* ParticipantUDP::ProcessNewThing(Participant* owner,
+                                       ThingMsg* msg,
+                                       bool isRemote) {
+  switch (msg->thingType) {
+    case Thing::Type::DistanceSensor:
+      return new DistanceSensor(owner->root);
+    case Thing::Type::TouchSensor:
+      return new TouchSensor(owner->root);
+    case Thing::Type::DifferentialDrive:
+      return new DifferentialDrive(owner->root);
+    default:
+      return new Thing(owner->root);
+  }
 }
 
 void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
@@ -387,7 +428,7 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
             << (int)msg->thingId << "] ";
 #endif
 
-  Thing* thing = sender->Get(msg->thingId);
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing != nullptr) {
     int nameLength = msg->nameLength;
     int stringLen = nameLength + 1;
@@ -429,7 +470,7 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
   if (owner == nullptr)
     return;
 
-  Thing* thing = owner->Get(msg->thingId);
+  Thing* thing = owner->Get(msg->networkId, msg->thingId);
   if (thing == nullptr)
     return;
 
@@ -451,7 +492,7 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
 
   Participant* owner = Participant::registry.Get(msg->networkId);
   if (owner != nullptr) {
-    Thing* thing = owner->Get(msg->thingId);
+    Thing* thing = owner->Get(msg->networkId, msg->thingId);
     if (thing != nullptr)
       thing->ProcessBinary(msg->data);
 #if !defined(NO_STD)
@@ -478,7 +519,7 @@ void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) {
             << (int)msg->thingId << "]\n";
 #endif
 
-  Thing* thing = sender->Get(msg->thingId);
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing != nullptr)
     this->Remove(thing);
 }

Participants/ParticipantUDP.h

@@ -96,7 +96,7 @@ public:
 #pragma region Update
 
  public:
-  virtual void Update() override;
+  virtual void Update(bool recurse = true) override;
 
  protected:
   unsigned long nextPublishMe = 0;
@@ -133,7 +133,10 @@ protected:
   virtual void Process(Participant* sender, ParticipantMsg* msg);
   virtual void Process(Participant* sender, NetworkIdMsg* msg);
   virtual void Process(Participant* sender, InvestigateMsg* msg);
+
   virtual void Process(Participant* sender, ThingMsg* msg);
+  virtual Thing* ProcessNewThing(Participant* sender, ThingMsg* msg, bool isRemote);
+
   virtual void Process(Participant* sender, NameMsg* msg);
   virtual void Process(Participant* sender, ModelUrlMsg* msg);
   virtual void Process(Participant* sender, PoseMsg* msg);

Participants/SiteServer.cpp

@@ -70,16 +70,16 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
 void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
 
 void SiteServer::Process(Participant* sender, ThingMsg* msg) {
-  Thing* thing = sender->Get(msg->thingId);
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
-  if (thing == nullptr)
+  if (thing == nullptr) {
     // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
     // Thing::Reconstruct(sender, msg->thingType, msg->thingId);
     //thing = new Thing(msg->thingType, sender->root);
-    ;
+  }
   thing->id = msg->thingId;
 
   if (msg->parentId != 0) {
-    thing->SetParent(Get(msg->parentId));
+    thing->SetParent(Get(msg->networkId, msg->parentId));
     if (thing->IsRoot())
     // if (thing->GetParent() != nullptr)
 #if defined(NO_STD)

Posix/PosixParticipant.cpp

@@ -90,7 +90,7 @@ void ParticipantUDP::Receive() {
 #endif
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) {
 #if defined(__unix__) || defined(__APPLE__)
   // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
   //           << "\n";

Posix/PosixParticipant.h

@@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:

Thing.h

@@ -88,6 +88,11 @@ class Thing {
   /// This can be either a Thing::Type of a byte value for custom types
   unsigned char type = Type::Undetermined;
 
+  /// @brief Is this a remote thing?
+  /// A remote thing is owned by other participant
+  /// and is not simulated by the local participant
+  bool isRemote = false;
+
   /// @brief The participant owning this thing
   Participant* owner = nullptr;
 

Windows/WindowsParticipant.cpp

@@ -102,7 +102,7 @@ void ParticipantUDP::Receive() {
 #endif // _WIN32 || _WIN64
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) {
 #if defined(_WIN32) || defined(_WIN64)
   char ip_str[INET_ADDRSTRLEN];
   inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);

Windows/WindowsParticipant.h

@@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected: