Steps toward hierarchy changes

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;
@@ -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/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 << ":"

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>
@@ -109,28 +108,41 @@ 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) {
+      std::cout << "thing hierarchy changed " << (int)thing->id << std::endl;
+      if (!(this->isIsolated || this->networkId == 0)) {
+          ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
+          this->Send(this->remoteSite, thingMsg);
+          delete thingMsg;
+        }
     }
+
+    // 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->isTerminated) {
+        DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
+        this->Send(this->remoteSite, destroyMsg);
+        delete destroyMsg;
+      } 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->isTerminated)
+      this->Remove(thing);
   }
 }
 
@@ -139,7 +151,7 @@ void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) {
     if (participant == nullptr || participant == this)
       continue;
 
-    participant->Update(currentTimeMs);
+    //participant->Update(currentTimeMs);
     if (this->isIsolated)
       continue;
 
@@ -363,12 +375,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) {

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,11 @@ 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;
-
-//   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);
 }
 
 Thing* Thing::FindThing(const char* name) {
@@ -94,6 +77,8 @@ void Thing::SetParent(Thing* parent) {
     this->parent = nullptr;
   } else
     parent->AddChild(this);
+  std::cout << "setting parent for " << (int) this->id << std::endl;
+  this->hierarchyChanged = true;
 }
 
 void Thing::SetParent(Thing* root, const char* name) {
@@ -199,6 +184,9 @@ 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;
 
   if (recursive) {
     for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {

Thing.h

@@ -107,7 +107,10 @@ 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;
 

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