Merge remote-tracking branch 'origin_gitea/main' into Braccio

EspIdf/EspIdfParticipant.cpp

@@ -1,8 +1,6 @@
 #include "EspIdfParticipant.h"
 
-#if defined(IDF_VER)
 #include "esp_wifi.h"
-#endif
 
 namespace RoboidControl {
 namespace EspIdf {
@@ -11,6 +9,7 @@ void ParticipantUDP::Setup(int localPort,
                              const char* remoteIpAddress,
                              int remotePort) {
 #if defined(IDF_VER)
+std::cout << "Set up UDP\n";
   GetBroadcastAddress();
 
   wifi_ap_record_t ap_info;
@@ -21,8 +20,8 @@ void ParticipantUDP::Setup(int localPort,
   }
 
   // Create a UDP socket
-  sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+  this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-  if (sockfd < 0) {
+  if (this->sockfd < 0) {
     std::cout << "Unable to create UDP socket: errno " << errno << "\n";
     vTaskDelete(NULL);
     return;
@@ -38,7 +37,7 @@ void ParticipantUDP::Setup(int localPort,
 
       
   // Bind the socket to the address and port
-  if (bind(sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) {
+  if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) {
     std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
     close(sockfd);
     vTaskDelete(NULL);
@@ -61,59 +60,66 @@ void ParticipantUDP::Setup(int localPort,
 void ParticipantUDP::GetBroadcastAddress() {
   // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION...
 
-  // esp_netif_ip_info_t ip_info;
+  esp_netif_ip_info_t ip_info;
-  // esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
+  esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
 
-  // // Get IP information (IP address, netmask, gateway)
+  // Get IP information (IP address, netmask, gateway)
-  // if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
+  if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
-  //   std::cout << "Failed to get IP info\n";
+    std::cout << "Failed to get IP info\n";
-  //   return;
+    return;
-  // }
+  }
 
-  // ip_addr_t broadcast_addr = {};
+  ip_addr_t broadcast_addr = {};
-  // broadcast_addr.u_addr.ip4.addr =
+  broadcast_addr.u_addr.ip4.addr =
-  //     (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr;
+      (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr;
 
-  // snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR,
+  snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR,
-  //          IP2STR(&broadcast_addr.u_addr.ip4));
+           IP2STR(&broadcast_addr.u_addr.ip4));
-  // std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n";
+  std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n";
 }
 
 void ParticipantUDP::Receive() {
 #if defined(IDF_VER)
-  struct pollfd fds;
+  struct pollfd fds[1];
-  fds.fd = sockfd;
+  fds[0].fd = sockfd;
-  fds.events = POLLIN;  // We're looking for data available to read
+  fds[0].events = POLLIN;  // We're looking for data available to read
 
   // Use poll() with a timeout of 0 to return immediately
-  int ret = poll(&fds, 1, 0);
+  int ret = poll(fds, 1, 0);
   if (ret == -1) {
     std::cout << "poll() error\n";
     return;
   }
-  socklen_t addr_len = sizeof(this->src_addr);
+  
+  //char buffer[1024];
+  struct sockaddr_in source_addr;
+
+  socklen_t addr_len = sizeof(source_addr);
   char sender_ipAddress[INET_ADDRSTRLEN];
 
-  while (ret > 0 && fds.revents & POLLIN) {
+  while (ret > 0 && fds[0].revents & POLLIN) {
     int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0,
-                              (struct sockaddr*)&this->src_addr, &addr_len);
+                              (struct sockaddr*)&source_addr, &addr_len);
     if (packetSize < 0) {
       std::cout << "recvfrom() error\n";
       return;
+    } else if (packetSize == 0) {
+      break;
     }
 
-    std::cout << "receiving " << packetSize << " bytes\n";
+    // std::cout << "receiving " << packetSize << " bytes, msgId " << (int)this->buffer[0] << "\n";
-    // inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
+    inet_ntoa_r(source_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
-    // unsigned int sender_port = ntohs(this->src_addr.sin_port);
+    unsigned int sender_port = ntohs(source_addr.sin_port);
 
-    // ReceiveData(packetSize, sender_ipAddress, sender_port);
+    ReceiveData(packetSize, sender_ipAddress, sender_port);
 
-    int ret = poll(&fds, 1, 0);
+    ret = poll(fds, 1, 0);
     if (ret == -1) {
       std::cout << "poll() error\n";
       return;
     }
   }
+  //std::cout << "no more messages\n";
 
 #endif // IDF_VER
 }
@@ -146,12 +152,6 @@ bool ParticipantUDP::Publish(IMessage* msg) {
                    sizeof(dest_addr));
   if (err != 0)
     std::cout << "Publish error\n";
-  //   udp.beginPacket(this->broadcastIpAddress, this->remotePort);
-  //   udp.write((unsigned char*)buffer, bufferSize);
-  //   udp.endPacket();
-
-  //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
-  //             << this->remotePort << "\n";
 #endif
   return true;
 };

EspIdf/EspIdfParticipant.h

@@ -2,9 +2,7 @@
 
 #include "Participants/ParticipantUDP.h"
 
-#if defined(IDF_VER)
 #include "lwip/sockets.h"
-#endif
 
 namespace RoboidControl {
 namespace EspIdf {
@@ -17,13 +15,12 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
   bool Publish(IMessage* msg);
 
  protected:
-#if defined(IDF_VER)
+  char broadcastIpAddress[INET_ADDRSTRLEN];  // IPv4 address can have a max of 15 characters
-  char broadcastIpAddress[INET_ADDRSTRLEN];
+                                // + null  terminator
 
   int sockfd;
   struct sockaddr_in dest_addr;
-  struct sockaddr_in src_addr;
+  //struct sockaddr_in src_addr;
-#endif
 
   void GetBroadcastAddress();
 };

LinearAlgebra/Matrix.cpp

@@ -1,6 +1,8 @@
 #include "Matrix.h"
 #include <iostream>
 
+namespace LinearAlgebra {
+
 #pragma region Matrix1
 
 Matrix1::Matrix1(int size) : size(size) {
@@ -12,8 +14,7 @@ Matrix1::Matrix1(int size) : size(size) {
   }
 }
 
-Matrix1::Matrix1(float* data, int size)
+Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
-    : data(data), size(size) {
   this->externalData = true;
 }
 
@@ -28,12 +29,7 @@ Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
 }
 
 Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
-  return Quaternion(
+  return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
-    this->data[0],
-    this->data[1],
-    this->data[2],
-    this->data[3]
-  );
 }
 
 // Matrix1
@@ -46,9 +42,9 @@ Matrix2::Matrix2() {}
 Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
   this->nValues = nRows * nCols;
   if (this->nValues == 0)
-    data = nullptr;
+    this->data = nullptr;
   else {
-    this->data = new float[nValues]();
+    this->data = new float[this->nValues];
     this->externalData = false;
   }
 }
@@ -59,11 +55,44 @@ Matrix2::Matrix2(float* data, int nRows, int nCols)
   this->externalData = true;
 }
 
+Matrix2::Matrix2(const Matrix2& m)
+    : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) {
+  if (this->nValues == 0)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->nValues];
+    std::copy(m.data, m.data + nValues, this->data);
+  }
+}
+
+Matrix2& Matrix2::operator=(const Matrix2& m) {
+  if (this != &m) {
+    delete[] this->data;  // Free the current memory
+
+    this->nRows = m.nRows;
+    this->nCols = m.nCols;
+    this->nValues = m.nValues;
+    if (this->nValues == 0)
+      this->data = nullptr;
+    else {
+      this->data = new float[this->nValues];
+      std::copy(m.data, m.data + this->nValues, this->data);
+    }
+  }
+  return *this;
+}
+
 Matrix2::~Matrix2() {
-  if (data != nullptr && !this->externalData)
+  if (!this->externalData)
     delete[] data;
 }
 
+Matrix2 Matrix2::Clone() const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  std::copy(this->data, this->data + this->nValues, r.data);
+  return r;
+}
+
 // Move constructor
 Matrix2::Matrix2(Matrix2&& other) noexcept
     : nRows(other.nRows),
@@ -94,6 +123,11 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) {
   return r;
 }
 
+void Matrix2::Clear() {
+  for (int ix = 0; ix < this->nValues; ix++)
+    this->data[ix] = 0;
+}
+
 Matrix2 Matrix2::Identity(int size) {
   return Diagonal(1, size);
 }
@@ -121,6 +155,17 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
   return r;
 }
 
+Matrix2 Matrix2::Transpose() const {
+  Matrix2 r = Matrix2(this->nCols, this->nRows);
+
+  for (uint rowIx = 0; rowIx < this->nRows; rowIx++) {
+    for (uint colIx = 0; colIx < this->nCols; colIx++)
+      r.data[colIx * this->nCols + rowIx] =
+          this->data[rowIx * this->nCols + colIx];
+  }
+  return r;
+}
+
 Matrix2 LinearAlgebra::Matrix2::operator-() const {
   Matrix2 r = Matrix2(this->nRows, this->nCols);
   for (int ix = 0; ix < r.nValues; ix++)
@@ -135,6 +180,12 @@ Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
   return r;
 }
 
+Matrix2 Matrix2::operator+=(const Matrix2& v) {
+  for (int ix = 0; ix < this->nValues; ix++)
+    this->data[ix] += v.data[ix];
+  return *this;
+}
+
 Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
   Matrix2 r = Matrix2(this->nRows, B.nCols);
 
@@ -149,30 +200,54 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
     int BColOffset = i * BCols;  // BColOffset is constant for each row of B
     for (int j = 0; j < BCols; ++j) {
       float sum = 0;
-      std::cout << " 0";
+      // std::cout << " 0";
       int BIndex = j;
       for (int k = 0; k < ACols; ++k) {
-        std::cout << " + " << this->data[ARowOffset + k] << " * "
+        // std::cout << " + " << this->data[ARowOffset + k] << " * "
-                  << B.data[BIndex];
+        //           << B.data[BIndex];
         sum += this->data[ARowOffset + k] * B.data[BIndex];
         BIndex += BCols;
       }
       r.data[BColOffset + j] = sum;
-      std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
+      // std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
     }
   }
   return r;
 }
 
-void LinearAlgebra::Matrix2::SetSlice(int rowStart,
+Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
-                                      int rowStop,
+  Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
-                                      int colStart,
+
-                                      int colStop,
+  int resultRowIx = 0;
-                                      const Matrix2& m) const {
+  int resultColIx = 0;
   for (int i = rowStart; i < rowStop; i++) {
     for (int j = colStart; j < colStop; j++)
-      this->data[i * this->nCols + j] =
+      r.data[resultRowIx * r.nCols + resultColIx] =
-          m.data[(i - rowStart) * m.nCols + (j - colStart)];
+          this->data[i * this->nCols + j];
+  }
+  return r;
+}
+
+void Matrix2::UpdateSlice(int rowStart,
+                          int rowStop,
+                          int colStart,
+                          int colStop,
+                          const Matrix2& m) const {
+  // for (int i = rowStart; i < rowStop; i++) {
+  //   for (int j = colStart; j < colStop; j++)
+  //     this->data[i * this->nCols + j] =
+  //         m.data[(i - rowStart) * m.nCols + (j - colStart)];
+  // }
+
+  int rRowDataIx = rowStart * this->nCols;
+  int mRowDataIx = 0;
+  for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
+    rRowDataIx = rowIx * this->nCols;
+    // rRowDataIx += this->nCols;
+    mRowDataIx += m.nCols;
+    for (int colIx = colStart; colIx < colStop; colIx++) {
+      this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
+    }
   }
 }
 
@@ -181,7 +256,7 @@ void LinearAlgebra::Matrix2::SetSlice(int rowStart,
 /// @return 4x4 Omega matrix
 Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
   Matrix2 r = Matrix2::Zero(4, 4);
-  r.SetSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));
+  r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));
 
   // set last row to -v
   int ix = 3 * 4;
@@ -200,6 +275,8 @@ Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
 // Matrix2
 #pragma endregion
 
+}  // namespace LinearAlgebra
+
 template <>
 MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   if (rows <= 0 || cols <= 0) {

LinearAlgebra/Matrix.h

@@ -33,10 +33,15 @@ class Matrix2 {
   Matrix2();
   Matrix2(int nRows, int nCols);
   Matrix2(float* data, int nRows, int nCols);
+  Matrix2(const Matrix2& m);
+  Matrix2& operator=(const Matrix2& other);
 
   ~Matrix2();
 
+  Matrix2 Clone() const;
+
   static Matrix2 Zero(int nRows, int nCols);
+  void Clear();
 
   static Matrix2 Identity(int size);
 
@@ -44,12 +49,15 @@ class Matrix2 {
 
   static Matrix2 SkewMatrix(const Vector3& v);
 
+  Matrix2 Transpose() const;
+
   Matrix2 operator-() const;
 
   /// @brief Add a matrix to this matrix
   /// @param m The matrix to add to this matrix
   /// @return The result of the addition
   Matrix2 operator+(const Matrix2& v) const;
+  Matrix2 operator+=(const Matrix2& v);
 
   Matrix2 operator*(const Matrix2& m) const;
   friend Matrix2 operator*(const Matrix2& m, float f) {
@@ -74,14 +82,23 @@ class Matrix2 {
     }
     return r;
   }
-  // friend Matrix2 operator*(float f, const Matrix2& v) {
-  //   Matrix2 r = Matrix2(v.nRows, v.nCols);
-  //   for (int ix = 0; ix < r.nValues; ix++)
-  //     r.data[ix] = f * v.data[ix];
-  //   return r;
-  // }
 
-  void SetSlice(int rowStart,
+  friend Matrix2 operator/(const Matrix2& m, float f) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = m.data[ix] / f;
+    return r;
+  }
+  friend Matrix2 operator/(float f, const Matrix2& m) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = f / m.data[ix];
+    return r;
+  }
+
+  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
+
+  void UpdateSlice(int rowStart,
                 int rowStop,
                 int colStart,
                 int colStop,
@@ -208,6 +225,6 @@ class MatrixOf {
 };
 
 }  // namespace LinearAlgebra
-//using namespace LinearAlgebra;
+// using namespace LinearAlgebra;
 
 #endif

Messages/BinaryMsg.cpp

@@ -2,33 +2,41 @@
 
 namespace RoboidControl {
 
-BinaryMsg::BinaryMsg(char* buffer) {
-  unsigned char ix = 1;
-  this->networkId = buffer[ix++];
-  this->thingId = buffer[ix++];
-  this->bytes = buffer + ix;  // This is only valid because the code ensures the the msg
-                              // lifetime is shorter than the buffer lifetime...
-}
-
 BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
   this->thing = thing;
+  unsigned char ix = BinaryMsg::length;
+  this->dataLength = this->thing->GenerateBinary(this->data, &ix);
 }
 
-BinaryMsg::~BinaryMsg() {}
+BinaryMsg::BinaryMsg(char* buffer) {
+  unsigned char ix = 1;
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->dataLength = buffer[ix++];
+  char* data = new char[this->dataLength];
+  for (int i = 0; i < this->dataLength; i++)
+    data[i] = buffer[ix++];
+  this->data = data;
+}
+
+BinaryMsg::~BinaryMsg() {
+  delete[] this->data;
+}
 
 unsigned char BinaryMsg::Serialize(char* buffer) {
-  unsigned char ix = this->length;
+  // unsigned char ix = this->length;
-  this->thing->GenerateBinary(buffer, &ix);
+  //  this->dataLength = this->thing->GenerateBinary(buffer, &ix);
-  if (ix <= this->length)  // in this case, no data is actually sent
+  if (this->dataLength <= 0)  // in this case, no data is actually sent
     return 0;
 
-  buffer[0] = this->id;
+  unsigned char ix = 0;
-  buffer[1] = this->networkId;
+  buffer[ix++] = this->id;
-  buffer[2] = this->thingId;
+  buffer[ix++] = this->networkId;
-  return ix;
+  buffer[ix++] = this->thingId;
+  buffer[ix++] = this->dataLength;
+  return this->length + this->dataLength;
 }
 
-
 }  // namespace RoboidControl

Messages/BinaryMsg.h

@@ -10,7 +10,7 @@ class BinaryMsg : public IMessage {
   /// @brief The message ID
   static const unsigned char id = 0xB1;
   /// @brief The length of the message without the binary data itslef
-  static const unsigned length = 3;
+  static const unsigned length = 4;
 
   /// @brief The network ID of the thing
   unsigned char networkId;
@@ -19,8 +19,9 @@ class BinaryMsg : public IMessage {
   /// @brief The thing for which the binary data is communicated
   Thing* thing;
 
+  unsigned char dataLength;
   /// @brief The binary data which is communicated
-  char* bytes = nullptr;
+  char* data = nullptr;
 
   /// @brief Create a new message for sending
   /// @param networkId The network ID of the thing

Messages/Messages.cpp

@@ -1,7 +1,6 @@
 #include "Messages.h"
 
 #include "LowLevelMessages.h"
-//#include "Participant.h"
 #include "string.h"
 
 namespace RoboidControl {
@@ -10,26 +9,10 @@ namespace RoboidControl {
 
 IMessage::IMessage() {}
 
-// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
-
-// IMessage::IMessage(char* buffer) {}
-
 unsigned char IMessage::Serialize(char* buffer) {
   return 0;
 }
 
-// bool IMessage::SendMsg(ParticipantUDP *client, IMessage msg) {
-//   // return SendMsg(client, client.buffer, );nameLength
-//   return client->SendBuffer(msg.Serialize(client->buffer));
-// }
-
-// bool IMessage::Publish(ParticipantUDP *participant) {
-//   return participant->PublishBuffer(Serialize(participant->buffer));
-// }
-// bool IMessage::SendTo(ParticipantUDP *participant) {
-//   return participant->SendBuffer(Serialize(participant->buffer));
-// }
-
 // IMessage
 #pragma endregion
 

Messages/ModelUrlMsg.h

@@ -8,7 +8,7 @@ class ModelUrlMsg : public IMessage {
   /// @brief The message ID
   static const unsigned char id = 0x90;
   /// @brief The length of the message without the URL string itself
-  static const unsigned char length = 3;
+  static const unsigned char length = 4;
 
   /// @brief The network ID of the thing
   unsigned char networkId;

Messages/NameMsg.cpp

@@ -25,6 +25,7 @@ NameMsg::NameMsg(const char* buffer) {
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
   this->nameLength = buffer[ix++];
+
   // 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++)

Messages/PoseMsg.cpp

@@ -11,12 +11,10 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
   if (thing->positionUpdated || force) {
     this->position = thing->GetPosition();
     this->poseType |= Pose_Position;
-    thing->positionUpdated = false;
   }
   if (thing->orientationUpdated || force) {
     this->orientation = thing->GetOrientation();
     this->poseType |= Pose_Orientation;
-    thing->orientationUpdated = false;
   }
   if (thing->linearVelocityUpdated) {
     this->linearVelocity = thing->GetLinearVelocity();

Participant.cpp

@@ -27,17 +27,20 @@ Participant::~Participant() {
   delete[] this->ipAddress;
 }
 
-void Participant::Update(unsigned long currentTimeMs) {}
+void Participant::Update(unsigned long currentTimeMs) {
-
+  for (Thing* thing : this->things) {
-Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
+    if (thing != nullptr)
+      thing->Update(currentTimeMs, true);
+  }
+}
+
+Thing* Participant::Get(unsigned char thingId) {
   for (Thing* thing : this->things) {
-    // if (thing->networkId == networkId && thing->id == thingId)
     if (thing->id == thingId)
       return thing;
   }
-  //   std::cout << "Could not find thing " << this->ipAddress << ":" <<
+  std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
-  //   this->port
+            << "[" << (int)thingId << "]\n";
-  //             << "[" << (int)networkId << "/" << (int)thingId << "]\n";
   return nullptr;
 }
 
@@ -51,25 +54,22 @@ void Participant::Add(Thing* thing, bool checkId) {
     thing->id = (unsigned char)this->things.size() + 1;
     this->things.push_back(thing);
 #endif
-    // std::cout << "Add thing with generated ID " << this->ipAddress << ":" <<
+    // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
-    // this->port << "[" << (int)thing->networkId << "/"
+    //           << this->port << "[" << (int)thing->id << "]\n";
-    //           << (int)thing->id << "]\n";
   } else {
-    Thing* foundThing = Get(thing->networkId, thing->id);
+    Thing* foundThing = Get(thing->id);
     if (foundThing == nullptr) {
 #if defined(NO_STD)
       this->things[this->thingCount++] = thing;
 #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->networkId << "/"
       //           << (int)thing->id << "]\n";
+    } else {
+      // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
+      //           << this->port << "[" << (int)thing->id << "]\n";
     }
-    // else
-    //   std::cout << "Did not add, existing thing " << this->ipAddress << ":"
-    //   << this->port << "["
-    //             << (int)thing->networkId << "/" << (int)thing->id << "]\n";
   }
 }
 

Participant.h

@@ -50,7 +50,7 @@ class Participant {
   /// @param thingId The ID of the thing
   /// @return The thing if found or nullptr when no thing has been found
   /// @note The use of the network ID is likely to disappear in future versions.
-  Thing* Get(unsigned char networkId, unsigned char thingId);
+  Thing* Get(unsigned char thingId);
   /// @brief Add a new thing for this participant.
   /// @param thing The thing to add
   /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing

Participants/ParticipantUDP.cpp

@@ -28,13 +28,12 @@ namespace RoboidControl {
 ParticipantUDP::ParticipantUDP(int port) {
   this->ipAddress = "0.0.0.0";
   this->port = port;
+  this->remoteSite = nullptr;
   if (this->port == 0)
     this->isIsolated = true;
 }
 
-ParticipantUDP::ParticipantUDP(const char* ipAddress,
+ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
-                                   int port,
-                                   int localPort)
     : Participant("127.0.0.1", localPort) {
   if (this->port == 0)
     this->isIsolated = true;
@@ -58,15 +57,14 @@ void ParticipantUDP::begin() {
 }
 
 void ParticipantUDP::SetupUDP(int localPort,
-                                const char* remoteIpAddress,
+                              const char* remoteIpAddress,
-                                int remotePort) {
+                              int remotePort) {
 #if defined(_WIN32) || defined(_WIN64)
   Windows::ParticipantUDP* thisWindows =
       static_cast<Windows::ParticipantUDP*>(this);
   thisWindows->Setup(localPort, remoteIpAddress, remotePort);
 #elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix =
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-      static_cast<Posix::ParticipantUDP*>(this);
   thisPosix->Setup(localPort, remoteIpAddress, remotePort);
 #elif defined(ARDUINO)
   Arduino::ParticipantUDP* thisArduino =
@@ -90,6 +88,7 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
 
     if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
       ParticipantMsg* msg = new ParticipantMsg(this->networkId);
+      std::cout << "Send Participant\n";
       if (this->remoteSite == nullptr)
         this->Publish(msg);
       else
@@ -98,19 +97,22 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
 
       this->nextPublishMe = currentTimeMs + this->publishInterval;
     }
+
     this->ReceiveUDP();
   }
 
   for (Thing* thing : this->things) {
-    if (thing != nullptr) {
+    if (thing == nullptr)
-      thing->Update(currentTimeMs);
+      continue;
-      if (this->isIsolated == false) {
+
-        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+    if (this->isIsolated == false) {
-        for (Participant* sender : this->senders)
+      PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-          this->Send(sender, poseMsg);
+      this->Send(thing->owner, poseMsg);
-        delete poseMsg;
+      BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-      }
+      this->Send(thing->owner, binaryMsg);
+      delete poseMsg;
     }
+    thing->Update(currentTimeMs, true);
   }
 }
 
@@ -120,8 +122,7 @@ void ParticipantUDP::ReceiveUDP() {
       static_cast<Windows::ParticipantUDP*>(this);
   thisWindows->Receive();
 #elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix =
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-      static_cast<Posix::ParticipantUDP*>(this);
   thisPosix->Receive();
 #elif defined(ARDUINO)
   Arduino::ParticipantUDP* thisArduino =
@@ -158,8 +159,8 @@ Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
 #pragma region Send
 
 void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
-                                     Thing* thing) {
+                                   Thing* thing) {
-  // std::cout << "Send thing info " << (int)thing->id << " \n";
+  std::cout << "Send thing info [" << (int)thing->id << "] \n";
   ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
   this->Send(remoteParticipant, thingMsg);
   delete thingMsg;
@@ -187,8 +188,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
       static_cast<Windows::ParticipantUDP*>(this);
   return thisWindows->Send(remoteParticipant, bufferSize);
 #elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix =
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-      static_cast<Posix::ParticipantUDP*>(this);
   return thisPosix->Send(remoteParticipant, bufferSize);
 #elif defined(ARDUINO)
   Arduino::ParticipantUDP* thisArduino =
@@ -230,8 +230,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
       static_cast<Windows::ParticipantUDP*>(this);
   return thisWindows->Publish(msg);
 #elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix =
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-      static_cast<Posix::ParticipantUDP*>(this);
   return thisPosix->Publish(msg);
 #elif defined(ARDUINO)
   Arduino::ParticipantUDP* thisArduino =
@@ -252,66 +251,83 @@ bool ParticipantUDP::Publish(IMessage* msg) {
 #pragma region Receive
 
 void ParticipantUDP::ReceiveData(unsigned char packetSize,
-                                   char* senderIpAddress,
+                                 char* senderIpAddress,
-                                   unsigned int senderPort) {
+                                 unsigned int senderPort) {
-  Participant* remoteParticipant =
+  Participant* sender =
       this->GetParticipant(senderIpAddress, senderPort);
-  if (remoteParticipant == nullptr) {
+  if (sender == nullptr) {
-    remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
+    sender = this->AddParticipant(senderIpAddress, senderPort);
-    // std::cout << "New sender " << senderIpAddress << ":" << senderPort
+    std::cout << "New remote participant " << sender->ipAddress
-    //           << "\n";
+              << ":" << sender->port << std::endl;
-    std::cout << "New remote participant " << remoteParticipant->ipAddress
-              << ":" << remoteParticipant->port << " "
-              << (int)remoteParticipant->networkId << "\n";
   }
 
-  ReceiveData(packetSize, remoteParticipant);
+  ReceiveData(packetSize, sender);
 }
 
 void ParticipantUDP::ReceiveData(unsigned char bufferSize,
-                                   Participant* remoteParticipant) {
+                                 Participant* sender) {
   unsigned char msgId = this->buffer[0];
-  // std::cout << "receive msg " << (int)msgId << "\n";
+  std::cout << "receive msg " << (int)msgId << "\n";
+  // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
   switch (msgId) {
     case ParticipantMsg::id: {
       ParticipantMsg* msg = new ParticipantMsg(this->buffer);
-      Process(remoteParticipant, msg);
+      bufferSize -= msg->length;
+      Process(sender, msg);
       delete msg;
     } break;
     case SiteMsg::id: {
       SiteMsg* msg = new SiteMsg(this->buffer);
-      Process(remoteParticipant, msg);
+      bufferSize -= msg->length;
+      Process(sender, msg);
       delete msg;
     } break;
     case InvestigateMsg::id: {
       InvestigateMsg* msg = new InvestigateMsg(this->buffer);
-      Process(remoteParticipant, msg);
+      Process(sender, msg);
       delete msg;
     } break;
     case ThingMsg::id: {
       ThingMsg* msg = new ThingMsg(this->buffer);
-      Process(remoteParticipant, msg);
+      bufferSize -= msg->length;
+      Process(sender, msg);
       delete msg;
     } break;
     case NameMsg::id: {
       NameMsg* msg = new NameMsg(this->buffer);
-      Process(remoteParticipant, msg);
+      bufferSize -= msg->length + msg->nameLength;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case ModelUrlMsg::id: {
+      ModelUrlMsg* msg = new ModelUrlMsg(this->buffer);
+      bufferSize -= msg->length + msg->urlLength;
+      Process(sender, msg);
       delete msg;
     } break;
     case PoseMsg::id: {
       PoseMsg* msg = new PoseMsg(this->buffer);
-      Process(remoteParticipant, msg);
+      bufferSize -= msg->length;
+      Process(sender, msg);
       delete msg;
     } break;
     case BinaryMsg::id: {
       BinaryMsg* msg = new BinaryMsg(this->buffer);
-      Process(remoteParticipant, msg);
+      bufferSize -= msg->length + msg->dataLength;
+      Process(sender, msg);
       delete msg;
     } break;
   };
+
+  // Check if the buffer has been read completely
+  if (bufferSize > 0)
+    std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
 }
 
-void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {}
+void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
+  std::cout << this->name << ": Process Participant " << (int)msg->networkId
+            << "\n";
+}
 
 void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
   std::cout << this->name << ": process Site Id " << (int)this->networkId
@@ -333,9 +349,9 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
 
 void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
   std::cout << this->name << ": process Name [" << (int)msg->networkId << "/"
-            << (int)msg->thingId << "]\n";
+            << (int)msg->thingId << "] ";
 
-  Thing* thing = sender->Get(msg->networkId, msg->thingId);
+  Thing* thing = sender->Get(msg->thingId);
   if (thing != nullptr) {
     int nameLength = msg->nameLength;
     int stringLen = nameLength + 1;
@@ -347,15 +363,19 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
     // Use strncpy with bounds checking for other platforms (Arduino, POSIX,
     // ESP-IDF)
     strncpy(thingName, msg->name,
-            stringLen - 1);           // Leave space for null terminator
+            nameLength);  // Leave space for null terminator
-    thingName[stringLen - 1] = '\0';  // Ensure null termination
 #endif
     thingName[nameLength] = '\0';
     thing->name = thingName;
-    // std::cout << "thing name = " << thing->name << " length = " <<
+
-    // nameLength
+    std::cout << thing->name;
-    //           << "\n";
   }
+  std::cout << std::endl;
+}
+
+void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
+  std::cout << this->name << ": process model url [" << (int)msg->networkId
+            << "/" << (int)msg->thingId << "]\n";
 }
 
 void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
@@ -365,18 +385,15 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
 
 void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
   std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/"
-            << (int)msg->thingId << "]\n";
+            << (int)msg->thingId << "] ";
-  Thing* thing = sender->Get(msg->networkId, msg->thingId);
+  Thing* thing = sender->Get(msg->thingId);
   if (thing != nullptr)
-    thing->ProcessBinary(msg->bytes);
+    thing->ProcessBinary(msg->data);
   else {
-    thing = this->Get(msg->networkId, msg->thingId);
+    std::cout << "  unknown thing [" << (int)msg->networkId << "/"
-    if (thing != nullptr)
+              << (int)msg->thingId << "]";
-      thing->ProcessBinary(msg->bytes);
-    // else
-    //   std::cout << "custom msg for unknown thing [" << (int)msg->networkId
-    //             << "/" << (int)msg->thingId << "]\n";
   }
+  std::cout << std::endl;
 }
 
 // Receive

Participants/ParticipantUDP.h

@@ -136,6 +136,7 @@ class ParticipantUDP : public Participant {
   virtual void Process(Participant* sender, InvestigateMsg* msg);
   virtual void Process(Participant* sender, ThingMsg* msg);
   virtual void Process(Participant* sender, NameMsg* msg);
+  virtual void Process(Participant* sender, ModelUrlMsg* msg);
   virtual void Process(Participant* sender, PoseMsg* msg);
   virtual void Process(Participant* sender, BinaryMsg* msg);
 

Participants/SiteServer.cpp

@@ -25,7 +25,7 @@ SiteServer::SiteServer(int port) {
   SetupUDP(port, ipAddress, 0);
 
 #if !defined(NO_STD)
-  //Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
+  // Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
 #endif
 }
 
@@ -43,22 +43,23 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
 void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
 
 void SiteServer::Process(Participant* sender, ThingMsg* msg) {
-  Thing* thing = sender->Get(msg->networkId, msg->thingId);
+  Thing* thing = sender->Get(msg->thingId);
   if (thing == nullptr) {
-// #if defined(NO_STD)
+    // #if defined(NO_STD)
-    new Thing(sender, msg->networkId, msg->thingId,
+    new Thing(sender, (Thing::Type)msg->thingType,
-              (Thing::Type)msg->thingType);
+              msg->thingId);
-// #else
+    // #else
-//     auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
+    //     auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
-//     //Thing* newThing;
+    //     //Thing* newThing;
-//     if (thingMsgProcessor != thingMsgProcessors.end())  // found item
+    //     if (thingMsgProcessor != thingMsgProcessors.end())  // found item
-//       //newThing =
+    //       //newThing =
-//           thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
+    //           thingMsgProcessor->second(sender, msg->networkId,
-//     else
+    //           msg->thingId);
-//       //newThing = 
+    //     else
-//         new Thing(sender, msg->networkId, msg->thingId,
+    //       //newThing =
-//                            (Thing::Type)msg->thingType);
+    //         new Thing(sender, msg->networkId, msg->thingId,
-// #endif
+    //                            (Thing::Type)msg->thingType);
+    // #endif
   }
 }
 

Thing.cpp

@@ -1,5 +1,6 @@
 #include "Thing.h"
 
+#include "Messages/PoseMsg.h"
 #include "Participant.h"
 #include "Participants/IsolatedParticipant.h"
 
@@ -22,14 +23,15 @@ namespace RoboidControl {
 //   return isolatedParticipant;
 // }
 
-Thing::Thing(int thingType) : Thing(IsolatedParticipant::Isolated(), thingType) {}
+Thing::Thing(int thingType)
+    : Thing(IsolatedParticipant::Isolated(), thingType) {}
 
-Thing::Thing(Participant* owner, Type thingType)
+Thing::Thing(Participant* owner, Type thingType, unsigned char thingId)
-    : Thing(owner, (unsigned char)thingType) {}
+    : Thing(owner, (unsigned char)thingType, thingId) {}
 
-Thing::Thing(Participant* owner, int thingType) {
+Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
   this->owner = owner;
-  this->id = 0;
+  this->id = thingId;
   this->type = thingType;
   this->networkId = 0;
 
@@ -39,27 +41,27 @@ Thing::Thing(Participant* owner, int thingType) {
   this->linearVelocity = Spherical::zero;
   this->angularVelocity = Spherical::zero;
 
-  // std::cout << "add thing to participant\n";
+  // std::cout << "add thing [" << (int)this->id << "] to owner "
-  owner->Add(this);
+  //           << this->owner->ipAddress << ":" << this->owner->port << std::endl;
+  this->owner->Add(this, false);
 }
 
-Thing::Thing(Participant* owner,
+// Thing::Thing(Participant* owner,
-             unsigned char networkId,
+//             Type thingType,
-             unsigned char thingId,
+//              int thingId) {
-             Type thingType) {
+//   // no participant reference yet..
-  // no participant reference yet..
+//   this->owner = owner;
-  this->owner = owner;
+//   this->networkId = networkId;
-  this->networkId = networkId;
+//   this->id = thingId;
-  this->id = thingId;
+//   this->type = (unsigned char)thingType;
-  this->type = (unsigned char)thingType;
 
-  this->linearVelocity = Spherical::zero;
+//   this->linearVelocity = Spherical::zero;
-  this->angularVelocity = Spherical::zero;
+//   this->angularVelocity = Spherical::zero;
-  // std::cout << "Created thing " << (int)this->networkId << "/" <<
+//   // std::cout << "Created thing " << (int)this->networkId << "/" <<
-  // (int)this->id
+//   // (int)this->id
-  //           << "\n";
+//   //           << "\n";
-  owner->Add(this, false);
+//   owner->Add(this, false);
-}
+// }
 
 void Thing::Terminate() {
   // Thing::Remove(this);
@@ -190,7 +192,11 @@ void Thing::Update(bool recursive) {
 }
 
 void Thing::Update(unsigned long currentTimeMs, bool recursive) {
-  (void)currentTimeMs;
+  // if (this->positionUpdated || this->orientationUpdated)
+  //   OnPoseChanged callback
+  this->positionUpdated = false;
+  this->orientationUpdated = false;
+
   if (recursive) {
     for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
       Thing* child = this->children[childIx];
@@ -205,9 +211,10 @@ void Thing::UpdateThings(unsigned long currentTimeMs) {
   IsolatedParticipant::Isolated()->Update(currentTimeMs);
 }
 
-void Thing::GenerateBinary(char* buffer, unsigned char* ix) {
+int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
   (void)buffer;
   (void)ix;
+  return 0;
 }
 void Thing::ProcessBinary(char* bytes) {
   (void)bytes;

Thing.h

@@ -43,19 +43,21 @@ class Thing {
   Thing(int thingType = Type::Undetermined);
   /// @brief Create a new thing of the given type
   /// @param thingType The predefined type of thing
-  Thing(Participant* participant, Type thingType = Type::Undetermined);
+  Thing(Participant* participant,
+        Type thingType = Type::Undetermined,
+        unsigned char thingId = 0);
   /// @brief Create a new thing of the give type
   /// @param thingType The custom type of the thing
-  Thing(Participant* participant, int thingType);
+  Thing(Participant* participant, int thingType, unsigned char thingId = 0);
   /// @brief Create a new thing for the given participant
   /// @param participant The participant for which this thing is created
   /// @param networkId The network ID of the thing
   /// @param thingId The ID of the thing
   /// @param thingType The type of thing
-  Thing(Participant* participant,
+  // Thing(Participant* participant,
-        unsigned char networkId,
+  //       unsigned char networkId,
-        unsigned char thingId,
+  //       unsigned char thingId,
-        Type thingType = Type::Undetermined);
+  //       Type thingType = Type::Undetermined);
 
   /// @brief The participant managing this thing
   Participant* owner;
@@ -192,7 +194,8 @@ class Thing {
   /// @brief Function used to generate binary data for this thing
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  virtual void GenerateBinary(char* buffer, unsigned char* ix);
+  /// @returns The size of the binary data
+  virtual int GenerateBinary(char* buffer, unsigned char* ix);
   // /// @brief FUnction used to process binary data received for this thing
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes);

Things/TemperatureSensor.cpp

@@ -5,17 +5,18 @@
 namespace RoboidControl {
 
 TemperatureSensor::TemperatureSensor(Participant* participant,
-                                     unsigned char networkId,
                                      unsigned char thingId)
-    : Thing(participant, networkId, thingId, Type::TemperatureSensor) {}
+    : Thing(participant, Type::TemperatureSensor, thingId) {}
 
 void TemperatureSensor::SetTemperature(float temp) {
   this->temperature = temp;
 }
 
-void TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
+int TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
+  unsigned char startIx = *ix;
   // std::cout << "Send temperature: " << this->temperature << "\n";
   LowLevelMessages::SendFloat16(buffer, ix, this->temperature);
+  return *ix - startIx;
 }
 
 void TemperatureSensor::ProcessBinary(char* bytes) {

Things/TemperatureSensor.h

@@ -15,7 +15,7 @@ class TemperatureSensor : public Thing {
   /// @brief Create a temperature sensor with the given ID
   /// @param networkId The network ID of the sensor
   /// @param thingId The ID of the thing
-  TemperatureSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
+  TemperatureSensor(Participant* participant, unsigned char thingId);
 
   /// @brief Manually override the measured temperature
   /// @param temperature The new temperature
@@ -24,7 +24,7 @@ class TemperatureSensor : public Thing {
   /// @brief Function to create a binary message with the temperature
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  void GenerateBinary(char* bytes, unsigned char* ix) override;
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
   /// @brief Function to extract the temperature received in the binary message
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes) override;

Things/TouchSensor.cpp

@@ -10,7 +10,9 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
   this->SetParent(parent);
 }
 
-void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
+int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
+  return 0;
+}
 
 void TouchSensor::ProcessBinary(char* bytes) {
   // if (bytes[0] == 1)

Things/TouchSensor.h

@@ -25,7 +25,7 @@ class TouchSensor : public Thing {
   /// @brief Function to create a binary message with the temperature
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  void GenerateBinary(char* bytes, unsigned char* ix) override;
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
   /// @brief Function to extract the temperature received in the binary message
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes) override;