Improved network sync

Arduino/Things/DRV8833.cpp

@@ -9,11 +9,13 @@ namespace Arduino {
 uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
 #endif
 
-DRV8833Motor::DRV8833Motor(Participant* participant,
+DRV8833Motor::DRV8833Motor(DRV8833* driver,
                            unsigned char pinIn1,
                            unsigned char pinIn2,
                            bool reverse)
-    : Thing(participant) {
+    : Thing(driver->owner) {
+  this->parent = driver;
+  
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
 
@@ -120,11 +122,30 @@ DRV8833::DRV8833(Participant* participant,
   if (pinStandby != 255)
     pinMode(pinStandby, OUTPUT);
 
-  this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA);
+  this->motorA = new DRV8833Motor(this, pinAIn1, pinAIn2, reverseA);
   this->motorA->name = "Motor A";
-  this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB);
+  this->motorB = new DRV8833Motor(this, pinBIn1, pinBIn2, reverseB);
   this->motorB->name = "Motor B";
 }
 
+DRV8833::DRV8833(Thing* parent,
+                 unsigned char pinAIn1,
+                 unsigned char pinAIn2,
+                 unsigned char pinBIn1,
+                 unsigned char pinBIn2,
+                 unsigned char pinStandby,
+                 bool reverseA,
+                 bool reverseB)
+    : DRV8833(parent->owner,
+              pinAIn1,
+              pinAIn2,
+              pinBIn1,
+              pinBIn2,
+              pinStandby,
+              reverseA,
+              reverseB) {
+  this->parent = parent;
+}
+
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DRV8833.h

@@ -7,6 +7,42 @@
 namespace RoboidControl {
 namespace Arduino {
 
+class DRV8833Motor;
+
+class DRV8833 : public Thing {
+ public:
+  /// @brief Setup a DRV8833 motor controller
+  /// @param pinAIn1 The pin number connected to the AIn1 port
+  /// @param pinAIn2 The pin number connected to the AIn2 port
+  /// @param pinBIn1 The pin number connected to the BIn1 port
+  /// @param pinBIn2 The pin number connceted to the BIn2 port
+  /// @param pinStandby The pin number connected to the standby port, 255
+  /// indicated that the port is not connected
+  /// @param reverseA The forward turning direction of motor A
+  /// @param reverseB The forward turning direction of motor B
+  DRV8833(Participant* participant,
+          unsigned char pinAIn1,
+          unsigned char pinAIn2,
+          unsigned char pinBIn1,
+          unsigned char pinBIn2,
+          unsigned char pinStandby = 255,
+          bool reverseA = false,
+          bool reverseB = false);
+  DRV8833(Thing* parent,
+          unsigned char pinAIn1,
+          unsigned char pinAIn2,
+          unsigned char pinBIn1,
+          unsigned char pinBIn2,
+          unsigned char pinStandby = 255,
+          bool reverseA = false,
+          bool reverseB = false);
+  DRV8833Motor* motorA = nullptr;
+  DRV8833Motor* motorB = nullptr;
+
+ protected:
+  unsigned char pinStandby = 255;
+};
+
 /// @brief Support for a DRV8833 motor controller
 class DRV8833Motor : public Thing {
  public:
@@ -17,7 +53,7 @@ class DRV8833Motor : public Thing {
   /// @param pinIn1 the pin number for the in1 signal
   /// @param pinIn2 the pin number for the in2 signal
   /// @param direction the forward turning direction of the motor
-  DRV8833Motor(Participant* participant,
+  DRV8833Motor(DRV8833* driver,
                unsigned char pinIn1,
                unsigned char pinIn2,
                bool reverse = false);
@@ -39,32 +75,5 @@ class DRV8833Motor : public Thing {
 #endif
 };
 
-class DRV8833 : public Thing {
- public:
-  /// @brief Setup a DRV8833 motor controller
-  /// @param pinAIn1 The pin number connected to the AIn1 port
-  /// @param pinAIn2 The pin number connected to the AIn2 port
-  /// @param pinBIn1 The pin number connected to the BIn1 port
-  /// @param pinBIn2 The pin number connceted to the BIn2 port
-  /// @param pinStandby The pin number connected to the standby port, 255
-  /// indicated that the port is not connected
-  /// @param reverseA The forward turning direction of motor A
-  /// @param reverseB The forward turning direction of motor B
-  DRV8833(Participant* participant,
-          unsigned char pinAIn1,
-          unsigned char pinAIn2,
-          unsigned char pinBIn1,
-          unsigned char pinBIn2,
-          unsigned char pinStandby = 255,
-          bool reverseA = false,
-          bool reverseB = false);
-
-  DRV8833Motor* motorA = nullptr;
-  DRV8833Motor* motorB = nullptr;
-
- protected:
-  unsigned char pinStandby = 255;
-};
-
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/UltrasonicSensor.cpp

@@ -16,6 +16,13 @@ UltrasonicSensor::UltrasonicSensor(Participant* participant,
   pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
 }
 
+UltrasonicSensor::UltrasonicSensor(Thing* parent,
+                                   unsigned char pinTrigger,
+                                   unsigned char pinEcho)
+    : UltrasonicSensor(parent->owner, pinTrigger, pinEcho) {
+  this->parent = parent;
+}
+
 float UltrasonicSensor::GetDistance() {
   // Start the ultrasonic 'ping'
   digitalWrite(pinTrigger, LOW);
@@ -44,10 +51,10 @@ float UltrasonicSensor::GetDistance() {
   // if (distance > 30)
   //   distance = 0;
 
-  this->touchedSomething |= (this->distance <= this->touchDistance);
+  this->touchedSomething |= (this->distance > 0 && this->distance <= this->touchDistance);
 
-  //   std::cout << "Ultrasonic " << this->distance << " " <<
-  //   this->touchedSomething << "\n";
+  // std::cout << "Ultrasonic " << this->touchedSomething << " |  " << (this->distance > 0) << " " <<
+  // (this->distance <= this->touchDistance) << "\n";
 
   return distance;
 }

Arduino/Things/UltrasonicSensor.h

@@ -8,6 +8,9 @@ namespace Arduino {
 /// @brief An HC-SR04 ultrasonic distance sensor
 class UltrasonicSensor : public TouchSensor {
  public:
+  // Inherit all constructors
+  //using TouchSensor::TouchSensor;
+
   /// @brief  Setup an ultrasonic sensor
   /// @param participant The participant to use
   /// @param pinTrigger The pin number of the trigger signal
@@ -15,6 +18,9 @@ class UltrasonicSensor : public TouchSensor {
   UltrasonicSensor(Participant* participant,
                    unsigned char pinTrigger,
                    unsigned char pinEcho);
+  UltrasonicSensor(Thing* parent,
+                   unsigned char pinTrigger,
+                   unsigned char pinEcho);
 
   // parameters
 

Messages/DestroyMsg.cpp

@@ -2,20 +2,23 @@
 
 namespace RoboidControl {
 
-DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
+DestroyMsg::DestroyMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
 }
 
-DestroyMsg::DestroyMsg(char* buffer) {}
+DestroyMsg::DestroyMsg(char* buffer) {
+  this->networkId = buffer[1];
+  this->thingId = buffer[2];
+}
 
 DestroyMsg::~DestroyMsg() {}
 
-unsigned char DestroyMsg::Serialize(char *buffer) {
-  #if defined(DEBUG)
-  std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId
-            << "] " << std::endl;
-#endif
+unsigned char DestroyMsg::Serialize(char* buffer) {
+//#if defined(DEBUG)
+  std::cout << "Send DestroyMsg [" << (int)this->networkId << "/"
+            << (int)this->thingId << "] " << std::endl;
+//#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;
@@ -23,4 +26,4 @@ unsigned char DestroyMsg::Serialize(char *buffer) {
   return ix;
 }
 
-} // namespace RoboidControl
+}  // namespace RoboidControl

Messages/PoseMsg.cpp

@@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
   this->thingId = thing->id;
 
   this->poseType = 0;
-  if (thing->positionUpdated || force) {
+  if (thing->positionUpdated || (force && thing->GetLinearVelocity().distance == 0)) {
     this->position = thing->GetPosition();
     this->poseType |= Pose_Position;
   }
-  if (thing->orientationUpdated || force) {
+  if (thing->orientationUpdated || (force && thing->GetAngularVelocity().distance == 0)) {
     this->orientation = thing->GetOrientation();
     this->poseType |= Pose_Orientation;
   }
@@ -45,7 +45,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
   if (this->poseType == 0)
     return 0;
 
-#if defined(DEBUG)
+#if defined(DEBUG) && DEBUG > 1
   std::cout << "Send PoseMsg [" << (int)this->networkId << "/"
             << (int)this->thingId << "] " << (int)this->poseType << std::endl;
 #endif

Participant.cpp

@@ -4,6 +4,8 @@
 
 namespace RoboidControl {
 
+std::list<Participant*> Participant::participants;
+
 Participant::Participant() {}
 
 Participant::Participant(const char* ipAddress, int port) {
@@ -34,12 +36,63 @@ void Participant::Update(unsigned long currentTimeMs) {
   }
 }
 
+Participant* Participant::GetParticipant(const char* ipAddress,
+                                         unsigned int port) {
+  for (Participant* participant : Participant::participants) {
+    if (participant == nullptr)
+      continue;
+    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
+        participant->port == port)
+      return participant;
+  }
+  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
+            << std::endl;
+  return nullptr;
+}
+
+Participant* Participant::GetParticipant(unsigned char participantId) {
+  for (Participant* participant : Participant::participants) {
+    if (participant == nullptr)
+      continue;
+    if (participant->networkId == participantId)
+      return participant;
+  }
+  std::cout << "Could not find participant " << (int)participantId << std::endl;
+  return nullptr;
+}
+
+Participant* Participant::AddParticipant(const char* ipAddress, unsigned int port) {
+  Participant* participant = new Participant(ipAddress, port);
+  Participant::AddParticipant(participant);
+  return participant;
+}
+
+void Participant::AddParticipant(Participant* participant) {
+  Participant* foundParticipant =
+      Participant::GetParticipant(participant->ipAddress, participant->port);
+  if (foundParticipant == nullptr) {
+#if defined(NO_STD)
+    this->things[this->thingCount++] = thing;
+#else
+    Participant::participants.push_back(participant);
+#endif
+    std::cout << "Add participant " << participant->ipAddress << ":"
+              << participant->port << "[" << (int)participant->networkId
+              << "]\n";
+  } else {
+    std::cout << "Did not add, existing participant " << participant->ipAddress << ":"
+              << participant->port << "[" << (int)participant->networkId
+              << "]\n";
+  }
+}
+
 Thing* Participant::Get(unsigned char thingId) {
   for (Thing* thing : this->things) {
     if (thing->id == thingId)
       return thing;
   }
-  // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
+  // std::cout << "Could not find thing " << this->ipAddress << ":" <<
+  // this->port
   //           << "[" << (int)thingId << "]\n";
   return nullptr;
 }
@@ -64,7 +117,8 @@ void Participant::Add(Thing* thing, bool checkId) {
 #else
       this->things.push_back(thing);
 #endif
-      // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "["
+      // std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
+      // "["
       //           << (int)thing->id << "]\n";
     } else {
       // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
@@ -89,8 +143,8 @@ void Participant::Remove(Thing* thing) {
   this->thingCount = lastThingIx;
 #else
   this->things.remove_if([thing](Thing* obj) { return obj == thing; });
-  std::cout << "Removing " << thing->networkId << "/" << thing->id
-            << " list size = " << this->things.size() << "\n";
+  std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id
+            << "] list size = " << this->things.size() << "\n";
 #endif
 }
 

Participant.h

@@ -18,7 +18,7 @@ class Participant {
   const char* ipAddress = "0.0.0.0";
   /// @brief The port number for UDP communication with the participant. This is
   /// 0 for isolated participants.
-  int port = 0;
+  unsigned int port = 0;
 
   /// @brief The network Id to identify the participant.
   /// @note This field is likely to disappear in future versions
@@ -35,16 +35,24 @@ class Participant {
 
   virtual void Update(unsigned long currentTimeMs = 0);
 
- protected:
+ public:
 #if defined(NO_STD)
   unsigned char thingCount = 0;
   Thing* things[MAX_THING_COUNT];
 #else
+  /// @brief The list of known participants
+  static std::list<Participant*> participants;
+
   /// @brief The list of things managed by this participant
   std::list<Thing*> things;
 #endif
 
  public:
+  static Participant* GetParticipant(const char* ipAddress, unsigned int port);
+  static Participant* GetParticipant(unsigned char participantId);
+  static Participant* AddParticipant(const char* ipAddress, unsigned int port);
+  static void AddParticipant(Participant* participant);
+
   /// @brief Find a thing managed by this participant
   /// @param networkId The network ID for the thing
   /// @param thingId The ID of the thing

Participants/ParticipantUDP.cpp

@@ -5,6 +5,7 @@
 #include "Arduino/ArduinoParticipant.h"
 #include "EspIdf/EspIdfParticipant.h"
 
+
 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
 #include <ws2tcpip.h>
@@ -31,6 +32,7 @@ ParticipantUDP::ParticipantUDP(int port) {
   this->remoteSite = nullptr;
   if (this->port == 0)
     this->isIsolated = true;
+  Participant::AddParticipant(this);
 }
 
 ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
@@ -39,6 +41,7 @@ ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
     this->isIsolated = true;
   else
     this->remoteSite = new Participant(ipAddress, port);
+  Participant::AddParticipant(this);
 }
 
 static ParticipantUDP* isolatedParticipant = nullptr;
@@ -100,18 +103,53 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
     this->ReceiveUDP();
   }
 
+  UpdateMyThings(currentTimeMs);
+  UpdateOtherThings(currentTimeMs);
+}
+
+void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
   for (Thing* thing : this->things) {
-    if (thing == nullptr)
+    if (thing == nullptr || thing->GetParent() != nullptr)
       continue;
 
     thing->Update(currentTimeMs, true);
 
-    if (this->isIsolated == false && thing->owner != this) {
+    if (this->isIsolated || this->networkId == 0)
+      continue;
+
+    if (thing->isTerminated) {
+      DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
+      this->Send(this->remoteSite, destroyMsg);
+      delete destroyMsg;
+      this->Remove(thing);
+    } else {
+      //  Send to remote site
       PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-      this->Send(thing->owner, poseMsg);
-      BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-      this->Send(thing->owner, binaryMsg);
+      this->Send(this->remoteSite, poseMsg);
       delete poseMsg;
+      BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+      this->Send(this->remoteSite, binaryMsg);
+      delete binaryMsg;
+    }
+  }
+}
+
+void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) {
+  for (Participant* participant : Participant::participants) {
+    if (participant == nullptr || participant == this)
+      continue;
+
+    participant->Update(currentTimeMs);
+    if (this->isIsolated)
+      continue;
+
+    for (Thing* thing : participant->things) {
+      PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
+      this->Send(participant, poseMsg);
+      delete poseMsg;
+      BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
+      this->Send(participant, binaryMsg);
+      delete binaryMsg;
     }
   }
 }
@@ -135,27 +173,6 @@ void ParticipantUDP::ReceiveUDP() {
 #endif
 }
 
-Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) {
-  for (Participant* sender : this->senders) {
-    if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
-      return sender;
-  }
-  return nullptr;
-}
-
-Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
-  // std::cout << "New Participant " << ipAddress << ":" << port << "\n";
-  Participant* participant = new Participant(ipAddress, port);
-#if defined(NO_STD)
-  participant->networkId = this->senderCount;
-  this->senders[this->senderCount++] = participant;
-#else
-  participant->networkId = (unsigned char)this->senders.size();
-  this->senders.push_back(participant);
-#endif
-  return participant;
-}
-
 #pragma region Send
 
 void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
@@ -179,12 +196,13 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
 }
 
 bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
-  // std::cout << "send msg " << (int)this->buffer[0] << " to "
-  //           << remoteParticipant->ipAddress << std::endl;
   int bufferSize = msg->Serialize(this->buffer);
   if (bufferSize <= 0)
     return true;
 
+  // std::cout << "send msg " << (int)this->buffer[0] << " to "
+  // << remoteParticipant->ipAddress << std::endl;
+
 #if defined(_WIN32) || defined(_WIN64)
   Windows::ParticipantUDP* thisWindows =
       static_cast<Windows::ParticipantUDP*>(this);
@@ -256,6 +274,8 @@ bool ParticipantUDP::Publish(IMessage* msg) {
 void ParticipantUDP::ReceiveData(unsigned char packetSize,
                                  char* senderIpAddress,
                                  unsigned int senderPort) {
+  // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
+  //           << std::endl;
   Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
   if (sender == nullptr) {
     sender = this->AddParticipant(senderIpAddress, senderPort);
@@ -343,12 +363,12 @@ void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
             << " -> " << (int)msg->networkId << "\n";
 #endif
 
-  if (this->networkId != msg->networkId) {
+  if (this->networkId != msg->networkId)
     this->networkId = msg->networkId;
-    // std::cout << this->things.size() << " things\n";
-    for (Thing* thing : this->things)
-      this->SendThingInfo(sender, thing);
-  }
+
+  // std::cout << this->things.size() << " things\n";
+  for (Thing* thing : this->things)
+    this->SendThingInfo(sender, thing);
 }
 
 void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
@@ -410,6 +430,17 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
   std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
             << "/" << (int)msg->networkId << "]\n";
 #endif
+  Participant* owner = Participant::GetParticipant(msg->networkId);
+  if (owner == nullptr)
+    return;
+
+  Thing* thing = owner->Get(msg->thingId);
+  if (thing == nullptr)
+    return;
+
+  if ((msg->poseType & PoseMsg::Pose_Position) != 0)
+    thing->SetPosition(msg->position);
+  std::cout << "update position for" << (int)thing->id << std::endl;
 }
 
 void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
@@ -418,16 +449,23 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
             << "/" << (int)msg->thingId << "] ";
 #endif
 
-  Thing* thing = sender->Get(msg->thingId);
-  if (thing != nullptr)
-    thing->ProcessBinary(msg->data);
+  Participant* owner = Participant::GetParticipant(msg->networkId);
+  if (owner != nullptr) {
+    Thing* thing = owner->Get(msg->thingId);
+    if (thing != nullptr)
+      thing->ProcessBinary(msg->data);
 #if !defined(NO_STD)
-  else {
-    std::cout << "  unknown thing [" << (int)msg->networkId << "/"
-              << (int)msg->thingId << "]";
-  }
-  std::cout << std::endl;
+    else {
+#if defined(DEBUG)
+      std::cout << "  unknown thing [" << (int)msg->networkId << "/"
+                << (int)msg->thingId << "]";
 #endif
+    }
+#if defined(DEBUG)
+    std::cout << std::endl;
+#endif
+#endif
+  }
 }
 
 // Receive

Participants/ParticipantUDP.h

@@ -2,6 +2,7 @@
 
 #include "Messages/BinaryMsg.h"
 #include "Messages/InvestigateMsg.h"
+#include "Messages/DestroyMsg.h"
 #include "Messages/ModelUrlMsg.h"
 #include "Messages/NameMsg.h"
 #include "Messages/ParticipantMsg.h"
@@ -100,6 +101,8 @@ class ParticipantUDP : public Participant {
   bool connected = false;
 
   virtual void Update(unsigned long currentTimeMs = 0) override;
+  virtual void UpdateMyThings(unsigned long currentTimeMs);
+  virtual void UpdateOtherThings(unsigned long currentTimeMs);
 
   void SendThingInfo(Participant* remoteParticipant, Thing* thing);
   void PublishThingInfo(Thing* thing);
@@ -112,12 +115,12 @@ class ParticipantUDP : public Participant {
                    unsigned int senderPort);
   void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
 
-#if defined(NO_STD)
-  unsigned char senderCount = 0;
-  Participant* senders[MAX_SENDER_COUNT];
-#else
-  std::list<Participant*> senders;
-#endif
+// #if defined(NO_STD)
+//   unsigned char senderCount = 0;
+//   Participant* senders[MAX_SENDER_COUNT];
+// #else
+//   std::list<Participant*> senders;
+// #endif
 
  protected:
   unsigned long nextPublishMe = 0;
@@ -126,8 +129,8 @@ class ParticipantUDP : public Participant {
 
   void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
 
-  Participant* GetParticipant(const char* ipAddress, int port);
-  Participant* AddParticipant(const char* ipAddress, int port);
+  //Participant* GetParticipant(const char* ipAddress, int port);
+  // Participant* AddParticipant(const char* ipAddress, int port);
 
   void ReceiveUDP();
 

Participants/SiteServer.cpp

@@ -16,17 +16,31 @@ SiteServer::SiteServer(int port) {
   this->ipAddress = "0.0.0.0";
   this->port = port;
 
-#if defined(NO_STD)
-  this->senders[this->senderCount++] = this;
-#else
-  this->senders.push_back(this);
-#endif
-
   SetupUDP(port, ipAddress, 0);
+}
 
-#if !defined(NO_STD)
-  // Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
-#endif
+void SiteServer::UpdateMyThings(unsigned long currentTimeMs) {
+  for (Thing* thing : this->things) {
+    if (thing == nullptr)
+      continue;
+
+    thing->Update(currentTimeMs, true);
+
+    if (this->isIsolated == false) {
+      // Send to all other participants
+      for (Participant* participant : Participant::participants) {
+        if (participant == nullptr || participant == this)
+          continue;
+
+        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+        this->Send(participant, poseMsg);
+        delete poseMsg;
+        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+        this->Send(participant, binaryMsg);
+        delete binaryMsg;
+      }
+    }
+  }
 }
 
 void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
@@ -46,8 +60,7 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) {
   Thing* thing = sender->Get(msg->thingId);
   if (thing == nullptr) {
     // #if defined(NO_STD)
-    new Thing(sender, (Thing::Type)msg->thingType,
-              msg->thingId);
+    new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
     // #else
     //     auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
     //     //Thing* newThing;

Participants/SiteServer.h

@@ -15,18 +15,7 @@ class SiteServer : public ParticipantUDP {
  public:
   SiteServer(int port = 7681);
 
-  // virtual void Update(unsigned long currentTimeMs = 0) override;
-
-// #if !defined(NO_STD)
-//   template <typename ThingClass>
-//   void Register(unsigned char thingType) {
-//     thingMsgProcessors[thingType] = [](Participant* participant,
-//                                        unsigned char networkId,
-//                                        unsigned char thingId) {
-//       return new ThingClass(participant, networkId, thingId);
-//     };
-//   };
-// #endif
+  virtual void UpdateMyThings(unsigned long currentTimeMs) override;
 
  protected:
   unsigned long nextPublishMe = 0;
@@ -34,13 +23,6 @@ class SiteServer : public ParticipantUDP {
   virtual void Process(Participant* sender, ParticipantMsg* msg) override;
   virtual void Process(Participant* sender, SiteMsg* msg) override;
   virtual void Process(Participant* sender, ThingMsg* msg) override;
-
-// #if !defined(NO_STD)
-//   using ThingConstructor = std::function<Thing*(Participant* participant,
-//                                                 unsigned char networkId,
-//                                                 unsigned char thingId)>;
-//   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
-// #endif
 };
 
 }  // namespace RoboidControl

Thing.cpp

@@ -17,12 +17,6 @@
 
 namespace RoboidControl {
 
-// LocalParticipant* Thing::CheckHiddenParticipant() {
-//   if (isolatedParticipant == nullptr)
-//     isolatedParticipant = new LocalParticipant(0);
-//   return isolatedParticipant;
-// }
-
 Thing::Thing(int thingType)
     : Thing(IsolatedParticipant::Isolated(), thingType) {}
 
@@ -36,14 +30,22 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
   this->networkId = 0;
 
   this->position = Spherical::zero;
+  this->positionUpdated = true;
   this->orientation = SwingTwist::identity;
+  this->orientationUpdated = true;
 
   this->linearVelocity = Spherical::zero;
   this->angularVelocity = Spherical::zero;
 
   // std::cout << "add thing [" << (int)this->id << "] to owner "
-  //           << this->owner->ipAddress << ":" << this->owner->port << std::endl;
-  this->owner->Add(this, false);
+  //           << this->owner->ipAddress << ":" << this->owner->port <<
+  //           std::endl;
+  this->owner->Add(this, true);
+}
+
+Thing::Thing(Thing* parent, int thingType, unsigned char thingId)
+    : Thing(parent->owner, thingType, thingId) {
+  this->parent = parent;
 }
 
 // Thing::Thing(Participant* owner,
@@ -64,6 +66,7 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
 // }
 
 void Thing::Terminate() {
+  this->isTerminated = true;
   // Thing::Remove(this);
 }
 
@@ -238,8 +241,10 @@ SwingTwist Thing::GetOrientation() {
 }
 
 void Thing::SetLinearVelocity(Spherical linearVelocity) {
-  this->linearVelocity = linearVelocity;
-  this->linearVelocityUpdated = true;
+  if (this->linearVelocity.distance != linearVelocity.distance) {
+    this->linearVelocity = linearVelocity;
+    this->linearVelocityUpdated = true;
+  }
 }
 
 Spherical Thing::GetLinearVelocity() {
@@ -247,8 +252,10 @@ Spherical Thing::GetLinearVelocity() {
 }
 
 void Thing::SetAngularVelocity(Spherical angularVelocity) {
-  this->angularVelocity = angularVelocity;
-  this->angularVelocityUpdated = true;
+  if (this->angularVelocity.distance != angularVelocity.distance) {
+    this->angularVelocity = angularVelocity;
+    this->angularVelocityUpdated = true;
+  }
 }
 
 Spherical Thing::GetAngularVelocity() {

Thing.h

@@ -58,6 +58,7 @@ class Thing {
   //       unsigned char networkId,
   //       unsigned char thingId,
   //       Type thingType = Type::Undetermined);
+  Thing(Thing* parent, int thingType = 0, unsigned char thingId = 0);
 
   /// @brief The participant managing this thing
   Participant* owner;
@@ -173,6 +174,7 @@ class Thing {
  public:
   /// @brief Terminated things are no longer updated
   void Terminate();
+  bool isTerminated = false;
 
   /// @brief Sets the location from where the 3D model of this Thing can be
   /// loaded from

Things/TouchSensor.cpp

@@ -15,8 +15,8 @@ int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
 }
 
 void TouchSensor::ProcessBinary(char* bytes) {
-  // if (bytes[0] == 1)
-  //   std::cout << this->name << " is Touching something!\n";
+  if (bytes[0] == 1)
+    std::cout << this->name << " is Touching something!\n";
   this->touchedSomething |= (bytes[0] == 1);
 }
 

Things/TouchSensor.h

@@ -7,6 +7,9 @@ namespace RoboidControl {
 /// @brief A sensor which can detect touches
 class TouchSensor : public Thing {
  public:
+  // Inherit all constructors
+  using Thing::Thing;
+  
   /// @brief Create a touch sensor with isolated participant
   TouchSensor();
   /// @brief Create a touch sensor