Improved network sync

Arduino/Things/UltrasonicSensor.cpp

@@ -51,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

@@ -9,7 +9,7 @@ namespace Arduino {
 class UltrasonicSensor : public TouchSensor {
  public:
   // Inherit all constructors
-  using TouchSensor::TouchSensor;
+  //using TouchSensor::TouchSensor;
 
   /// @brief  Setup an ultrasonic sensor
   /// @param participant The participant to use

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;

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

@@ -69,7 +69,7 @@ Participant* Participant::AddParticipant(const char* ipAddress, unsigned int por
 
 void Participant::AddParticipant(Participant* participant) {
   Participant* foundParticipant =
-      Participant::GetParticipant(participant->networkId);
+      Participant::GetParticipant(participant->ipAddress, participant->port);
   if (foundParticipant == nullptr) {
 #if defined(NO_STD)
     this->things[this->thingCount++] = thing;
@@ -80,7 +80,7 @@ void Participant::AddParticipant(Participant* participant) {
               << participant->port << "[" << (int)participant->networkId
               << "]\n";
   } else {
-    std::cout << "Did not add, existing thing " << participant->ipAddress << ":"
+    std::cout << "Did not add, existing participant " << participant->ipAddress << ":"
               << participant->port << "[" << (int)participant->networkId
               << "]\n";
   }
@@ -143,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

@@ -35,7 +35,7 @@ class Participant {
 
   virtual void Update(unsigned long currentTimeMs = 0);
 
- protected:
+ public:
 #if defined(NO_STD)
   unsigned char thingCount = 0;
   Thing* things[MAX_THING_COUNT];

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>
@@ -102,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;
     }
   }
 }
@@ -238,8 +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;
+  // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
+  //           << std::endl;
   Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
   if (sender == nullptr) {
     sender = this->AddParticipant(senderIpAddress, senderPort);
@@ -394,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) {
@@ -409,10 +456,14 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
       thing->ProcessBinary(msg->data);
 #if !defined(NO_STD)
     else {
+#if defined(DEBUG)
       std::cout << "  unknown thing [" << (int)msg->networkId << "/"
                 << (int)msg->thingId << "]";
+#endif
     }
+#if defined(DEBUG)
     std::cout << std::endl;
+#endif
 #endif
   }
 }

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);

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,7 +30,9 @@ 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;
@@ -70,6 +66,7 @@ Thing::Thing(Thing* parent, int thingType, unsigned char thingId)
 // }
 
 void Thing::Terminate() {
+  this->isTerminated = true;
   // Thing::Remove(this);
 }
 
@@ -244,8 +241,10 @@ SwingTwist Thing::GetOrientation() {
 }
 
 void Thing::SetLinearVelocity(Spherical linearVelocity) {
+  if (this->linearVelocity.distance != linearVelocity.distance) {
     this->linearVelocity = linearVelocity;
     this->linearVelocityUpdated = true;
+  }
 }
 
 Spherical Thing::GetLinearVelocity() {
@@ -253,8 +252,10 @@ Spherical Thing::GetLinearVelocity() {
 }
 
 void Thing::SetAngularVelocity(Spherical angularVelocity) {
+  if (this->angularVelocity.distance != angularVelocity.distance) {
     this->angularVelocity = angularVelocity;
     this->angularVelocityUpdated = true;
+  }
 }
 
 Spherical Thing::GetAngularVelocity() {

Thing.h

@@ -174,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);
 }