From 25a0d5e9a4704a329404d1f690738ed5092ba0e3 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Mon, 24 Feb 2025 17:42:37 +0100
Subject: [PATCH] Networked Ant random walk

---
 Arduino/Participant.cpp  | 30 ++++++++++++-------
 Messages/ModelUrlMsg.cpp | 37 ++++++++++++++++-------
 Messages/NameMsg.cpp     |  8 ++++-
 Messages/PoseMsg.cpp     | 32 ++++++++++++++++++++
 Messages/PoseMsg.h       |  6 ++++
 Participant.cpp          | 46 +++++++++++++++++++----------
 Participant.h            |  6 ++--
 RemoteParticipant.cpp    | 64 ++++++++++++++++++++++++++--------------
 RemoteParticipant.h      |  1 +
 Sensors/TouchSensor.cpp  |  1 +
 Thing.cpp                | 20 +++++++++++++
 Thing.h                  | 30 ++++++++++++-------
 12 files changed, 207 insertions(+), 74 deletions(-)

diff --git a/Arduino/Participant.cpp b/Arduino/Participant.cpp
index daa99d7..bb9142f 100644
--- a/Arduino/Participant.cpp
+++ b/Arduino/Participant.cpp
@@ -51,14 +51,14 @@ void Participant::Receive() {
     senderAddress.toCharArray(sender_ipAddress, 16);
     int sender_port = udp.remotePort();
 
-    RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
+    RemoteParticipant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
     if (remoteParticipant == nullptr) {
       remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
-      // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
-      //           << "\n";
-      // std::cout << "New remote participant " << remoteParticipant->ipAddress
-      //           << ":" << remoteParticipant->port << " "
-      //           << (int)remoteParticipant->networkId << "\n";
+      std::cout << "New sender " << sender_ipAddress << ":" << sender_port
+                << "\n";
+      std::cout << "New remote participant " << remoteParticipant->ipAddress
+                << ":" << remoteParticipant->port << " "
+                << (int)remoteParticipant->networkId << "\n";
     }
 
     ReceiveData(packetSize, remoteParticipant);
@@ -69,12 +69,20 @@ void Participant::Receive() {
 
 bool Participant::Send(RemoteParticipant* remoteParticipant, int bufferSize) {
 #if defined(ARDUINO)
-  udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
-  udp.write((unsigned char*)buffer, bufferSize);
-  udp.endPacket();
+// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
+// << remoteParticipant->port << "\n";
+
+int n = 0;
+  do {
+    if (n > 0) {
+      std::cout << "Retry sending\n";
+      delay(10);
+    }
+    n++;
+    udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
+    udp.write((unsigned char*)buffer, bufferSize);
+  } while (udp.endPacket() == 0 && n < 10);
 
-    std::cout << "Sent to " << remoteParticipant->ipAddress << ":"
-              << remoteParticipant->port << "\n";
 #endif
   return true;
 }
diff --git a/Messages/ModelUrlMsg.cpp b/Messages/ModelUrlMsg.cpp
index bb7bdd0..5a61165 100644
--- a/Messages/ModelUrlMsg.cpp
+++ b/Messages/ModelUrlMsg.cpp
@@ -14,15 +14,6 @@ namespace RoboidControl {
 //   this->scale = scale;
 // }
 
-ModelUrlMsg::ModelUrlMsg(const char* buffer) {
-  unsigned char ix = 1;  // first byte is msg id
-  this->networkId = buffer[ix++];
-  this->thingId = buffer[ix++];
-  this->urlLength = buffer[ix++];
-  this->url = &buffer[ix];  // dangerous! name should not be used anymore after
-                            // buffer has been re-used...
-}
-
 ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
@@ -31,10 +22,34 @@ ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
   else
     this->urlLength = (unsigned char)strlen(thing->modelUrl);
 
-  this->url = thing->modelUrl;  // dangerous!
+  //this->url = thing->modelUrl;  // dangerous!
+
+  // the url string in the buffer is not \0 terminated!
+  char* url = new char[this->urlLength + 1];
+  for (int i = 0; i < this->urlLength; i++)
+    url[i] = thing->modelUrl[i];
+  url[this->urlLength] = '\0';
+  this->url = url;}
+
+ModelUrlMsg::ModelUrlMsg(const char* buffer) {
+  unsigned char ix = 1;  // first byte is msg id
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->urlLength = buffer[ix++];
+  // this->url = &buffer[ix];  // dangerous! name should not be used anymore after
+  //                           // buffer has been re-used...
+
+  // the url string in the buffer is not \0 terminated!
+  char* url = new char[this->urlLength + 1];
+  for (int i = 0; i < this->urlLength; i++)
+    url[i] = buffer[ix++];
+  url[this->urlLength] = '\0';
+  this->url = url;
 }
 
-ModelUrlMsg::~ModelUrlMsg() {}
+ModelUrlMsg::~ModelUrlMsg() {
+  delete[] this->url;
+}
 
 unsigned char ModelUrlMsg::Serialize(char* buffer) {
   if (this->urlLength == 0 || this->url == nullptr)
diff --git a/Messages/NameMsg.cpp b/Messages/NameMsg.cpp
index 1cd7125..31b5d0e 100644
--- a/Messages/NameMsg.cpp
+++ b/Messages/NameMsg.cpp
@@ -11,7 +11,13 @@ NameMsg::NameMsg(unsigned char networkId, Thing* thing) {
     this->nameLength = 0;
   else
     this->nameLength = (unsigned char)strlen(thing->name);
-  this->name = thing->name;  // dangerous!
+
+  // 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[this->nameLength] = '\0';
+  this->name = name;
 }
 
 NameMsg::NameMsg(const char* buffer) {
diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp
index 676e557..d164c9e 100644
--- a/Messages/PoseMsg.cpp
+++ b/Messages/PoseMsg.cpp
@@ -19,6 +19,33 @@ PoseMsg::PoseMsg(unsigned char networkId,
   this->linearVelocity = linearVelocity;
   this->angularVelocity = angularVelocity;
 }
+PoseMsg::PoseMsg(unsigned char networkId, Thing* thing) {
+  this->networkId = networkId;
+  this->thingId = thing->id;
+
+  this->poseType = 0;
+  if (thing->positionUpdated) {
+    this->position = thing->GetPosition();
+    this->poseType |= Pose_Position;
+    thing->positionUpdated = false;
+  }
+  if (thing->orientationUpdated) {
+    this->orientation = thing->GetOrientation();
+    this->poseType |= Pose_Orientation;
+    thing->orientationUpdated = false;
+  }
+  if (thing->linearVelocityUpdated) {
+    this->linearVelocity = thing->GetLinearVelocity();
+    this->poseType |= Pose_LinearVelocity;
+    thing->linearVelocityUpdated = false;
+  }
+  if (thing->angularVelocityUpdated) {
+    this->angularVelocity = thing->GetAngularVelocity();
+    this->poseType |= Pose_AngularVelocity;
+    thing->angularVelocityUpdated = false;
+  }
+}
+
 PoseMsg::PoseMsg(const char* buffer) {
   unsigned char ix = 1;  // First byte is msg id
   this->networkId = buffer[ix++];
@@ -26,11 +53,16 @@ PoseMsg::PoseMsg(const char* buffer) {
   this->poseType = buffer[ix++];
   this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
   this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
+  // linearVelocity
+  // angularVelocity
 }
 
 PoseMsg::~PoseMsg() {}
 
 unsigned char PoseMsg::Serialize(char* buffer) {
+  if (this->poseType == 0)
+    return 0;
+
   unsigned char ix = 0;
   buffer[ix++] = PoseMsg::id;
   buffer[ix++] = this->networkId;
diff --git a/Messages/PoseMsg.h b/Messages/PoseMsg.h
index 6d9151a..ca0add2 100644
--- a/Messages/PoseMsg.h
+++ b/Messages/PoseMsg.h
@@ -52,6 +52,12 @@ class PoseMsg : public IMessage {
           SwingTwist16 orientation,
           Spherical16 linearVelocity = Spherical16(),
           Spherical16 angularVelocity = Spherical16());
+
+  /// @brief Create a new message for sending
+  /// @param networkId he network ID of the thing
+  /// @param thing The thing for which the pose shouldbe sent
+  PoseMsg(unsigned char networkId, Thing* thing);
+
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   PoseMsg(const char* buffer);
   /// @brief Destructor for the message
diff --git a/Participant.cpp b/Participant.cpp
index 59a7a4d..974d4fc 100644
--- a/Participant.cpp
+++ b/Participant.cpp
@@ -21,14 +21,13 @@
 
 namespace RoboidControl {
 
-//Participant::Participant() {}
+// Participant::Participant() {}
 
 Participant::Participant(int port) {
   this->ipAddress = "0.0.0.0";
   this->port = port;
 
-  this->senders.push_back(this);
-  this->senders.push_back(this);
+  // this->senders.push_back(this);
 
   // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
   this->localPort = port;
@@ -39,7 +38,7 @@ Participant::Participant(const char* ipAddress, int port) {
   this->ipAddress = ipAddress;
   this->port = port;
 
-  this->senders.push_back(this);
+  // this->senders.push_back(this);
 
   // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
   this->localPort = port;  // randomPort;
@@ -82,14 +81,25 @@ void Participant::Update(unsigned long currentTimeMs) {
     ParticipantMsg* msg = new ParticipantMsg(this->networkId);
     this->Publish(msg);
     delete msg;
-    //std::cout << this->name << " published ParticipantMsg\n";
+    // std::cout << this->name << " published ParticipantMsg\n";
+
+    // for (RemoteParticipant* sender : this->senders) {
+    //   for (Thing* thing : this->things)
+    //     SendThingInfo(sender, thing);
+    // }
+
     this->nextPublishMe = currentTimeMs + this->publishInterval;
   }
   this->ReceiveUDP();
 
   for (Thing* thing : this->things) {
-    if (thing != nullptr)  //} && thing->GetParent() == nullptr)   // update all root things
+    if (thing != nullptr) {
       thing->Update(currentTimeMs);
+      PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+      for (RemoteParticipant* sender : this->senders)
+        this->Send(sender, poseMsg);
+      delete poseMsg;
+    }
   }
 }
 
@@ -106,17 +116,17 @@ void Participant::ReceiveUDP() {
 #endif
 }
 
-Participant* Participant::GetParticipant(const char* ipAddress, int port) {
-  for (Participant* sender : this->senders) {
+RemoteParticipant* Participant::GetParticipant(const char* ipAddress, int port) {
+  for (RemoteParticipant* sender : this->senders) {
     if (sender->ipAddress == ipAddress && sender->port == port)
       return sender;
   }
   return nullptr;
 }
 
-Participant* Participant::AddParticipant(const char* ipAddress, int port) {
+RemoteParticipant* Participant::AddParticipant(const char* ipAddress, int port) {
   std::cout << "New Participant " << ipAddress << ":" << port << "\n";
-  Participant* participant = new Participant(ipAddress, port);
+  RemoteParticipant* participant = new RemoteParticipant(ipAddress, port);
   participant->networkId = (unsigned char)this->senders.size();
   this->senders.push_back(participant);
   return participant;
@@ -129,12 +139,15 @@ void Participant::SendThingInfo(RemoteParticipant* remoteParticipant, Thing* thi
   ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
   this->Send(remoteParticipant, thingMsg);
   delete thingMsg;
-  // NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  // this->Send(remoteParticipant, nameMsg);
-  // delete nameMsg;
-  // ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  // this->Send(remoteParticipant, modelMsg);
-  // delete modelMsg;
+  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+  this->Send(remoteParticipant, nameMsg);
+  delete nameMsg;
+  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
+  this->Send(remoteParticipant, modelMsg);
+  delete modelMsg;
+  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+  this->Send(remoteParticipant, poseMsg);
+  delete poseMsg;
 }
 
 void Participant::PublishThingInfo(Thing* thing) {
@@ -238,6 +251,7 @@ void Participant::Process(RemoteParticipant* sender, SiteMsg* msg) {
   std::cout << this->name << ": process NetworkId [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n";
   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);
   }
diff --git a/Participant.h b/Participant.h
index feaa1aa..83c36e2 100644
--- a/Participant.h
+++ b/Participant.h
@@ -71,14 +71,14 @@ class Participant : public RemoteParticipant {
   void ReceiveData(unsigned char bufferSize, RemoteParticipant* remoteParticipant);
 
  protected:
-  std::list<Participant*> senders;
+  std::list<RemoteParticipant*> senders;
 
   unsigned long nextPublishMe = 0;
 
   void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
 
-  Participant* GetParticipant(const char* ipAddress, int port);
-  Participant* AddParticipant(const char* ipAddress, int port);
+  RemoteParticipant* GetParticipant(const char* ipAddress, int port);
+  RemoteParticipant* AddParticipant(const char* ipAddress, int port);
 
   void ReceiveUDP();
 
diff --git a/RemoteParticipant.cpp b/RemoteParticipant.cpp
index 2c19389..52efd53 100644
--- a/RemoteParticipant.cpp
+++ b/RemoteParticipant.cpp
@@ -1,47 +1,67 @@
 #include "RemoteParticipant.h"
 
+#include <string.h>
+
 namespace RoboidControl {
 
 RemoteParticipant::RemoteParticipant() {}
 
-RemoteParticipant::RemoteParticipant(const char *ipAddress, int port) {
-  this->ipAddress = ipAddress;
+RemoteParticipant::RemoteParticipant(const char* ipAddress, int port) {
+  // make a copy of the ip address string
+  int addressLength = strlen(ipAddress);
+  int stringLength = addressLength + 1;
+  char* addressString = new char[stringLength];
+  strncpy(addressString, ipAddress, addressLength);
+  addressString[addressLength] = '\0';
+
+  this->ipAddress = addressString;
   this->port = port;
 }
 
-Thing *RemoteParticipant::Get(unsigned char networkId, unsigned char thingId) {
-  for (Thing *thing : this->things) {
+RemoteParticipant::~RemoteParticipant() {
+  delete[] this->ipAddress;
+}
+
+Thing* RemoteParticipant::Get(unsigned char networkId, unsigned char thingId) {
+  for (Thing* thing : this->things) {
     if (thing->networkId == networkId && thing->id == thingId)
       return thing;
   }
-//   std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
-//             << "[" << (int)networkId << "/" << (int)thingId << "]\n";
+  //   std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
+  //             << "[" << (int)networkId << "/" << (int)thingId << "]\n";
   return nullptr;
 }
 
-void RemoteParticipant::Add(Thing *thing) {
-  Thing *foundThing = Get(thing->networkId, thing->id);
-  if (foundThing == nullptr) {
+void RemoteParticipant::Add(Thing* thing) {
+  if (thing->id == 0) {
+    // allocate a new thing ID
+    thing->id = this->things.size() + 1;
     this->things.push_back(thing);
-    std::cout << "Add thing " << this->ipAddress << ":" << this->port << "["
-              << (int)thing->networkId << "/" << (int)thing->id << "]\n";
-  } else
-    std::cout << "Did not add, existing thing " << this->ipAddress << ":" << this->port << "["
-              << (int)thing->networkId << "/" << (int)thing->id << "]\n";
+    // std::cout << "Add thing with generated ID " << this->ipAddress << ":" << this->port << "[" << (int)thing->networkId << "/"
+    //           << (int)thing->id << "]\n";
+  } else {
+    Thing* foundThing = Get(thing->networkId, thing->id);
+    if (foundThing == nullptr) {
+      this->things.push_back(thing);
+      // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "[" << (int)thing->networkId << "/"
+      //           << (int)thing->id << "]\n";
+    } 
+    // else
+    //   std::cout << "Did not add, existing thing " << this->ipAddress << ":" << this->port << "["
+    //             << (int)thing->networkId << "/" << (int)thing->id << "]\n";
+  }
 }
 
-void RemoteParticipant::Remove(Thing *thing) {
-  this->things.remove_if([thing](Thing *obj) { return obj == thing; });
-  std::cout << "Removing " << thing->networkId << "/" << thing->id
-            << " list size = " << this->things.size() << "\n";
+void RemoteParticipant::Remove(Thing* thing) {
+  this->things.remove_if([thing](Thing* obj) { return obj == thing; });
+  std::cout << "Removing " << thing->networkId << "/" << thing->id << " list size = " << this->things.size() << "\n";
 }
 
 void RemoteParticipant::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
+  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);
@@ -49,4 +69,4 @@ void RemoteParticipant::UpdateAll(unsigned long currentTimeMs) {
   }
 }
 
-} // namespace Control
+}  // namespace RoboidControl
diff --git a/RemoteParticipant.h b/RemoteParticipant.h
index e9d76a8..4f7fa3a 100644
--- a/RemoteParticipant.h
+++ b/RemoteParticipant.h
@@ -12,6 +12,7 @@ public:
 
   RemoteParticipant();
   RemoteParticipant(const char *ipAddress, int port);
+  ~RemoteParticipant();
 
 protected:
   std::list<Thing *> things;
diff --git a/Sensors/TouchSensor.cpp b/Sensors/TouchSensor.cpp
index 86c3590..508aa8c 100644
--- a/Sensors/TouchSensor.cpp
+++ b/Sensors/TouchSensor.cpp
@@ -4,6 +4,7 @@ namespace RoboidControl {
 
 TouchSensor::TouchSensor(RemoteParticipant* participant) : Thing(participant) {
     this->touchedSomething = false;
+    this->type = (unsigned char) Thing::Type::TouchSensor;
 }
 
 // TouchSensor::TouchSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant, networkId, thingId) {
diff --git a/Thing.cpp b/Thing.cpp
index 361be03..22a4d7a 100644
--- a/Thing.cpp
+++ b/Thing.cpp
@@ -22,6 +22,7 @@ Thing::Thing(RemoteParticipant* participant, unsigned char thingType) {
   this->linearVelocity = Spherical16::zero;
   this->angularVelocity = Spherical16::zero;
 
+  // std::cout << "add thing to participant\n";
   participant->Add(this);
 }
 
@@ -157,6 +158,15 @@ void Thing::Update() {
 }
 #endif
 
+void Thing::Update(unsigned long currentTimeMs) {
+  (void)currentTimeMs;
+
+  // PoseMsg* poseMsg = new PoseMsg(this->networkId, this);
+  // participant->Send(remoteParticipant, poseMsg);
+  // delete poseMsg;
+
+}
+
 void Thing::GenerateBinary(char* buffer, unsigned char* ix) {
   (void)buffer;
   (void)ix;
@@ -182,10 +192,20 @@ SwingTwist16 Thing::GetOrientation() {
   return this->orientation;
 }
 
+void Thing::SetLinearVelocity(Spherical16 linearVelocity) {
+  this->linearVelocity = linearVelocity;
+  this->linearVelocityUpdated = true;
+}
+
 Spherical16 Thing::GetLinearVelocity() {
   return this->linearVelocity;
 }
 
+void Thing::SetAngularVelocity(Spherical16 angularVelocity) {
+  this->angularVelocity = angularVelocity;
+  this->angularVelocityUpdated = true;
+}
+
 Spherical16 Thing::GetAngularVelocity() {
   return this->angularVelocity;
 }
diff --git a/Thing.h b/Thing.h
index a8bdc76..78176cf 100644
--- a/Thing.h
+++ b/Thing.h
@@ -28,6 +28,7 @@ class Thing {
     DistanceSensor,
     DirectionalSensor,
     TemperatureSensor,
+    TouchSensor,
     // Motor,
     ControlledMotor,
     UncontrolledMotor,
@@ -123,7 +124,22 @@ class Thing {
   /// @brief boolean indicating if the orientation was updated
   bool orientationUpdated = false;
 
- protected:
+  /// @brief Set the linear velocity of the thing
+  /// @param linearVelocity The new linear velocity in local space, in meters per second
+  void SetLinearVelocity(Spherical16 linearVelocity);
+  /// @brief Get the linear velocity of the thing
+  /// @return The linear velocity in local space, in meters per second
+  virtual Spherical16 GetLinearVelocity();
+  /// @brief Set the angular velocity of the thing
+  /// @param angularVelocity the new angular velocity in local space
+  void SetAngularVelocity(Spherical16 angularVelocity);
+  /// @brief Get the angular velocity of the thing
+  /// @return The angular velocity in local space
+  virtual Spherical16 GetAngularVelocity();
+  bool linearVelocityUpdated = false;
+  bool angularVelocityUpdated = false;
+
+ private:
   /// @brief The position in local space
   /// @remark When this Thing has a parent, the position is relative to the
   /// parent's position and orientation
@@ -133,17 +149,11 @@ class Thing {
   /// parent's orientation
   SwingTwist16 orientation;
 
- public:
+  /// @brief The linear velocity in local space
   Spherical16 linearVelocity;
+  /// @brief The angular velocity in local spze
   Spherical16 angularVelocity;
 
-  /// @brief Get the linear velocity of the thing
-  /// @return The linear velocity in local space, in meters per second
-  virtual Spherical16 GetLinearVelocity();
-  /// @brief Get the angular velocity of the thing
-  /// @return The angular velocity in local space
-  virtual Spherical16 GetAngularVelocity();
-
  public:
   /// @brief Terminated things are no longer updated
   void Terminate();
@@ -161,7 +171,7 @@ class Thing {
 
   /// @brief Updates the state of the thing
   /// @param currentTimeMs The current clock time in milliseconds
-  virtual void Update(unsigned long currentTimeMs) { (void)currentTimeMs; };
+  virtual void Update(unsigned long currentTimeMs); // { (void)currentTimeMs; };
 
   /// @brief Function used to generate binary data for this thing
   /// @param buffer The byte array for thw binary data