Binary msg sending fix

Arduino/Things/DRV8833.cpp

@@ -14,7 +14,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
                            unsigned char pinIn2,
                            bool reverse)
     : Thing(driver->owner) {
-  this->parent = driver;
+  this->SetParent(driver);
   
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
@@ -123,9 +123,9 @@ DRV8833::DRV8833(Participant* participant,
     pinMode(pinStandby, OUTPUT);
 
   this->motorA = new DRV8833Motor(this, pinAIn1, pinAIn2, reverseA);
-  this->motorA->name = "Motor A";
+  this->motorA->SetName("Motor A");
   this->motorB = new DRV8833Motor(this, pinBIn1, pinBIn2, reverseB);
-  this->motorB->name = "Motor B";
+  this->motorB->SetName("Motor B");
 }
 
 DRV8833::DRV8833(Thing* parent,
@@ -144,7 +144,7 @@ DRV8833::DRV8833(Thing* parent,
               pinStandby,
               reverseA,
               reverseB) {
-  this->parent = parent;
+                this->SetParent(parent);
 }
 
 }  // namespace Arduino

Arduino/Things/UltrasonicSensor.cpp

@@ -20,7 +20,7 @@ UltrasonicSensor::UltrasonicSensor(Thing* parent,
                                    unsigned char pinTrigger,
                                    unsigned char pinEcho)
     : UltrasonicSensor(parent->owner, pinTrigger, pinEcho) {
-  this->parent = parent;
+  this->SetParent(parent);
 }
 
 float UltrasonicSensor::GetDistance() {

Messages/BinaryMsg.cpp

@@ -6,7 +6,7 @@ BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
   this->thing = thing;
-  unsigned char ix = BinaryMsg::length;
+  unsigned char ix = 0; //BinaryMsg::length;
   this->data = new char[255];
   this->dataLength = this->thing->GenerateBinary(this->data, &ix);
 }
@@ -41,6 +41,8 @@ unsigned char BinaryMsg::Serialize(char* buffer) {
   buffer[ix++] = this->networkId;
   buffer[ix++] = this->thingId;
   buffer[ix++] = this->dataLength;
+  for (int dataIx = 0; dataIx < this->dataLength; dataIx++)
+    buffer[ix++] = this->data[dataIx];
   return this->length + this->dataLength;
 }
 

Messages/NameMsg.cpp

@@ -7,15 +7,16 @@ namespace RoboidControl {
 NameMsg::NameMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
-  if (thing->name == nullptr)
+  const char* thingName = thing->GetName();
+  if (thingName == nullptr)
     this->nameLength = 0;
   else
-    this->nameLength = (unsigned char)strlen(thing->name);
+    this->nameLength = (unsigned char)strlen(thingName);
 
   // the name string in the buffer is not \0 terminated!
   char* name = new char[this->nameLength + 1];
   for (int i = 0; i < this->nameLength; i++)
-    name[i] = thing->name[i];
+    name[i] = thingName[i];
   name[this->nameLength] = '\0';
   this->name = name;
 }

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 && thing->GetLinearVelocity().distance == 0)) {
+  if (thing->positionUpdated || (force && thing->GetParent() != nullptr)) {
     this->position = thing->GetPosition();
     this->poseType |= Pose_Position;
   }
-  if (thing->orientationUpdated || (force && thing->GetAngularVelocity().distance == 0)) {
+  if (thing->orientationUpdated || (force && thing->GetParent() != nullptr)) {
     this->orientation = thing->GetOrientation();
     this->poseType |= Pose_Orientation;
   }

Participant.cpp

@@ -45,8 +45,8 @@ Participant* Participant::GetParticipant(const char* ipAddress,
         participant->port == port)
       return participant;
   }
-  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
-            << std::endl;
+  // std::cout << "Could not find participant " << ipAddress << ":" << (int)port
+  //           << std::endl;
   return nullptr;
 }
 
@@ -57,7 +57,7 @@ Participant* Participant::GetParticipant(unsigned char participantId) {
     if (participant->networkId == participantId)
       return participant;
   }
-  std::cout << "Could not find participant " << (int)participantId << std::endl;
+  // std::cout << "Could not find participant " << (int)participantId << std::endl;
   return nullptr;
 }
 
@@ -104,7 +104,15 @@ void Participant::Add(Thing* thing, bool checkId) {
     thing->id = this->thingCount + 1;
     this->things[this->thingCount++] = thing;
 #else
-    thing->id = (unsigned char)this->things.size() + 1;
+    // find highest id
+    int highestIx = 0;
+    for (Thing* thing : this->things) {
+      if (thing == nullptr)
+        continue;
+      if (thing->id > highestIx)
+        highestIx = thing->id;
+    }
+    thing->id = highestIx + 1;
     this->things.push_back(thing);
 #endif
     // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
@@ -143,22 +151,9 @@ void Participant::Remove(Thing* thing) {
   this->thingCount = lastThingIx;
 #else
   this->things.remove_if([thing](Thing* obj) { return obj == thing; });
-  std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id
-            << "] list size = " << this->things.size() << "\n";
+  // std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id
+  //           << "] list size = " << this->things.size() << "\n";
 #endif
 }
 
-// void Participant::UpdateAll(unsigned long currentTimeMs) {
-//   // Not very efficient, but it works for now.
-
-//   for (Thing* thing : this->things) {
-//     if (thing != nullptr && thing->GetParent() == nullptr) {  // update all
-//     root things
-//       // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
-//       //           << "\n";
-//       thing->Update(currentTimeMs);
-//     }
-//   }
-// }
-
 }  // namespace RoboidControl

Participants/ParticipantUDP.cpp

@@ -5,7 +5,6 @@
 #include "Arduino/ArduinoParticipant.h"
 #include "EspIdf/EspIdfParticipant.h"
 
-
 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
 #include <ws2tcpip.h>
@@ -81,6 +80,8 @@ void ParticipantUDP::SetupUDP(int localPort,
   this->connected = true;
 }
 
+#pragma region Update
+
 void ParticipantUDP::Update(unsigned long currentTimeMs) {
   if (currentTimeMs == 0)
     currentTimeMs = Thing::GetTimeMs();
@@ -109,28 +110,51 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
 
 void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
   for (Thing* thing : this->things) {
-    if (thing == nullptr || thing->GetParent() != nullptr)
+    if (thing == nullptr)  // || thing->GetParent() != nullptr)
       continue;
 
-    thing->Update(currentTimeMs, true);
-
-    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(this->remoteSite, poseMsg);
-      delete poseMsg;
-      BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-      this->Send(this->remoteSite, binaryMsg);
-      delete binaryMsg;
+    if (thing->hierarchyChanged) {
+      if (!(this->isIsolated || this->networkId == 0)) {
+        ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
+        this->Send(this->remoteSite, thingMsg);
+        delete thingMsg;
+        
+        if (thing->nameChanged) {
+          NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+          this->Send(this->remoteSite, nameMsg);
+          delete nameMsg;
+        }
+      }
     }
+
+    // Why don't we do recursive?
+    // Because when a thing creates a thing in the update,
+    // that new thing is not sent out (because of hierarchyChanged)
+    // before it is updated itself: it is immediatedly updated!
+    thing->Update(currentTimeMs, false);
+
+    if (!(this->isIsolated || this->networkId == 0)) {
+      if (thing->terminate) {
+        DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
+        this->Send(this->remoteSite, destroyMsg);
+        delete destroyMsg;
+      } else {
+        //  Send to remote site
+        if (thing->nameChanged) {
+          NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+          this->Send(this->remoteSite, nameMsg);
+          delete nameMsg;
+        }
+        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+        this->Send(this->remoteSite, poseMsg);
+        delete poseMsg;
+        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+        this->Send(this->remoteSite, binaryMsg);
+        delete binaryMsg;
+      }
+    }
+    if (thing->terminate)
+      this->Remove(thing);
   }
 }
 
@@ -154,24 +178,8 @@ void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) {
   }
 }
 
-void ParticipantUDP::ReceiveUDP() {
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  thisWindows->Receive();
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  thisPosix->Receive();
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  thisArduino->Receive();
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  thisEspIdf->Receive();
-#endif
-}
+// Update
+#pragma endregion
 
 #pragma region Send
 
@@ -271,6 +279,25 @@ bool ParticipantUDP::Publish(IMessage* msg) {
 
 #pragma region Receive
 
+void ParticipantUDP::ReceiveUDP() {
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  thisWindows->Receive();
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  thisPosix->Receive();
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  thisArduino->Receive();
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  thisEspIdf->Receive();
+#endif
+}
+
 void ParticipantUDP::ReceiveData(unsigned char packetSize,
                                  char* senderIpAddress,
                                  unsigned int senderPort) {
@@ -363,12 +390,13 @@ 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) {
@@ -407,10 +435,10 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
             nameLength);  // Leave space for null terminator
 #endif
     thingName[nameLength] = '\0';
-    thing->name = thingName;
+    thing->SetName(thingName);
 
 #if !defined(NO_STD)
-    std::cout << thing->name;
+    std::cout << thing->GetName();
 #endif
   }
 #if !defined(NO_STD)
@@ -446,7 +474,7 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
 void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
-            << "/" << (int)msg->thingId << "] ";
+            << "/" << (int)msg->thingId << "]\n";
 #endif
 
   Participant* owner = Participant::GetParticipant(msg->networkId);
@@ -461,9 +489,6 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
                 << (int)msg->thingId << "]";
 #endif
     }
-#if defined(DEBUG)
-    std::cout << std::endl;
-#endif
 #endif
   }
 }

Participants/SiteServer.cpp

@@ -9,6 +9,8 @@
 
 namespace RoboidControl {
 
+#pragma region Init
+
 SiteServer::SiteServer(int port) {
   this->name = "Site Server";
   this->publishInterval = 0;
@@ -19,6 +21,10 @@ SiteServer::SiteServer(int port) {
   SetupUDP(port, ipAddress, 0);
 }
 
+#pragma endregion Init
+
+#pragma region Update
+
 void SiteServer::UpdateMyThings(unsigned long currentTimeMs) {
   for (Thing* thing : this->things) {
     if (thing == nullptr)
@@ -43,8 +49,12 @@ void SiteServer::UpdateMyThings(unsigned long currentTimeMs) {
   }
 }
 
+#pragma endregion Update
+
+#pragma region Receive
+
 void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
-  if (msg->networkId == 0) {
+  if (msg->networkId != sender->networkId) {
     // std::cout << this->name << " received New Client -> " <<
     // sender->ipAddress
     //           << ":" << (int)sender->port << "\n";
@@ -59,21 +69,18 @@ void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
 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);
-    // #else
-    //     auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
-    //     //Thing* newThing;
-    //     if (thingMsgProcessor != thingMsgProcessors.end())  // found item
-    //       //newThing =
-    //           thingMsgProcessor->second(sender, msg->networkId,
-    //           msg->thingId);
-    //     else
-    //       //newThing =
-    //         new Thing(sender, msg->networkId, msg->thingId,
-    //                            (Thing::Type)msg->thingType);
-    // #endif
+
+    if (msg->parentId != 0) {
+      thing->SetParent(Get(msg->parentId));
+      if (thing->GetParent() != nullptr)
+        std::cout << "Could not find parent [" << (int)msg->networkId << "/"
+                  << (int)msg->parentId << "]\n";
+    } else
+      thing->SetParent(nullptr);
   }
 }
 
+#pragma endregion Receive
+
 }  // namespace RoboidControl

Thing.cpp

@@ -33,6 +33,7 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
   this->positionUpdated = true;
   this->orientation = SwingTwist::identity;
   this->orientationUpdated = true;
+  this->hierarchyChanged = true;
 
   this->linearVelocity = Spherical::zero;
   this->angularVelocity = Spherical::zero;
@@ -45,29 +46,16 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
 
 Thing::Thing(Thing* parent, int thingType, unsigned char thingId)
     : Thing(parent->owner, thingType, thingId) {
-  this->parent = parent;
+  this->SetParent(parent);
 }
 
-// Thing::Thing(Participant* owner,
-//             Type thingType,
-//              int thingId) {
-//   // no participant reference yet..
-//   this->owner = owner;
-//   this->networkId = networkId;
-//   this->id = thingId;
-//   this->type = (unsigned char)thingType;
+void Thing::SetName(const char* name) {
+  this->name = name;
+  this->nameChanged = true;
+}
 
-//   this->linearVelocity = Spherical::zero;
-//   this->angularVelocity = Spherical::zero;
-//   // std::cout << "Created thing " << (int)this->networkId << "/" <<
-//   // (int)this->id
-//   //           << "\n";
-//   owner->Add(this, false);
-// }
-
-void Thing::Terminate() {
-  this->isTerminated = true;
-  // Thing::Remove(this);
+const char* Thing::GetName() const {
+  return this->name;
 }
 
 Thing* Thing::FindThing(const char* name) {
@@ -94,6 +82,7 @@ void Thing::SetParent(Thing* parent) {
     this->parent = nullptr;
   } else
     parent->AddChild(this);
+  this->hierarchyChanged = true;
 }
 
 void Thing::SetParent(Thing* root, const char* name) {
@@ -179,6 +168,8 @@ void Thing::SetModel(const char* url) {
   this->modelUrl = url;
 }
 
+#pragma region Update
+
 unsigned long Thing::GetTimeMs() {
 #if defined(ARDUINO)
   return millis();
@@ -199,6 +190,10 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) {
   //   OnPoseChanged callback
   this->positionUpdated = false;
   this->orientationUpdated = false;
+  // this->linearVelocityUpdated = false;
+  // this->angularVelocityUpdated = false;
+  this->hierarchyChanged = false;
+  this->nameChanged = false;
 
   if (recursive) {
     for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
@@ -214,6 +209,8 @@ void Thing::UpdateThings(unsigned long currentTimeMs) {
   IsolatedParticipant::Isolated()->Update(currentTimeMs);
 }
 
+#pragma endregion Update
+
 int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
   (void)buffer;
   (void)ix;

Thing.h

@@ -107,13 +107,21 @@ class Thing {
   /// @return The found thing of nullptr when nothing is found
   Thing* GetChildByIndex(unsigned char ix);
 
- protected:
+  /// @brief Indicator that the hierarchy of the thing has changed
+  bool hierarchyChanged = true;
+
+ private:
   Thing* parent = nullptr;
   Thing** children = nullptr;
 
- public:
   /// @brief The name of the thing
   const char* name = nullptr;
+
+ public:
+  void SetName(const char* name);
+  const char* GetName() const;
+  bool nameChanged = false;
+
   /// @brief An URL pointing to the location where a model of the thing can be
   /// found
   const char* modelUrl = nullptr;
@@ -174,7 +182,7 @@ class Thing {
  public:
   /// @brief Terminated things are no longer updated
   void Terminate();
-  bool isTerminated = false;
+  bool terminate = false;
 
   /// @brief Sets the location from where the 3D model of this Thing can be
   /// loaded from

Things/DifferentialDrive.cpp

@@ -42,30 +42,27 @@ void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
 }
 
 void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
-  if (this->linearVelocityUpdated == false)
-    return;
-  // this assumes forward velocity only....
-  float linearVelocity = this->GetLinearVelocity().distance;
+  if (this->linearVelocityUpdated) {
+    // this assumes forward velocity only....
+    float linearVelocity = this->GetLinearVelocity().distance;
 
-  Spherical angularVelocity = this->GetAngularVelocity();
-  float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
-  // Determine the rotation direction
-  if (angularVelocity.direction.horizontal.InDegrees() < 0)
-    angularSpeed = -angularSpeed;
+    Spherical angularVelocity = this->GetAngularVelocity();
+    float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
+    // Determine the rotation direction
+    if (angularVelocity.direction.horizontal.InDegrees() < 0)
+      angularSpeed = -angularSpeed;
 
-  // wheel separation can be replaced by this->leftwheel->position->distance
-  float speedLeft =
-      (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
-      this->wheelRadius * Rad2Deg;
-  float speedRight =
-      (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
-      this->wheelRadius * Rad2Deg;
+    // wheel separation can be replaced by this->leftwheel->position->distance
+    float speedLeft =
+        (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
+        this->wheelRadius * Rad2Deg;
+    float speedRight =
+        (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
+        this->wheelRadius * Rad2Deg;
 
-  this->SetWheelVelocity(speedLeft, speedRight);
+    this->SetWheelVelocity(speedLeft, speedRight);
+  }
   Thing::Update(currentMs, recursive);
-  // std::cout << "lin. speed " << linearVelocity << "   ang. speed " <<
-  // angularVelocity.distance << "   left wheel  "
-  //         << speedLeft << "   right wheel " << speedRight << "\n";
 }
 
 }  // namespace RoboidControl

Things/TouchSensor.cpp

@@ -16,7 +16,7 @@ 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";
+    std::cout << this->GetName() << " is Touching something!\n";
   this->touchedSomething |= (bytes[0] == 1);
 }