Arduino/ArduinoParticipant.cpp

@@ -1,9 +1,4 @@
 #include "ArduinoParticipant.h"
-#if defined(ARDUINO)
-
-#if !defined(NO_STD)
-#include <iostream>
-#endif
 
 #if defined(ARDUINO)
 #if defined(ARDUINO_ARCH_ESP8266)
@@ -22,20 +17,19 @@
 #endif
 
 namespace RoboidControl {
+namespace Arduino {
 
+void ParticipantUDP::Setup(int localPort,
+                             const char* remoteIpAddress,
+                             int remotePort) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-WiFiUDP* udp;
+  this->remoteIpAddress = remoteIpAddress;
-#endif
+  this->remotePort = remotePort;
-
-void ParticipantUDP::Setup() {
-#if defined(ARDUINO) && defined(HAS_WIFI)
   GetBroadcastAddress();
 
 #if defined(UNO_R4)
   if (WiFi.status() == WL_NO_MODULE) {
-#if !defined(NO_STD)
     std::cout << "No network available!\n";
-#endif
     return;
   }
 #else
@@ -44,16 +38,9 @@ void ParticipantUDP::Setup() {
     return;
   }
 #endif
+  udp.begin(localPort);
 
-  udp = new WiFiUDP();
+  std::cout << "Wifi sync started to port " << this->remotePort << "\n";
-  udp->begin(this->port);
-
-#if !defined(NO_STD)
-  std::cout << "Wifi sync started local " << this->port;
-  if (this->remoteSite != nullptr)
-    std::cout << ", remote " << this->remoteSite->ipAddress << ":"
-              << this->remoteSite->port << "\n";
-#endif
 #endif
 }
 
@@ -65,51 +52,55 @@ void ParticipantUDP::GetBroadcastAddress() {
   this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
   broadcastIpString.toCharArray(this->broadcastIpAddress,
                                 broadcastIpString.length() + 1);
-#if !defined(NO_STD)
   std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
 #endif
-#endif
 }
 
 void ParticipantUDP::Receive() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  int packetSize = udp->parsePacket();
+  int packetSize = udp.parsePacket();
   while (packetSize > 0) {
-    udp->read(buffer, packetSize);
+    udp.read(buffer, packetSize);
 
-    String senderAddress = udp->remoteIP().toString();
+    String senderAddress = udp.remoteIP().toString();
     char sender_ipAddress[16];
     senderAddress.toCharArray(sender_ipAddress, 16);
-    unsigned int sender_port = udp->remotePort();
+    unsigned int sender_port = udp.remotePort();
 
+    // Participant* 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";
+    // }
+
+    // ReceiveData(packetSize, remoteParticipant);
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    packetSize = udp->parsePacket();
+    packetSize = udp.parsePacket();
   }
 #endif
 }
 
-bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
   // << remoteParticipant->port << "\n";
 
   int n = 0;
-  int r = 0;
   do {
     if (n > 0) {
-#if !defined(NO_STD)
       std::cout << "Retry sending\n";
-#endif
       delay(10);
     }
     n++;
-
+    udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
-    udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
+    udp.write((unsigned char*)buffer, bufferSize);
-    udp->write((unsigned char*)buffer, bufferSize);
+  } while (udp.endPacket() == 0 && n < 10);
-    r = udp->endPacket();
-    // On an Uno R4 WiFi, endPacket blocks for 10 seconds the first time
-    // It is not cleary yet why
-  } while (r == 0 && n < 10);
 
 #endif
   return true;
@@ -121,9 +112,9 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   if (bufferSize <= 0)
     return true;
 
-  udp->beginPacket(this->broadcastIpAddress, this->port);
+  udp.beginPacket(this->broadcastIpAddress, this->remotePort);
-  udp->write((unsigned char*)buffer, bufferSize);
+  udp.write((unsigned char*)buffer, bufferSize);
-  udp->endPacket();
+  udp.endPacket();
 
   //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
   //             << this->remotePort << "\n";
@@ -131,5 +122,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   return true;
 };
 
+}  // namespace Arduino
 }  // namespace RoboidControl
-#endif

Arduino/ArduinoParticipant.h

@@ -1,22 +1,32 @@
 #pragma once
-#if defined(ARDUINO)
 
 #include "Participants/ParticipantUDP.h"
 
-namespace RoboidControl {
+#if defined(HAS_WIFI)
+#include <WiFiUdp.h>
+#endif
 
-class ParticipantUDP : public ParticipantUDPGeneric {
+namespace RoboidControl {
+namespace Arduino {
+
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
-  void Setup();
+  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:
+#if defined(HAS_WIFI)
+  const char* remoteIpAddress = nullptr;
+  unsigned short remotePort = 0;
   char* broadcastIpAddress = nullptr;
 
+  WiFiUDP udp;
+#endif
+
   void GetBroadcastAddress();
 };
 
+}  // namespace Arduino
 }  // namespace RoboidControl
-#endif

Arduino/Examples/BB2A/BB2A.cpp

@@ -1,95 +0,0 @@
-#include "Arduino.h"
-
-#include "Things/ControlledMotor.h"
-#include "Things/DifferentialDrive.h"
-#include "Things/RelativeEncoder.h"
-#include "Things/TouchSensor.h"
-
-#include "Arduino/Things/DRV8833.h"
-#include "Arduino/Things/DigitalInput.h"
-#include "Arduino/Things/UltrasonicSensor.h"
-#include "Arduino/ArduinoUtils.h"
-
-#include "Participants/ParticipantUDP.h"
-
-#include "configuration.h"
-
-#include <iostream>
-
-using namespace RoboidControl;
-using namespace RoboidControl::Arduino;
-
-ParticipantUDPGeneric* localParticipant;
-
-DifferentialDrive* bb2b;
-TouchSensor* touchLeft;
-TouchSensor* touchRight;
-
-// RelativeEncoder* encoderLeft;
-// RelativeEncoder* encoderRight;
-
-void setup() {
-  Serial.begin(115200);
-  delay(3000);
-  Serial.println("started");
-
-  StartWifi("serrarens", "192.168.76.44");
-  localParticipant = new ParticipantUDPGeneric("192.168.77.76");
-
-  bb2b = new DifferentialDrive();
-  touchLeft = new TouchSensor(bb2b);
-  touchRight = new TouchSensor(bb2b);
-  
-  // bb2b = new DRV8833::DifferentialDrive(driveConfig);
-  // touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b);
-  // touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b);
-
-  touchLeft->name = "Left Touch Sensor";
-  touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0));
-  touchRight->name = "Right Touch Sensor";
-  touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0));
-
-  // encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig);
-  // encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig);
-
-  // bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b);
-  // bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b);
-}
-
-void loop() {
-  
-  // std::cout << touchLeft->touchedSomething << " | "
-  //           << touchRight->touchedSomething << std::endl;
-  // std::cout << encoderLeft->rotationSpeed << " : "
-  //           << encoderRight->rotationSpeed << std::endl;
-  // std::cout << bb2b->leftWheel->encoder->rotationSpeed
-  //           << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl;
-
-  
-  // The left wheel turns forward when nothing is touched on the right side
-  // and turn backward when the roboid hits something on the right
-  float leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f;
-  // The right wheel does the same, but instead is controlled by
-  // touches on the left side
-  float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f;
-  // When both sides are touching something, both wheels will turn backward
-  // and the roboid will move backwards
-
-  bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity);
-  bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity);
-  // std::cout << "  " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl;
-
-  // float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f;
-  // float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f;
-  // bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity);
-  // std::cout << "  " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl;
-
-  // std::cout << leftMotor->actualVelocity << std::endl;
-  
-  //Serial.println(".");
-  // Update the roboid state
-  localParticipant->Update();
-
-  // and sleep for 100ms
-  delay(10);
-}

Arduino/Examples/BB2A/configuration.h

@@ -1,52 +0,0 @@
-#pragma once
-
-#include "Arduino/Things/UltrasonicSensor.h"
-#include "Arduino/Things/DRV8833.h"
-#include "Arduino/Things/DigitalInput.h"
-
-using namespace RoboidControl::Arduino;
-
-#if defined(ESP32)
-static constexpr DRV8833::Configuration driveConfig = {
-    17,  // AIn1
-    16,  // AIn2
-    14,  // BIn1
-    27   // BIn2
-};
-static constexpr UltrasonicSensor::Configuration leftTouchConfig = {
-    25,  // Trigger
-    33   // Echo
-};
-static constexpr UltrasonicSensor::Configuration rightTouchConfig = {
-    15,  // Trigger
-    5    // Echo
-};
-
-#elif defined(UNO) || defined(UNO_R4)
-static constexpr DRV8833::Configuration driveConfig = {
-    5,  // AIn1
-    6,  // AIn2
-    7,  // BIn1
-    10  // BIn2
-};
-
-static constexpr UltrasonicSensor::Configuration leftTouchConfig = {
-    A0,  // Trigger
-    12   // Echo
-};
-static constexpr UltrasonicSensor::Configuration rightTouchConfig = {
-    A1,  // Trigger
-    11   // Echo
-};
-
-static constexpr DigitalInput::RelativeEncoder::Configuration
-    leftEncoderConfig = {
-        2,  // Input pin
-        80  // Pulses per revolution
-};
-static constexpr DigitalInput::RelativeEncoder::Configuration
-    rightEncoderConfig = {
-        3,  // Input pin
-        80  // Pulses per revolution
-};
-#endif

Arduino/Things/DRV8833.cpp

@@ -5,83 +5,49 @@
 namespace RoboidControl {
 namespace Arduino {
 
-#pragma region DRV8833
+DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
-
+    : Thing(participant) {
-DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) {
-  this->type = Type::Undetermined;
-  this->name = "DRV8833";
-  this->pinStandby = config.standby;
-  if (pinStandby != 255)
-    pinMode(pinStandby, OUTPUT);
-
-  this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
-  this->motorA->SetName("Motor A");
-  this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
-  this->motorB->SetName("Motor B");
-}
-
-#pragma endregion DRV8833
-
-#pragma region Differential drive
-
-DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
-                                              Thing* parent)
-    : RoboidControl::DifferentialDrive(nullptr, nullptr, parent) {
-  this->drv8833 = new DRV8833(config, this);
-  this->leftWheel = this->drv8833->motorA;
-  this->rightWheel = this->drv8833->motorB;
-}
-
-void DRV8833::DifferentialDrive::Update(bool recurse) {
-  RoboidControl::DifferentialDrive::Update(recurse);
-  this->drv8833->Update(false);
-}
-
-#pragma endregion Differential drive
-
-#pragma region Motor
-
-#if (ESP32)
-uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
-#endif
-
-DRV8833Motor::DRV8833Motor(DRV8833* driver,
-                           unsigned char pinIn1,
-                           unsigned char pinIn2,
-                           bool reverse)
-    : Motor(driver) {
-
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
 
 #if (ESP32)
-  in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
+  in1Ch = nextAvailablePwmChannel++;
   ledcSetup(in1Ch, 500, 8);
   ledcAttachPin(pinIn1, in1Ch);
-
+  in2Ch = nextAvailablePwmChannel++;
-  in2Ch = DRV8833Motor::nextAvailablePwmChannel++;
   ledcSetup(in2Ch, 500, 8);
   ledcAttachPin(pinIn2, in2Ch);
-
 #else
   pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
   pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
 #endif
 
-  // this->reverse = reverse;
+  this->reverse = reverse;
 }
 
-// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
+void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
-//   this->maxRpm = rpm;
+  this->maxRpm = rpm;
-// }
+}
 
-void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
+void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
-  Motor::SetTargetVelocity(motorSpeed);
+  Thing::SetAngularVelocity(velocity);
+  // ignoring rotation axis for now.
+  // Spherical angularVelocity = this->GetAngularVelocity();
+  float angularSpeed = velocity.distance;  // in degrees/sec
 
-  uint8_t motorSignal =
+  float rpm = angularSpeed / 360 * 60;
-      (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
+  float motorSpeed = rpm / this->maxRpm;
 
-  // std::cout << "moto speed " << this->name << " = " << motorSpeed
+  uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
+  // if (direction == RotationDirection::CounterClockwise)
+  //   speed = -speed;
+  // Determine the rotation direction
+  if (velocity.direction.horizontal.InDegrees() < 0)
+    motorSpeed = -motorSpeed;
+  if (this->reverse)
+    motorSpeed = -motorSpeed;
+
+  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
   //           << ", motor signal = " << (int)motorSignal << "\n";
 
 #if (ESP32)
@@ -131,7 +97,24 @@ void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
 #endif
 }
 
-#pragma endregion Motor
+DRV8833::DRV8833(Participant* participant,
+                 unsigned char pinAIn1,
+                 unsigned char pinAIn2,
+                 unsigned char pinBIn1,
+                 unsigned char pinBIn2,
+                 unsigned char pinStandby,
+                 bool reverseA,
+                 bool reverseB)
+    : Thing(participant) {
+  this->pinStandby = pinStandby;
+  if (pinStandby != 255)
+    pinMode(pinStandby, OUTPUT);
+
+  this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA);
+  this->motorA->name = "Motor A";
+  this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB);
+  this->motorB->name = "Motor B";
+}
 
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DRV8833.h

@@ -1,87 +1,60 @@
 #pragma once
 
-#include <Arduino.h>
-#include "Participants/IsolatedParticipant.h"
 #include "Thing.h"
 #include "Things/DifferentialDrive.h"
-#include "Things/Motor.h"
 
 namespace RoboidControl {
 namespace Arduino {
 
-class DRV8833Motor;
+/// @brief Support for a DRV8833 motor controller
+class DRV8833Motor : public Thing {
+ public:
+  /// @brief Motor turning direction
+  enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
+
+  /// @brief Setup the DC motor
+  /// @param pinIn1 the pin number for the in1 signal
+  /// @param pinIn2 the pin number for the in2 signal
+  /// @param direction the forward turning direction of the motor
+  DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false);
+  void SetMaxRPM(unsigned int rpm);
+
+  virtual void SetAngularVelocity(Spherical velocity) override;
+
+  bool reverse = false;
+
+ protected:
+  unsigned char pinIn1 = 255;
+  unsigned char pinIn2 = 255;
+  unsigned int maxRpm = 200;
+};
 
 class DRV8833 : public Thing {
  public:
-  struct Configuration {
-    int AIn1;
-    int AIn2;
-    int BIn1;
-    int BIn2;
-    int standby = 255;
-
-    constexpr Configuration(int a1, int a2, int b1, int b2, int standby = 255) : AIn1(a1), AIn2(a2), BIn1(b1), BIn2(b2), standby(standby) {}
-  };
-
   /// @brief Setup a DRV8833 motor controller
-  DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());
+  /// @param pinAIn1 The pin number connected to the AIn1 port
+  /// @param pinAIn2 The pin number connected to the AIn2 port
+  /// @param pinBIn1 The pin number connected to the BIn1 port
+  /// @param pinBIn2 The pin number connceted to the BIn2 port
+  /// @param pinStandby The pin number connected to the standby port, 255
+  /// indicated that the port is not connected
+  /// @param reverseA The forward turning direction of motor A
+  /// @param reverseB The forward turning direction of motor B
+  DRV8833(Participant* participant,
+          unsigned char pinAIn1,
+          unsigned char pinAIn2,
+          unsigned char pinBIn1,
+          unsigned char pinBIn2,
+          unsigned char pinStandby = 255,
+          bool reverseA = false,
+          bool reverseB = false);
 
   DRV8833Motor* motorA = nullptr;
   DRV8833Motor* motorB = nullptr;
 
  protected:
   unsigned char pinStandby = 255;
-
- public:
-  class DifferentialDrive;
 };
 
-#pragma region Differential drive
-
-class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
- public:
-  DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot());
-
-  virtual void Update(bool recurse = false) override;
-
- protected:
-  DRV8833* drv8833 = nullptr;
-};
-
-#pragma endregion Differential drive
-
-#pragma region Motor
-
-/// @brief Support for a DRV8833 motor controller
-class DRV8833Motor : public Motor {
- public:
-  /// @brief Setup the DC motor
-  /// @param pinIn1 the pin number for the in1 signal
-  /// @param pinIn2 the pin number for the in2 signal
-  /// @param direction the forward turning direction of the motor
-  DRV8833Motor(DRV8833* driver,
-               unsigned char pinIn1,
-               unsigned char pinIn2,
-               bool reverse = false);
-  // void SetMaxRPM(unsigned int rpm);
-
-  // virtual void SetAngularVelocity(Spherical velocity) override;
-  virtual void SetTargetVelocity(float targetSpeed) override;
-  // bool reverse = false;
-
- protected:
-  unsigned char pinIn1 = 255;
-  unsigned char pinIn2 = 255;
-  // unsigned int maxRpm = 200;
-
-#if (ESP32)
-  uint8_t in1Ch;
-  uint8_t in2Ch;
-  static uint8_t nextAvailablePwmChannel;
-#endif
-};
-
-#pragma endregion Motor
-
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DigitalInput.cpp

@@ -5,126 +5,19 @@
 namespace RoboidControl {
 namespace Arduino {
 
-#pragma region Digital input
+DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
-
+    : TouchSensor(participant) {
-DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(parent) {
-  this->type = Type::Switch;
-  this->name = "Digital Input";
   this->pin = pin;
-  pinMode(this->pin, INPUT);
+
-  std::cout << "digital input start\n";
+  pinMode(pin, INPUT);
 }
 
-void DigitalInput::Update(bool recursive) {
+void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) {
-  this->isHigh = digitalRead(this->pin);
+  this->touchedSomething = digitalRead(pin) == LOW;
-  this->isLow = !this->isHigh;
+  // std::cout << "DigitalINput pin " << (int)this->pin << ": " <<
-  Thing::Update(recursive);
+  // this->touchedSomething << "\n";
+  Thing::Update(currentTimeMs, recursive);
 }
 
-#pragma endregion Digital input
-
-#pragma region Touch sensor
-
-DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
-    : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
-
-void DigitalInput::TouchSensor::Update(bool recursive) {
-  this->internalTouch = digitalInput.isLow;
-}
-
-#pragma endregion Touch sensor
-
-#pragma region Relative encoder
-
-int DigitalInput::RelativeEncoder::interruptCount = 0;
-volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
-volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
-
-DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
-                                               Thing* parent)
-    : RoboidControl::RelativeEncoder(parent),
-      digitalInput(config.pin, parent),
-      pulsesPerRevolution(config.pulsesPerRevolution) {}
-
-void DigitalInput::RelativeEncoder::Start() {
-  // We support up to 2 pulse counters
-  if (interruptCount == 0) {
-    std::cout << "pin interrupt 1 activated" << std::endl;
-    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0,
-                    RISING);
-  } else if (interruptCount == 1) {
-    std::cout << "pin interrupt 2 activated" << std::endl;
-    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1,
-                    RISING);
-  } else {
-    // maximum interrupt count reached
-    std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached"
-              << std::endl;
-    return;
-  }
-  interruptIx = interruptCount;
-  interruptCount++;
-  std::cout << "pin ints. " << interruptCount << std::endl;
-}
-
-int DigitalInput::RelativeEncoder::GetPulseCount() {
-  if (interruptIx == 0) {
-    int pulseCount = pulseCount0;
-    pulseCount0 = 0;
-    return pulseCount;
-  } else if (interruptIx == 1) {
-    int pulseCount = pulseCount1;
-    pulseCount1 = 0;
-    return pulseCount;
-  }
-  return 0;
-}
-
-void DigitalInput::RelativeEncoder::Update(bool recursive) {
-  unsigned long currentTimeMs = GetTimeMs();
-  if (this->lastUpdateTime == 0) {
-    this->Start();
-    this->lastUpdateTime = currentTimeMs;
-    return;
-  }
-
-  float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f;
-  if (timeStep <= 0)
-    return;
-
-  int pulseCount = GetPulseCount();
-  netPulseCount += pulseCount;
-
-  this->pulseFrequency = pulseCount / timeStep;
-  this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
-
-  // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
-  //           << " timestep " << timeStep << std::endl;
-
-  this->lastUpdateTime = currentTimeMs;
-}
-
-#if defined(ESP8266) || defined(ESP32)
-void ICACHE_RAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt0() {
-  pulseCount0++;
-}
-#else
-void DigitalInput::RelativeEncoder::PulseInterrupt0() {
-  pulseCount0++;
-}
-#endif
-
-#if defined(ESP8266) || defined(ESP32)
-void IRAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt1() {
-  pulseCount1++;
-}
-#else
-void DigitalInput::RelativeEncoder::PulseInterrupt1() {
-  pulseCount1++;
-}
-#endif
-
-#pragma endregion Relative encoder
-
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DigitalInput.h

@@ -1,92 +1,26 @@
 #pragma once
 
-#include "Things/RelativeEncoder.h"
 #include "Things/TouchSensor.h"
 
 namespace RoboidControl {
 namespace Arduino {
 
 /// @brief A digital input represents the stat of a digital GPIO pin
-class DigitalInput : public Thing {
+class DigitalInput : public TouchSensor {
  public:
   /// @brief Create a new digital input
   /// @param participant The participant to use
   /// @param pin The digital pin
-  //DigitalInput(Participant* participant, unsigned char pin);
+  DigitalInput(Participant* participant, unsigned char pin);
-  // DigitalInput(Thing* parent, unsigned char pin);
-  DigitalInput(unsigned char pin, Thing* parent);
-
-  bool isHigh = false;
-  bool isLow = false;
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(bool recursive = false) override;
+  virtual void Update(unsigned long currentTimeMs,
+                      bool recursive = false) override;
 
  protected:
   /// @brief The pin used for digital input
   unsigned char pin = 0;
-
- public:
-  class TouchSensor;
-  class RelativeEncoder;
 };
 
-#pragma region Touch sensor
-
-class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
- public:
-  TouchSensor(unsigned char pin, Thing* parent);
-
-  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(bool recurse = false) override;
-
- protected:
-  DigitalInput digitalInput;
-};
-
-#pragma endregion Touch sensor
-
-#pragma region Incremental encoder
-
-class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
- public:
-  struct Configuration {
-    unsigned char pin;
-    unsigned char pulsesPerRevolution;
-  };
-
-  RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot());
-
-  unsigned char pulsesPerRevolution;
-
-  /// @brief The current pulse frequency in Hz
-  float pulseFrequency = 0;
-
-  /// @copydoc RoboidControl::Thing::Update()
-  virtual void Update(bool recurse = false) override;
-
- protected:
-  DigitalInput digitalInput;
-
-  int interruptIx = 0;
-
-  static int interruptCount;
-
-  volatile static int pulseCount0;
-  static void PulseInterrupt0();
-
-  volatile static int pulseCount1;
-  static void PulseInterrupt1();
-
-  int GetPulseCount();
-  long netPulseCount = 0;
-  unsigned long lastUpdateTime = 0;
-
- private:
-  void Start();
-};
-
-#pragma endregion Incremental encoder
-
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/UltrasonicSensor.cpp

@@ -1,17 +1,16 @@
 #include "UltrasonicSensor.h"
 
 #include <Arduino.h>
-#include <iostream>
 
 namespace RoboidControl {
 namespace Arduino {
 
-UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
+UltrasonicSensor::UltrasonicSensor(Participant* participant,
-    : Thing(parent) {
+                                   unsigned char pinTrigger,
-  this->type = Type::DistanceSensor;
+                                   unsigned char pinEcho)
-  this->name = "Ultrasonic sensor";
+    : TouchSensor(participant) {
-  this->pinTrigger = config.trigger;
+  this->pinTrigger = pinTrigger;
-  this->pinEcho = config.echo;
+  this->pinEcho = pinEcho;
 
   pinMode(pinTrigger, OUTPUT);  // configure the trigger pin to output mode
   pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
@@ -20,14 +19,14 @@ UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
 float UltrasonicSensor::GetDistance() {
   // Start the ultrasonic 'ping'
   digitalWrite(pinTrigger, LOW);
-  delayMicroseconds(2);
+  delayMicroseconds(5);
   digitalWrite(pinTrigger, HIGH);
   delayMicroseconds(10);
   digitalWrite(pinTrigger, LOW);
 
   // Measure the duration of the pulse on the echo pin
-  unsigned long duration_us =
+  float duration_us =
-      pulseIn(pinEcho, HIGH, 10000);  // the result is in microseconds
+      pulseIn(pinEcho, HIGH, 100000);  // the result is in microseconds
 
   // Calculate the distance:
   // * Duration should be divided by 2, because the ping goes to the object
@@ -38,60 +37,32 @@ float UltrasonicSensor::GetDistance() {
   // * Now we calculate the distance based on the speed of sound (340 m/s):
   // distance = duration_sec * 340;
   // * The result calculation is therefore:
-  this->distance = (float)duration_us / 2 / 1000000 * 340;
+  this->distance = duration_us / 2 / 1000000 * 340;
-
-  // Serial.println(this->distance);
-
-  // std::cout << "US distance " << this->distance << std::endl;
 
   // Filter faulty measurements. The sensor can often give values > 30 m which
   // are not correct
   // if (distance > 30)
   //   distance = 0;
 
-  return this->distance;
+  this->touchedSomething |= (this->distance <= this->touchDistance);
+
+  //   std::cout << "Ultrasonic " << this->distance << " " <<
+  //   this->touchedSomething << "\n";
+
+  return distance;
 }
 
-void UltrasonicSensor::Update(bool recursive) {
+void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
+  this->touchedSomething = false;
   GetDistance();
-  Thing::Update(recursive);
+  Thing::Update(currentTimeMs, recursive);
 }
 
-#pragma region Distance sensor
+// void UltrasonicSensor::ProcessBinary(char* bytes) {
-
+//   this->touchedSomething = (bytes[0] == 1);
-UltrasonicSensor::DistanceSensor::DistanceSensor(
+//   if (this->touchedSomething)
-    UltrasonicSensor::Configuration config,
+//     std::cout << "Touching something!\n";
-    Thing* parent)
+// }
-    : RoboidControl::DistanceSensor(parent), ultrasonic(config, this) {}
-
-void UltrasonicSensor::DistanceSensor::Update(bool recursive) {
-  RoboidControl::DistanceSensor::Update(recursive);
-  this->ultrasonic.Update(false);
-  if (this->ultrasonic.distance > 0)
-    this->internalDistance = this->ultrasonic.distance;
-  else
-#if ARDUNIO
-    this->internalDistance = INFINITY;
-#else
-    this->internalDistance = std::numeric_limits<float>::infinity();
-#endif
-}
-
-#pragma endregion Distance sensor
-
-#pragma region Touch sensor
-
-UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
-    : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}
-
-void UltrasonicSensor::TouchSensor::Update(bool recursive) {
-  RoboidControl::TouchSensor::Update(recursive);
-  this->ultrasonic.Update(false);
-  this->internalTouch = (this->ultrasonic.distance > 0 &&
-                         this->ultrasonic.distance <= this->touchDistance);
-}
-
-#pragma endregion Touch sensor
 
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/UltrasonicSensor.h

@@ -1,25 +1,25 @@
 #pragma once
 
-#include "Things/DistanceSensor.h"
 #include "Things/TouchSensor.h"
 
 namespace RoboidControl {
 namespace Arduino {
 
 /// @brief An HC-SR04 ultrasonic distance sensor
-class UltrasonicSensor : Thing {
+class UltrasonicSensor : public TouchSensor {
  public:
-  struct Configuration {
+  /// @brief  Setup an ultrasonic sensor
-    int trigger;
+  /// @param participant The participant to use
-    int echo;
+  /// @param pinTrigger The pin number of the trigger signal
-  };
+  /// @param pinEcho The pin number of the echo signal
-
+  UltrasonicSensor(Participant* participant,
-  UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
+                   unsigned char pinTrigger,
+                   unsigned char pinEcho);
 
   // parameters
 
   /// @brief The distance at which the object is considered to be touched
-  // float touchDistance = 0.2f;
+  float touchDistance = 0.2f;
 
   // state
 
@@ -31,52 +31,15 @@ class UltrasonicSensor : Thing {
   float GetDistance();
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(bool recursive = false) override;
+  virtual void Update(unsigned long currentTimeMs,
+                      bool recursive = false) override;
 
  protected:
   /// @brief The pin number of the trigger signal
   unsigned char pinTrigger = 0;
   /// @brief The pin number of the echo signal
   unsigned char pinEcho = 0;
-
- public:
-  class DistanceSensor;
-  class TouchSensor;
 };
 
-#pragma region Distance sensor
-
-class UltrasonicSensor::DistanceSensor : public RoboidControl::DistanceSensor {
- public:
-  DistanceSensor(UltrasonicSensor::Configuration config,
-                 Thing* parent = Thing::LocalRoot());
-
-  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(bool recursive = false) override;
-
- protected:
-  UltrasonicSensor ultrasonic;
-};
-
-#pragma endregion Distance sensor
-
-#pragma region Touch sensor
-
-class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
- public:
-  TouchSensor(UltrasonicSensor::Configuration config,
-              Thing* parent = Thing::LocalRoot());
-
-  float touchDistance = 0.2f;
-
-  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(bool recursive = false) override;
-
- protected:
-  UltrasonicSensor ultrasonic;
-};
-
-#pragma endregion Touch sensor
-
 }  // namespace Arduino
 }  // namespace RoboidControl

CMakeLists.txt

@@ -19,27 +19,46 @@ if(ESP_PLATFORM)
         REQUIRES esp_netif esp_wifi
     )
 else() 
+    project(RoboidControl)
+    add_subdirectory(LinearAlgebra)
+    add_subdirectory(Examples)
+
     set(CMAKE_CXX_STANDARD 17)      # Enable c++11 standard
     set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
+    add_compile_definitions(GTEST)
+    include(FetchContent)
+    FetchContent_Declare(
+        googletest
+        DOWNLOAD_EXTRACT_TIMESTAMP ON
+        URL https://github.com/google/googletest/archive/refs/heads/main.zip
+    )
+
+    # For Windows: Prevent overriding the parent project's compiler/linker settings
+    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+    FetchContent_MakeAvailable(googletest)
+
     include_directories(
         .
         LinearAlgebra
     )
     add_library(RoboidControl STATIC ${srcs})
-    target_include_directories(RoboidControl PUBLIC
+    
-        ${CMAKE_CURRENT_SOURCE_DIR}
+    enable_testing()
+
+    file(GLOB_RECURSE test_srcs test/*_test.cc)
+    add_executable(
+        RoboidControlTest
+        ${test_srcs}
+    )
+    target_link_libraries(
+        RoboidControlTest
+        gtest_main
+        RoboidControl
+        LinearAlgebra
     )
 
-    project(RoboidControl)
+    include(GoogleTest)
-    add_subdirectory(LinearAlgebra)
+    gtest_discover_tests(RoboidControlTest)
-
-    # Examples
-    option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF)
-    add_subdirectory(examples)
-
-    enable_testing()
-    add_subdirectory(test)
-
 endif()
 

DoxyGen/Doxyfile

@@ -48,7 +48,7 @@ PROJECT_NAME           = "Roboid Control for C++"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 0.4
+PROJECT_NUMBER         =
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

EspIdf/EspIdfParticipant.cpp

@@ -4,12 +4,13 @@
 #include "esp_wifi.h"
 #endif
 
-#include <arpa/inet.h>
 namespace RoboidControl {
+namespace EspIdf {
 
-void ParticipantUDP::SetupUDP(int localPort,
+void ParticipantUDP::Setup(int localPort,
                            const char* remoteIpAddress,
                            int remotePort) {
+#if defined(IDF_VER)
   std::cout << "Set up UDP\n";
   GetBroadcastAddress();
 
@@ -21,57 +22,42 @@ void ParticipantUDP::SetupUDP(int localPort,
   }
 
   // Create a UDP socket
-  this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+  this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-  if (this->sock < 0) {
+  if (this->sockfd < 0) {
     std::cout << "Unable to create UDP socket: errno " << errno << "\n";
     vTaskDelete(NULL);
     return;
   }
 
-  /*
+  // Set up the server address structure
-  // Set up the receiving(local) address
   struct sockaddr_in local_addr;
   memset(&local_addr, 0, sizeof(local_addr));
   local_addr.sin_family = AF_INET;
-  local_addr.sin_port = htons(localPort);
+  local_addr.sin_port = htons(this->port);
   local_addr.sin_addr.s_addr =
       htonl(INADDR_ANY);  // Listen on all available network interfaces
 
-  // Bind the socket to the receiving address
+  // Bind the socket to the address and port
-  if (bind(this->sock, (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(sock);
+    close(sockfd);
     vTaskDelete(NULL);
     return;
   }
-    */
 
-  // Initialize the destination(remote) address
+  // Initialize the dest_addr structure
   memset(&this->dest_addr, 0,
          sizeof(this->dest_addr));  // Clear the entire structure
   this->dest_addr.sin_family = AF_INET;
   this->dest_addr.sin_port = htons(this->remoteSite->port);
-  this->dest_addr.sin_addr.s_addr = inet_addr(this->remoteSite->ipAddress);
+  inet_pton(AF_INET, this->remoteSite->ipAddress,
-  // inet_pton(AF_INET, this->remoteSite->ipAddress,
+            &this->dest_addr.sin_addr.s_addr);
-  //           &this->dest_addr.sin_addr.s_addr);
 
-    this->connected = true;
+  std::cout << "Wifi sync started local " << this->port << ", remote "
-
-  std::cout << "Wifi sync started local " << localPort << ", remote "
             << this->remoteSite->ipAddress << ":" << this->remoteSite->port
             << "\n";
-
+#endif  // IDF_VER
-  // std::cout << "socket: " << (int)this->sock << std::endl;
-  // ParticipantMsg* msg = new ParticipantMsg(this->networkId);
-  // int bufferSize = msg->Serialize(this->buffer);
-  // int err = sendto(this->sock, buffer, bufferSize, 0,
-  //                  (struct sockaddr*)&dest_addr, sizeof(dest_addr));
-  // if (errno != 0)
-  //   std::cout << "AASend error " << err << " or " << errno << "\n";
-  // else
-  //   std::cout << "AASend SUCCESS\n";
-
-  //SendTest();
 }
 
 void ParticipantUDP::GetBroadcastAddress() {
@@ -97,11 +83,10 @@ void ParticipantUDP::GetBroadcastAddress() {
 #endif  // IDF_VER
 }
 
-void ParticipantUDP::ReceiveUDP() {
+void ParticipantUDP::Receive() {
 #if defined(IDF_VER)
-/*
   struct pollfd fds[1];
-  fds[0].fd = sock;
+  fds[0].fd = sockfd;
   fds[0].events = POLLIN;  // We're looking for data available to read
 
   // Use poll() with a timeout of 0 to return immediately
@@ -118,7 +103,7 @@ void ParticipantUDP::ReceiveUDP() {
   char sender_ipAddress[INET_ADDRSTRLEN];
 
   while (ret > 0 && fds[0].revents & POLLIN) {
-    int packetSize = recvfrom(this->sock, buffer, sizeof(buffer) - 1, 0,
+    int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0,
                               (struct sockaddr*)&source_addr, &addr_len);
     if (packetSize < 0) {
       std::cout << "recvfrom() error\n";
@@ -141,66 +126,18 @@ void ParticipantUDP::ReceiveUDP() {
     }
   }
   // std::cout << "no more messages\n";
-*/
+
 #endif  // IDF_VER
 }
 
-ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
-
-ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
-    : ParticipantUDPGeneric(ipAddress, port, localPort) {}
-
-// bool ParticipantUDP::SendTest() {
-// #if defined(IDF_VER)
-//   // std::cout << "socket: " << (int)this->sock << std::endl;
-//   // UBaseType_t stack_size = uxTaskGetStackHighWaterMark(NULL);  // NULL to check the main task
-//   // size_t free_heap = xPortGetFreeHeapSize();
-//   // std::cout <<  "Stack High Water Mark: " << stack_size << " heap " << free_heap << std::endl;
-
-//   ParticipantMsg* msg = new ParticipantMsg(this->networkId);
-//   int bSize = msg->Serialize(this->buffer);
-//   // std::cout << "buffer size " << bSize << std::endl;
-//   int err = sendto(this->sock, buffer, bSize, 0, (struct sockaddr*)&dest_addr,
-//                    sizeof(dest_addr));
-//   if (errno != 0)
-//     std::cout << "BBSend error " << err << " or " << errno << "\n";
-//   else
-//     std::cout << "BBSend SUCCESS\n";
-
-// #endif
-//   return true;
-// }
-
-bool ParticipantUDP::Send(IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
-  if (bufferSize <= 0)
-    return true;
-
-  std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
-            << " to " << this->remoteSite->ipAddress << std::endl;
-
-  return this->SendTo(this->remoteSite, bufferSize);
-}
-
-bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
-                            int bufferSize) {
 #if defined(IDF_VER)
-  uint16_t port = ntohs(dest_addr.sin_port);
+  // std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
+  //           << remoteParticipant->port << "\n";
 
-  char ip_str[INET_ADDRSTRLEN];
+  int err = sendto(this->sockfd, buffer, bufferSize, 0,
-  inet_ntop(AF_INET, &dest_addr.sin_addr, ip_str, sizeof(ip_str));
+                   (struct sockaddr*)&dest_addr, sizeof(dest_addr));
-  std::cout << "Sending " << bufferSize << " bytes to " << ip_str << ":" << port << "\n";
+  if (errno != 0)
-
-  // Print the IP address and port
-  // printf("IP Address: %s\n", ip_str);
-  // printf("Port: %d\n", port);
-
-  this->dest_addr.sin_port = htons(remoteParticipant->port);
-  this->dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);
-
-  int err = sendto(this->sock, buffer, bufferSize, 0,
-                   (struct sockaddr*)&this->dest_addr, sizeof(this->dest_addr));
-  if (errno < 0)
     std::cout << "Send error " << err << " or " << errno << "\n";
 
 #endif
@@ -217,7 +154,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   dest_addr.sin_family = AF_INET;
   dest_addr.sin_port = htons(this->port);
   inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr);
-  int err = sendto(sock, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
+  int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
                    sizeof(dest_addr));
   if (err != 0)
     std::cout << "Publish error\n";
@@ -225,4 +162,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   return true;
 };
 
+}  // namespace EspIdf
 }  // namespace RoboidControl

EspIdf/EspIdfParticipant.h

@@ -1,47 +1,26 @@
 #pragma once
-#if defined(IDF_VER)
 
 #include "Participants/ParticipantUDP.h"
 
+#if defined(IDF_VER)
 #include "lwip/sockets.h"
+#endif
 
 namespace RoboidControl {
+namespace EspIdf {
 
-class ParticipantUDP : public ParticipantUDPGeneric {
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
-  /// @brief Create a participant without connecting to a site
-  /// @param port The port on which the participant communicates
-  /// These participant typically broadcast Participant messages to let site
-  /// servers on the local network know their presence. Alternatively they can
-  /// broadcast information which can be used directly by other participants.
-  ParticipantUDP(int port = 7681);
-  /// @brief Create a participant which will try to connect to a site.
-  /// @param ipAddress The IP address of the site
-  /// @param port The port used by the site
-  /// @param localPort The port used by the local participant
-  ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
-
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
-  // bool SendTest();
-
-  /// @brief Sens a message to the remote site (if set)
-  /// @param msg The message to send
-  /// @return True if a message could be sent.
-  bool Send(IMessage* msg) override;
-  void SetupUDP(int localPort,
-                const char* remoteIpAddress,
-                int remotePort) override;
-  void ReceiveUDP() override;
-
  protected:
 #if defined(IDF_VER)
   char broadcastIpAddress[INET_ADDRSTRLEN];
 
-  int sock;
+  int sockfd;
   struct sockaddr_in dest_addr;
   // struct sockaddr_in src_addr;
 #endif
@@ -49,6 +28,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
   void GetBroadcastAddress();
 };
 
+}  // namespace EspIdf
 }  // namespace RoboidControl
-
-#endif

Examples/BB2B.cpp

@@ -17,32 +17,32 @@ using namespace RoboidControl;
 
 int main() {
   // The robot's propulsion is a differential drive
-  DifferentialDrive bb2b = DifferentialDrive();
+  DifferentialDrive* bb2b = new DifferentialDrive();
   // Is has a touch sensor at the front left of the roboid
-  TouchSensor touchLeft = TouchSensor(&bb2b); 
+  TouchSensor* touchLeft = new TouchSensor(bb2b);
   // and other one on the right
-  TouchSensor touchRight = TouchSensor(&bb2b);
+  TouchSensor* touchRight = new TouchSensor(bb2b);
 
   // Do forever:
   while (true) {
     // The left wheel turns forward when nothing is touched on the right side
     // and turn backward when the roboid hits something on the right
-    float leftWheelSpeed = (touchRight.IsTouching()) ? -600.0f : 600.0f;
+    float leftWheelSpeed = (touchRight->touchedSomething) ? -600.0f : 600.0f;
     // The right wheel does the same, but instead is controlled by
     // touches on the left side
-    float rightWheelSpeed = (touchLeft.IsTouching()) ? -600.0f : 600.0f;
+    float rightWheelSpeed = (touchLeft->touchedSomething) ? -600.0f : 600.0f;
     // When both sides are touching something, both wheels will turn backward
     // and the roboid will move backwards
-    bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
+    bb2b->SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
 
     // Update the roboid state
-    bb2b.Update(true);
+    bb2b->Update(true);
 
     // and sleep for 100ms
 #if defined(ARDUINO)
-    delay(10);
+    delay(100);
 #else
-    sleep_for(milliseconds(10));
+    sleep_for(milliseconds(100));
 #endif
   }
 

Examples/CMakeLists.txt

@@ -0,0 +1,25 @@
+# examples/CMakeLists.txt
+
+# Specify the minimum CMake version
+cmake_minimum_required(VERSION 3.10)
+
+# Specify the path to the main project directory
+set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..")
+
+# Set the project name
+project(Examples)
+
+include_directories(..)
+
+# Add the executable for the main project
+#add_executable(MainExecutable ${SOURCES})
+# Find the main project library (assuming it's defined in the root CMakeLists.txt)
+#find_package(RoboidControl REQUIRED)  # Replace MyLibrary with your actual library name
+
+# Add example executables
+add_executable(BB2B BB2B.cpp)
+target_link_libraries(
+    BB2B
+    RoboidControl
+    LinearAlgebra
+)

LinearAlgebra/Direction.cpp

@@ -9,7 +9,6 @@
 
 #include <math.h>
 
-namespace LinearAlgebra {
 template <typename T>
 DirectionOf<T>::DirectionOf() {
   this->horizontal = AngleOf<T>();
@@ -99,6 +98,5 @@ void DirectionOf<T>::Normalize() {
   }
 }
 
-template class LinearAlgebra::DirectionOf<float>;
+template class DirectionOf<float>;
-template class LinearAlgebra::DirectionOf<signed short>;
+template class DirectionOf<signed short>;
-}

LinearAlgebra/Direction.h

@@ -99,4 +99,6 @@ using Direction = DirectionSingle;
 
 }  // namespace LinearAlgebra
 
+using namespace LinearAlgebra;
+
 #endif

LinearAlgebra/Matrix.cpp

@@ -1,7 +1,5 @@
 #include "Matrix.h"
-#if !defined(NO_STD) 
 #include <iostream>
-#endif
 
 namespace LinearAlgebra {
 
@@ -63,9 +61,7 @@ Matrix2::Matrix2(const Matrix2& m)
     this->data = nullptr;
   else {
     this->data = new float[this->nValues];
-
+    std::copy(m.data, m.data + nValues, this->data);
-    for (int ix = 0; ix < this->nValues; ++ix)
-      this->data[ix] = m.data[ix];
   }
 }
 
@@ -80,8 +76,7 @@ Matrix2& Matrix2::operator=(const Matrix2& m) {
       this->data = nullptr;
     else {
       this->data = new float[this->nValues];
-      for (int ix = 0; ix < this->nValues; ++ix)
+      std::copy(m.data, m.data + this->nValues, this->data);
-        this->data[ix] = m.data[ix];
     }
   }
   return *this;
@@ -94,8 +89,7 @@ Matrix2::~Matrix2() {
 
 Matrix2 Matrix2::Clone() const {
   Matrix2 r = Matrix2(this->nRows, this->nCols);
-  for (int ix = 0; ix < this->nValues; ++ix)
+  std::copy(this->data, this->data + this->nValues, r.data);
-    r.data[ix] = this->data[ix];
   return r;
 }
 
@@ -139,7 +133,7 @@ Matrix2 Matrix2::Identity(int size) {
 }
 
 Matrix2 Matrix2::Diagonal(float f, int size) {
-  Matrix2 r = Matrix2::Zero(size, size);
+  Matrix2 r = Matrix2(size, size);
   float* data = r.data;
   int valueIx = 0;
   for (int ix = 0; ix < size; ix++) {
@@ -164,8 +158,8 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
 Matrix2 Matrix2::Transpose() const {
   Matrix2 r = Matrix2(this->nCols, this->nRows);
 
-  for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
+  for (uint rowIx = 0; rowIx < this->nRows; rowIx++) {
-    for (int colIx = 0; colIx < this->nCols; colIx++)
+    for (uint colIx = 0; colIx < this->nCols; colIx++)
       r.data[colIx * this->nCols + rowIx] =
           this->data[rowIx * this->nCols + colIx];
   }
@@ -206,16 +200,16 @@ 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;

LinearAlgebra/Polar.cpp

@@ -175,5 +175,5 @@ PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
   return r;
 }
 
-template class LinearAlgebra::PolarOf<float>;
+template class PolarOf<float>;
-template class LinearAlgebra::PolarOf<signed short>;
+template class PolarOf<signed short>;

LinearAlgebra/Spherical.cpp

@@ -5,8 +5,6 @@
 
 #include <math.h>
 
-namespace LinearAlgebra {
-
 template <typename T>
 SphericalOf<T>::SphericalOf() {
   this->distance = 0.0f;
@@ -303,5 +301,3 @@ SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
 
 template class SphericalOf<float>;
 template class SphericalOf<signed short>;
-
-}  // namespace LinearAlgebra

LinearAlgebra/Spherical.h

@@ -186,6 +186,7 @@ using Spherical = SphericalSingle;
 #endif
 
 }  // namespace LinearAlgebra
+using namespace LinearAlgebra;
 
 #include "Polar.h"
 #include "Vector3.h"

LinearAlgebra/SwingTwist.cpp

@@ -4,8 +4,6 @@
 
 #include "SwingTwist.h"
 
-namespace LinearAlgebra {
-
 template <typename T>
 SwingTwistOf<T>::SwingTwistOf() {
   this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
@@ -167,6 +165,4 @@ void SwingTwistOf<T>::Normalize() {
 }
 
 template class SwingTwistOf<float>;
-template class SwingTwistOf<signed short>;
+template class SwingTwistOf<signed short>;
-
-}

LinearAlgebra/test/Direction_test.cc

@@ -6,8 +6,6 @@
 
 #include "Direction.h"
 
-using namespace LinearAlgebra;
-
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(Direction16, Compare) {

LinearAlgebra/test/Polar_test.cc

@@ -2,7 +2,6 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
-#include <chrono>
 
 #include "Polar.h"
 #include "Spherical.h"

LinearAlgebra/test/Spherical16_test.cc

@@ -2,7 +2,6 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
-#include <chrono>
 
 #include "Spherical.h"
 #include "Vector3.h"

LinearAlgebra/test/SphericalSingle_test.cc

@@ -2,7 +2,6 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
-#include <chrono>
 
 #include "Spherical.h"
 

Messages/BinaryMsg.cpp

@@ -6,8 +6,7 @@ BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
   this->thing = thing;
-  unsigned char ix = 0; //BinaryMsg::length;
+  unsigned char ix = BinaryMsg::length;
-  this->data = new char[255];
   this->dataLength = this->thing->GenerateBinary(this->data, &ix);
 }
 
@@ -41,8 +40,6 @@ 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/BinaryMsg.h

@@ -1,17 +1,15 @@
 #pragma once
 
-#include "IMessage.h"
+#include "Messages.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
-/// @brief A message containing binary data for custom communication
+/// @brief Message to send thing-specific data
 class BinaryMsg : public IMessage {
  public:
   /// @brief The message ID
   static const unsigned char id = 0xB1;
-  /// @brief The length of the message in bytes, excluding the binary data
+  /// @brief The length of the message without the binary data itslef
-  /// For the total size of the message this.bytes.Length should be added to this value.
   static const unsigned length = 4;
 
   /// @brief The network ID of the thing
@@ -25,7 +23,7 @@ class BinaryMsg : public IMessage {
   /// @brief The binary data which is communicated
   char* data = nullptr;
 
-  /// @brief Create a BinaryMsg
+  /// @brief Create a new message for sending
   /// @param networkId The network ID of the thing
   /// @param thing The thing for which binary data is sent
   BinaryMsg(unsigned char networkId, Thing* thing);

Messages/DestroyMsg.cpp

@@ -2,22 +2,19 @@
 
 namespace RoboidControl {
 
-DestroyMsg::DestroyMsg(unsigned char networkId, Thing* thing) {
+DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
 }
 
-DestroyMsg::DestroyMsg(char* buffer) {
+DestroyMsg::DestroyMsg(char* buffer) {}
-  this->networkId = buffer[1];
-  this->thingId = buffer[2];
-}
 
 DestroyMsg::~DestroyMsg() {}
 
-unsigned char DestroyMsg::Serialize(char* buffer) {
+unsigned char DestroyMsg::Serialize(char *buffer) {
-#if defined(DEBUG)
+  #if defined(DEBUG)
-  std::cout << "Send DestroyMsg [" << (int)this->networkId << "/"
+  std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId
-            << (int)this->thingId << "] " << std::endl;
+            << "] " << std::endl;
 #endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
@@ -26,4 +23,4 @@ unsigned char DestroyMsg::Serialize(char* buffer) {
   return ix;
 }
 
-}  // namespace RoboidControl
+} // namespace RoboidControl

Messages/DestroyMsg.h

@@ -1,16 +1,13 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
-/// @brief A Message notifiying that a Thing no longer exists
+/// @brief Message notifiying that a Thing no longer exists
 class DestroyMsg : public IMessage {
  public:
   /// @brief The message ID
   static const unsigned char id = 0x20;
-  /// @brief The length of the message in bytes
+  /// @brief The length of the message
   static const unsigned length = 3;
   /// @brief The network ID of the thing
   unsigned char networkId;

Messages/IMessage.h

@@ -1,16 +0,0 @@
-#pragma once
-
-namespace RoboidControl {
-
-/// @brief Root structure for all communcation messages
-class IMessage {
- public:
-  IMessage();
-  /// @brief Serialize the message into a byte array for sending
-  /// @param buffer The buffer to serilize into
-  /// @return The length of the message in the buffer
-  virtual unsigned char Serialize(char* buffer);
-
-};
-
-}  // namespace RoboidControl

Messages/InvestigateMsg.cpp

@@ -7,9 +7,9 @@ InvestigateMsg::InvestigateMsg(char* buffer) {
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
 }
-InvestigateMsg::InvestigateMsg(unsigned char networkId, Thing* thing) {
+InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
   this->networkId = networkId;
-  this->thingId = thing->id;
+  this->thingId = thingId;
 }
 
 InvestigateMsg::~InvestigateMsg() {}

Messages/InvestigateMsg.h

@@ -1,7 +1,4 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -17,10 +14,10 @@ class InvestigateMsg : public IMessage {
   /// @brief the ID of the thing
   unsigned char thingId;
 
-  /// @brief Create an investigate message
+  /// @brief Create a new message for sending
   /// @param networkId The network ID for the thing
-  /// @param thing The thing for which the details are requested
+  /// @param thingId The ID of the thing
-  InvestigateMsg(unsigned char networkId, Thing* thing);
+  InvestigateMsg(unsigned char networkId, unsigned char thingId);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   InvestigateMsg(char* buffer);
   /// @brief Destructor for the message

Messages/LowLevelMessages.cpp

@@ -5,83 +5,6 @@
 
 namespace RoboidControl {
 
-void LowLevelMessages::SendSpherical(char* buffer,
-                                     unsigned char* ix,
-                                     Spherical s) {
-  SendFloat16(buffer, ix, s.distance);
-  SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
-  SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
-}
-Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
-                                             unsigned char* startIndex) {
-  float distance = ReceiveFloat16(buffer, startIndex);
-
-  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
-  Angle horizontal = Angle::Radians(horizontal8.InRadians());
-
-  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
-  Angle vertical = Angle::Radians(vertical8.InRadians());
-
-  Spherical s = Spherical(distance, horizontal, vertical);
-  return s;
-}
-
-void LowLevelMessages::SendSwingTwist(char* buffer,
-                                      unsigned char* ix,
-                                      SwingTwist s) {
-  SendAngle8(buffer, ix, s.swing.horizontal.InDegrees());
-  SendAngle8(buffer, ix, s.swing.vertical.InDegrees());
-  SendAngle8(buffer, ix, s.twist.InDegrees());
-}
-
-SwingTwist LowLevelMessages::ReceiveSwingTwist(const char* buffer,
-                                               unsigned char* startIndex) {
-  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
-  Angle horizontal = Angle::Radians(horizontal8.InRadians());
-
-  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
-  Angle vertical = Angle::Radians(vertical8.InRadians());
-
-  Angle8 twist8 = ReceiveAngle8(buffer, startIndex);
-  Angle twist = Angle::Radians(twist8.InRadians());
-
-  SwingTwist s = SwingTwist(horizontal, vertical, twist);
-  return s;
-}
-
-void LowLevelMessages::SendQuat32(char* buffer,
-                                  unsigned char* ix,
-                                  SwingTwist rotation) {
-  Quaternion q = rotation.ToQuaternion();
-  unsigned char qx = (char)(q.x * 127 + 128);
-  unsigned char qy = (char)(q.y * 127 + 128);
-  unsigned char qz = (char)(q.z * 127 + 128);
-  unsigned char qw = (char)(q.w * 255);
-  if (q.w < 0) {
-    qx = -qx;
-    qy = -qy;
-    qz = -qz;
-    qw = -qw;
-  }
-  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
-  // << "\n";
-  buffer[(*ix)++] = qx;
-  buffer[(*ix)++] = qy;
-  buffer[(*ix)++] = qz;
-  buffer[(*ix)++] = qw;
-}
-
-SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
-                                           unsigned char* ix) {
-  float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
-  float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
-  float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
-  float qw = buffer[(*ix)++] / 255.0F;
-  Quaternion q = Quaternion(qx, qy, qz, qw);
-  SwingTwist s = SwingTwist::FromQuaternion(q);
-  return s;
-}
-
 void LowLevelMessages::SendAngle8(char* buffer,
                                   unsigned char* ix,
                                   const float angle) {
@@ -119,5 +42,58 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
   return (float)f.toFloat();
 }
 
+void LowLevelMessages::SendSpherical(char* buffer,
+                                     unsigned char* ix,
+                                     Spherical s) {
+  SendFloat16(buffer, ix, s.distance);
+  SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
+  SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
+}
+Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
+                                             unsigned char* startIndex) {
+  float distance = ReceiveFloat16(buffer, startIndex);
+
+  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
+  Angle horizontal = Angle::Radians(horizontal8.InRadians());
+
+  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
+  Angle vertical = Angle::Radians(vertical8.InRadians());
+
+  Spherical s = Spherical(distance, horizontal, vertical);
+  return s;
+}
+
+void LowLevelMessages::SendQuat32(char* buffer,
+                                  unsigned char* ix,
+                                  SwingTwist rotation) {
+  Quaternion q = rotation.ToQuaternion();
+  unsigned char qx = (char)(q.x * 127 + 128);
+  unsigned char qy = (char)(q.y * 127 + 128);
+  unsigned char qz = (char)(q.z * 127 + 128);
+  unsigned char qw = (char)(q.w * 255);
+  if (q.w < 0) {
+    qx = -qx;
+    qy = -qy;
+    qz = -qz;
+    qw = -qw;
+  }
+  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
+  // << "\n";
+  buffer[(*ix)++] = qx;
+  buffer[(*ix)++] = qy;
+  buffer[(*ix)++] = qz;
+  buffer[(*ix)++] = qw;
+}
+
+SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
+                                           unsigned char* ix) {
+  float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qw = buffer[(*ix)++] / 255.0F;
+  Quaternion q = Quaternion(qx, qy, qz, qw);
+  SwingTwist s = SwingTwist::FromQuaternion(q);
+  return s;
+}
 
 }  // namespace RoboidControl

Messages/LowLevelMessages.h

@@ -1,5 +1,3 @@
-#pragma once
-
 #include "LinearAlgebra/Spherical.h"
 #include "LinearAlgebra/SwingTwist.h"
 
@@ -7,22 +5,18 @@ namespace RoboidControl {
 
 class LowLevelMessages {
  public:
-  static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
-  static Spherical ReceiveSpherical(const char* buffer,
-                                    unsigned char* startIndex);
-
-  static void SendSwingTwist(char* buffer, unsigned char* ix, SwingTwist s);
-  static SwingTwist ReceiveSwingTwist(const char* buffer,
-                                      unsigned char* startIndex);
-
-  static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
-  static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
-
   static void SendAngle8(char* buffer, unsigned char* ix, const float angle);
   static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex);
 
   static void SendFloat16(char* buffer, unsigned char* ix, float value);
   static float ReceiveFloat16(const char* buffer, unsigned char* startIndex);
+
+  static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
+  static Spherical ReceiveSpherical(const char* buffer,
+                                    unsigned char* startIndex);
+
+  static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
+  static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
 };
 
 }  // namespace RoboidControl

Messages/Messages.cpp

@@ -1,4 +1,7 @@
-#include "IMessage.h"
+#include "Messages.h"
+
+#include "LowLevelMessages.h"
+#include "string.h"
 
 namespace RoboidControl {
 

Messages/Messages.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "LinearAlgebra/Spherical.h"
+#include "LinearAlgebra/SwingTwist.h"
+#include "Thing.h"
+
+namespace RoboidControl {
+
+class ParticipantUDP;
+
+class IMessage {
+ public:
+  IMessage();
+  virtual unsigned char Serialize(char* buffer);
+
+  static unsigned char* ReceiveMsg(unsigned char packetSize);
+
+  // bool Publish(ParticipantUDP *participant);
+  // bool SendTo(ParticipantUDP *participant);
+};
+
+}  // namespace RoboidControl

Messages/ModelUrlMsg.h

@@ -1,7 +1,4 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -29,6 +26,8 @@ class ModelUrlMsg : public IMessage {
   ModelUrlMsg(unsigned char networkId, Thing* thing);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   ModelUrlMsg(const char* buffer);
+  //   ModelUrlMsg(unsigned char networkId, unsigned char thingId,
+  //               unsigned char urlLegth, const char *url, float scale = 1);
 
   /// @brief Destructor for the message
   virtual ~ModelUrlMsg();

Messages/NameMsg.cpp

@@ -7,16 +7,15 @@ namespace RoboidControl {
 NameMsg::NameMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
-  const char* thingName = thing->GetName();
+  if (thing->name == nullptr)
-  if (thingName == nullptr)
     this->nameLength = 0;
   else
-    this->nameLength = (unsigned char)strlen(thingName);
+    this->nameLength = (unsigned char)strlen(thing->name);
 
   // 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] = thingName[i];
+    name[i] = thing->name[i];
   name[this->nameLength] = '\0';
   this->name = name;
 }

Messages/NameMsg.h

@@ -1,7 +1,4 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -25,6 +22,9 @@ class NameMsg : public IMessage {
   /// @param networkId The network ID of the thing
   /// @param thing The ID of the thing
   NameMsg(unsigned char networkId, Thing* thing);
+  //   NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
+  //           unsigned char nameLength);
+
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   NameMsg(const char* buffer);
   /// @brief Destructor for the message

Messages/NetworkIdMsg.cpp

@@ -1,27 +0,0 @@
-#include "NetworkIdMsg.h"
-
-#include <iostream>
-
-namespace RoboidControl {
-
-NetworkIdMsg::NetworkIdMsg(const char* buffer) {
-  this->networkId = buffer[1];
-}
-
-NetworkIdMsg::NetworkIdMsg(unsigned char networkId) {
-  this->networkId = networkId;
-}
-
-NetworkIdMsg::~NetworkIdMsg() {}
-
-unsigned char NetworkIdMsg::Serialize(char* buffer) {
-#if defined(DEBUG)
-  std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl;
-#endif
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  return NetworkIdMsg::length;
-}
-
-}  // namespace RoboidControl

Messages/ParticipantMsg.cpp

@@ -1,9 +1,5 @@
 #include "ParticipantMsg.h"
 
-#if !defined(NO_STD)
-#include <iostream>
-#endif
-
 namespace RoboidControl {
 
 ParticipantMsg::ParticipantMsg(char networkId) {
@@ -17,7 +13,7 @@ ParticipantMsg::ParticipantMsg(const char* buffer) {
 ParticipantMsg::~ParticipantMsg() {}
 
 unsigned char ParticipantMsg::Serialize(char* buffer) {
-#if defined(DEBUG) && !defined(NO_STD)
+#if defined(DEBUG)
   std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] "
             << std::endl;
 #endif

Messages/ParticipantMsg.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "IMessage.h"
+#include "Messages.h"
 
 namespace RoboidControl {
 

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->IsRoot())) {
+  if (thing->positionUpdated || force) {
     this->position = thing->GetPosition();
     this->poseType |= Pose_Position;
   }
-  if (thing->orientationUpdated || (force || thing->IsRoot())) {
+  if (thing->orientationUpdated || force) {
     this->orientation = thing->GetOrientation();
     this->poseType |= Pose_Orientation;
   }
@@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) {
   this->thingId = buffer[ix++];
   this->poseType = buffer[ix++];
   this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
-  this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
+  this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
   // linearVelocity
   // angularVelocity
 }
@@ -45,7 +45,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
   if (this->poseType == 0)
     return 0;
 
-#if defined(DEBUG) && DEBUG > 1
+#if defined(DEBUG)
   std::cout << "Send PoseMsg [" << (int)this->networkId << "/"
             << (int)this->thingId << "] " << (int)this->poseType << std::endl;
 #endif
@@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
   if ((this->poseType & Pose_Position) != 0)
     LowLevelMessages::SendSpherical(buffer, &ix, this->position);
   if ((this->poseType & Pose_Orientation) != 0)
-    LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
+    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
   if ((this->poseType & Pose_LinearVelocity) != 0)
     LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
   if ((this->poseType & Pose_AngularVelocity) != 0)

Messages/PoseMsg.h

@@ -1,6 +1,4 @@
-#pragma once
+#include "Messages.h"
-#include "IMessage.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -11,7 +9,7 @@ class PoseMsg : public IMessage {
  public:
   /// @brief The message ID
   static const unsigned char id = 0x10;
-  /// @brief The length of the message in bytes
+  /// @brief The length of the message
   unsigned char length = 4 + 4 + 4;
 
   /// @brief The network ID of the thing
@@ -42,8 +40,7 @@ class PoseMsg : public IMessage {
 
   /// @brief Create a new message for sending
   /// @param networkId he network ID of the thing
-  /// @param thing The thing for which the pose should be sent
+  /// @param thing The thing for which the pose shouldbe sent
-  /// @param force If true, position and orientation are always included, even when they are not updated
   PoseMsg(unsigned char networkId, Thing* thing, bool force = false);
 
   /// @copydoc RoboidControl::IMessage::IMessage(char*)

Messages/SiteMsg.cpp

@@ -0,0 +1,25 @@
+#include "SiteMsg.h"
+
+namespace RoboidControl {
+
+SiteMsg::SiteMsg(const char* buffer) {
+  this->networkId = buffer[1];
+}
+
+SiteMsg::SiteMsg(unsigned char networkId) {
+  this->networkId = networkId;
+}
+
+SiteMsg::~SiteMsg() {}
+
+unsigned char SiteMsg::Serialize(char* buffer) {
+#if defined(DEBUG)
+  std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl;
+#endif
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  return SiteMsg::length;
+}
+
+}  // namespace RoboidControl

Messages/SiteMsg.h

@@ -1,11 +1,9 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
 
 namespace RoboidControl {
 
 /// @brief A message communicating the network ID for that participant
-class NetworkIdMsg : public IMessage {
+class SiteMsg : public IMessage {
 public:
   /// @brief The message ID
   static const unsigned char id = 0xA1;
@@ -16,11 +14,11 @@ public:
 
   /// @brief Create a new message for sending
   /// @param networkId The network ID for the participant
-  NetworkIdMsg(unsigned char networkId);
+  SiteMsg(unsigned char networkId);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
-  NetworkIdMsg(const char *buffer);
+  SiteMsg(const char *buffer);
   /// @brief Destructor for the message
-  virtual ~NetworkIdMsg();
+  virtual ~SiteMsg();
 
   /// @copydoc RoboidControl::IMessage::Serialize
   virtual unsigned char Serialize(char *buffer) override;

Messages/TextMsg.cpp

@@ -1,9 +1,5 @@
 #include "TextMsg.h"
 
-#if !defined(NO_STD)
-#include <iostream>
-#endif
-
 namespace RoboidControl {
 
 TextMsg::TextMsg(const char* text, unsigned char textLength) {
@@ -28,7 +24,7 @@ unsigned char TextMsg::Serialize(char* buffer) {
   if (this->textLength == 0 || this->text == nullptr)
     return 0;
 
-#if defined(DEBUG) && !defined(NO_STD)
+#if defined(DEBUG)
   std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl;
 #endif
   unsigned char ix = 0;

Messages/TextMsg.h

@@ -1,4 +1,4 @@
-#include "IMessage.h"
+#include "Messages.h"
 
 namespace RoboidControl {
 
@@ -9,6 +9,10 @@ class TextMsg : public IMessage {
   static const unsigned char id = 0xB0;
   /// @brief The length of the message without the text itself
   static const unsigned char length = 2;
+  /// @brief The network ID of the thing
+  unsigned char networkId;
+  /// @brief the ID of the thing
+  unsigned char thingId;
   /// @brief The text without the null terminator
   const char* text;
   /// @brief The length of the text

Messages/ThingMsg.cpp

@@ -14,21 +14,27 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
   this->thingType = thing->type;
-  if (thing->IsRoot())
+  Thing* parent = thing->GetParent();
-    this->parentId = 0;
+  if (parent != nullptr)
-  else {
-    Thing* parent = thing->GetParent();
     this->parentId = parent->id;
-  }
+  else
+    this->parentId = 0;
 }
 
+// ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
+//                    unsigned char thingType, unsigned char parentId) {
+//   this->networkId = networkId;
+//   this->thingId = thingId;
+//   this->thingType = thingType;
+//   this->parentId = parentId;
+// }
+
 ThingMsg::~ThingMsg() {}
 
 unsigned char ThingMsg::Serialize(char* buffer) {
 #if defined(DEBUG)
-  std::cout << "Send ThingMsg [" << (int)this->networkId << "/"
+  std::cout << "Send ThingMsg [" << (int)this->networkId << "/" << (int)this->thingId
-            << (int)this->thingId << "] " << (int)this->thingType << " "
+            << "] " << (int)this->thingType << " " << (int)this->parentId << std::endl;
-            << (int)this->parentId << std::endl;
 #endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;

Messages/ThingMsg.h

@@ -1,9 +1,8 @@
-#include "IMessage.h"
+#include "Messages.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
-/// @brief Message providing generic details about a Thing
+/// @brief  Message providing generic information about a Thing
 class ThingMsg : public IMessage {
  public:
   /// @brief The message ID
@@ -14,15 +13,17 @@ class ThingMsg : public IMessage {
   unsigned char networkId;
   /// @brief The ID of the thing
   unsigned char thingId;
-  /// @brief The type of thing
+  /// @brief The Thing.Type of the thing
   unsigned char thingType;
-  /// @brief The ID of the parent thing in the hierarchy. This is zero for root things
+  /// @brief The parent of the thing in the hierarachy. This is null for root Things
   unsigned char parentId;
 
   /// @brief Create a message for sending
   /// @param networkId The network ID of the thing</param>
   /// @param thing The thing
   ThingMsg(unsigned char networkId, Thing* thing);
+  //   ThingMsg(unsigned char networkId, unsigned char thingId,
+  //            unsigned char thingType, unsigned char parentId);
 
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   ThingMsg(const char* buffer);

Participant.cpp

@@ -1,32 +1,12 @@
 #include "Participant.h"
 
 #include <string.h>
-#include "Arduino/ArduinoParticipant.h"
-#include "EspIdf/EspIdfParticipant.h"
-#include "Posix/PosixParticipant.h"
-#include "Windows/WindowsParticipant.h"
 
 namespace RoboidControl {
 
-#pragma region Participant
+Participant::Participant() {}
 
-Participant* Participant::LocalParticipant = new Participant();
-
-void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
-  LocalParticipant = &newParticipant;
-  std::cout << "Replaced local participant" << std::endl;
-}
-
-Participant::Participant() {
-  Thing::CreateRoot(this);
-  //this->Add(this->root);
-}
-
-/*
 Participant::Participant(const char* ipAddress, int port) {
-  Thing::CreateRoot(this);
-  //this->Add(this->root);
-
   // make a copy of the ip address string
   int addressLength = (int)strlen(ipAddress);
   int stringLength = addressLength + 1;
@@ -42,62 +22,25 @@ Participant::Participant(const char* ipAddress, int port) {
   this->ipAddress = addressString;
   this->port = port;
 }
-*/
+
 Participant::~Participant() {
-  // registry.Remove(this);
+  delete[] this->ipAddress;
-  // delete[] this->ipAddress;
 }
 
-void Participant::Update(bool recurse) {
+void Participant::Update(unsigned long currentTimeMs) {
   for (Thing* thing : this->things) {
     if (thing != nullptr)
-      thing->Update();
+      thing->Update(currentTimeMs, true);
   }
 }
 
-bool Participant::Send(IMessage* msg) {
+Thing* Participant::Get(unsigned char thingId) {
-    std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff)
-            << " to base Participant without communcation support " << std::endl;
-
-  return true;
-}
-/*
-bool Participant::Send(IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
-  if (bufferSize <= 0)
-    return true;
-
-  // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
-  //           << " to " << this->ipAddress << std::endl;
-
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  return thisWindows->SendTo(this, bufferSize);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  return thisPosix->SendTo(this, bufferSize);
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  return thisArduino->SendTo(this, bufferSize);
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  return thisEspIdf->SendTo(this, bufferSize);
-#else
-  return false;
-#endif
-}
-*/
-
-Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
   for (Thing* thing : this->things) {
-    if (thing->owner->networkId == networkId && thing->id == thingId)
+    if (thing->id == thingId)
       return thing;
   }
-  std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port
+  // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
-            << "[" << (int)thingId << "]\n";
+  //           << "[" << (int)thingId << "]\n";
   return nullptr;
 }
 
@@ -108,35 +51,24 @@ void Participant::Add(Thing* thing, bool checkId) {
     thing->id = this->thingCount + 1;
     this->things[this->thingCount++] = thing;
 #else
-    // find highest id
+    thing->id = (unsigned char)this->things.size() + 1;
-    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 " 
+    // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
-    //<< this->ipAddress << ":" << this->port
+    //           << this->port << "[" << (int)thing->id << "]\n";
-    << "[" << (int)thing->id << "]\n";
   } else {
-    Thing* foundThing = Get(thing->owner->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->id << "]\n";
-                << (int)thing->id << "]\n";
     } else {
-      std::cout << "Did not add, existing thing " 
+      // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
-      //<< this->ipAddress << ":" << this->port
+      //           << this->port << "[" << (int)thing->id << "]\n";
-       << "[" << (int)thing->id << "]\n";
     }
   }
 }
@@ -157,95 +89,22 @@ 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
+  std::cout << "Removing " << thing->networkId << "/" << thing->id
-  //           << "] list size = " << this->things.size() << "\n";
+            << " list size = " << this->things.size() << "\n";
 #endif
 }
 
-#pragma endregion
+// void Participant::UpdateAll(unsigned long currentTimeMs) {
+//   // Not very efficient, but it works for now.
 
-// #pragma region ParticipantRegistry
+//   for (Thing* thing : this->things) {
-
+//     if (thing != nullptr && thing->GetParent() == nullptr) {  // update all
-// /*
+//     root things
-// Participant* ParticipantRegistry::Get(const char* ipAddress,
+//       // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
-//                                       unsigned int port) {
+//       //           << "\n";
-// #if !defined(NO_STD)
+//       thing->Update(currentTimeMs);
-//   for (Participant* participant : ParticipantRegistry::participants) {
-//     if (participant == nullptr)
-//       continue;
-//     if (strcmp(participant->ipAddress, ipAddress) == 0 &&
-//         participant->port == port) {
-//       // std::cout << "found participant " << participant->ipAddress << ":"
-//       //           << (int)participant->port << std::endl;
-//       return participant;
 //     }
 //   }
-//   std::cout << "Could not find participant " << ipAddress << ":" << (int)port
-//             << std::endl;
-// #endif
-//   return nullptr;
 // }
-// */
-// Participant* ParticipantRegistry::Get(unsigned char participantId) {
-// #if !defined(NO_STD)
-//   for (Participant* participant : ParticipantRegistry::participants) {
-//     if (participant == nullptr)
-//       continue;
-//     if (participant->networkId == participantId)
-//       return participant;
-//   }
-//   std::cout << "Could not find participant " << (int)participantId << std::endl;
-// #endif
-//   return nullptr;
-// }
-
-// // Participant* ParticipantRegistry::Add(const char* ipAddress,
-// //                                       unsigned int port) {
-// //   Participant* participant = new Participant(ipAddress, port);
-// //   Add(participant);
-// //   return participant;
-// // }
-
-// void ParticipantRegistry::Add(Participant* participant) {
-//   Participant* foundParticipant =
-//     Get(participant->networkId);
-//       //Get(participant->ipAddress, participant->port);
-
-//   if (foundParticipant == nullptr) {
-// #if defined(NO_STD)
-//     // this->things[this->thingCount++] = thing;
-// #else
-//     ParticipantRegistry::participants.push_back(participant);
-// #endif
-//     //   std::cout << "Add participant " << participant->ipAddress << ":"
-//     //             << participant->port << "[" << (int)participant->networkId
-//     //             << "]\n";
-//     //   std::cout << "participants " <<
-//     //   ParticipantRegistry::participants.size()
-//     //             << "\n";
-//     // } else {
-//     //   std::cout << "Did not add, existing participant " <<
-//     //   participant->ipAddress
-//     //             << ":" << participant->port << "[" <<
-//     //             (int)participant->networkId
-//     //             << "]\n";
-//   }
-// }
-
-// void ParticipantRegistry::Remove(Participant* participant) {
-//   // participants.remove(participant);
-// }
-
-// #if defined(NO_STD)
-// Participant** ParticipantRegistry::GetAll() const {
-//   return ParticipantRegistry::participants;
-// }
-// #else
-// const std::list<Participant*>& ParticipantRegistry::GetAll() const {
-//   return ParticipantRegistry::participants;
-// }
-// #endif
-
-// #pragma endregion ParticipantRegistry
 
 }  // namespace RoboidControl

Participant.h

@@ -1,61 +1,10 @@
 #pragma once
-
-#include "Messages/IMessage.h"
 #include "Thing.h"
 
 namespace RoboidControl {
 
 constexpr int MAX_THING_COUNT = 256;
 
-/*
-/// @brief class which manages all known participants
-class ParticipantRegistry {
- public:
-  /// @brief Retrieve a participant by its address
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The participant or a nullptr when it could not be found
-  //Participant* Get(const char* ipAddress, unsigned int port);
-  /// @brief Retrieve a participant by its network ID
-  /// @param networkID The network ID of the participant
-  /// @return The participant or a nullptr when it could not be found
-  Participant* Get(unsigned char networkID);
-
-  /// @brief Add a participant with the given details
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The added participant
-  //Participant* Add(const char* ipAddress, unsigned int port);
-
-  /// @brief Add a participant
-  /// @param participant The participant to add
-  void Add(Participant* participant);
-
-  /// @brief Remove a participant
-  /// @param participant The participant to remove
-  void Remove(Participant* participant);
-
- private:
-#if defined(NO_STD)
- public:
-  Participant** GetAll() const;
-  int count = 0;
-
- private:
-  Participant** participants;
-#else
- public:
-  /// @brief Get all participants
-  /// @return All participants
-  const std::list<Participant*>& GetAll() const;
-
- private:
-  /// @brief The list of known participants
-  std::list<Participant*> participants;
-#endif
-};
-*/
-
 /// @brief A participant is a device which manages things.
 /// It can communicate with other participant to synchronise the state of
 /// things. This class is used to register the things the participant is
@@ -63,86 +12,53 @@ class ParticipantRegistry {
 /// participant. It is used as a basis for the local participant, but also as a
 /// reference to remote participants.
 class Participant {
-#pragma region Init
-
  public:
-  /// @brief Create a generic participant
+  /// @brief The Ip Address of a participant. When the participant is local,
+  /// this contains 0.0.0.0
+  const char* ipAddress = "0.0.0.0";
+  /// @brief The port number for UDP communication with the participant. This is
+  /// 0 for isolated participants.
+  int port = 0;
+
+  /// @brief The network Id to identify the participant.
+  /// @note This field is likely to disappear in future versions
+  unsigned char networkId = 0;
+
+  /// @brief Default constructor
   Participant();
   /// @brief Create a new participant with the given communcation info
   /// @param ipAddress The IP address of the participant
-  /// @param port The UDP port of the participant
+  /// @param port The port of the participant
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
   Participant(const char* ipAddress, int port);
   /// @brief Destructor for the participant
   ~Participant();
 
-  /// @brief The local participant for this application
+  virtual void Update(unsigned long currentTimeMs = 0);
-  static Participant* LocalParticipant;
-  /// @brief Replace the local participant
-  /// @param newParticipant The new local Participant
-  static void ReplaceLocalParticipant(Participant& newParticipant);
 
-#pragma endregion Init
+ protected:
-
-#pragma region Properties
-
- public:
-  /// @brief The name of the participant
-  const char* name = "Participant";
-
-  /// @brief The network Id to identify the participant
-  unsigned char networkId = 0;
-
-  /// @brief The root thing for this participant
-  Thing* root = nullptr;
-
- public:
 #if defined(NO_STD)
   unsigned char thingCount = 0;
   Thing* things[MAX_THING_COUNT];
 #else
-  /// @brief  The things managed by this participant
+  /// @brief The list of things managed by this participant
   std::list<Thing*> things;
 #endif
+
+ public:
   /// @brief Find a thing managed by this participant
-  /// @param networkId The network ID of the thing
+  /// @param networkId The network ID for the thing
   /// @param thingId The ID of the thing
-  /// @return The thing if found, nullptr when no thing has been found
+  /// @return The thing if found or nullptr when no thing has been found
-  Thing* Get(unsigned char networkId, unsigned char thingId);
+  /// @note The use of the network ID is likely to disappear in future versions.
+  Thing* Get(unsigned char thingId);
   /// @brief Add a new thing for this participant.
   /// @param thing The thing to add
-  /// @param checkId If true, the thing.id is regenerated if it is zero
+  /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing
+  /// Id will be assigned.
   void Add(Thing* thing, bool checkId = true);
   /// @brief Remove a thing for this participant
   /// @param thing The thing to remove
   void Remove(Thing* thing);
-
-#pragma endregion Properties
-
-#pragma region Update
-
- public:
-  /// @brief Update all things for this participant
-  virtual void Update(bool recurse = true);
-
-#pragma endregion Update
-
-#pragma region Send
-
- public:
-  char buffer[1024];
-
-  virtual bool Send(IMessage* msg);
-
-#pragma endregion Send
-
-// #pragma region Participant Registry
-
-//  public:
-//   static ParticipantRegistry registry;
-
-// #pragma endregion Participant Registry
 };
 
 }  // namespace RoboidControl

Participants/IsolatedParticipant.cpp

@@ -1,16 +1,14 @@
-/*
 #include "IsolatedParticipant.h"
 #include "ParticipantUDP.h"
 
 namespace RoboidControl {
 
-static ParticipantUDPGeneric* isolatedParticipant = nullptr;
+static ParticipantUDP* isolatedParticipant = nullptr;
 
 Participant* IsolatedParticipant::Isolated() {
   if (isolatedParticipant == nullptr)
-    isolatedParticipant = new ParticipantUDPGeneric(0);
+    isolatedParticipant = new ParticipantUDP(0);
   return isolatedParticipant;
 }
 
-}  // namespace RoboidControl
+}  // namespace RoboidControl
-  */

Participants/IsolatedParticipant.h

@@ -1,4 +1,3 @@
-/*
 #include "Participant.h"
 
 namespace RoboidControl {
@@ -11,5 +10,4 @@ class IsolatedParticipant {
   static Participant* Isolated();
 };
 
-}
+}
-*/

Participants/ParticipantUDP.cpp

@@ -1,184 +1,86 @@
 #include "ParticipantUDP.h"
 
-#include "Participant.h"
 #include "Thing.h"
 
 #include "Arduino/ArduinoParticipant.h"
 #include "EspIdf/EspIdfParticipant.h"
-#include "Posix/PosixParticipant.h"
-#include "Windows/WindowsParticipant.h"
 
-#include "Things/DifferentialDrive.h"
+#if defined(_WIN32) || defined(_WIN64)
-#include "Things/DistanceSensor.h"
+#include <winsock2.h>
-#include "Things/TouchSensor.h"
+#include <ws2tcpip.h>
+#include "Windows/WindowsParticipant.h"
+#pragma comment(lib, "ws2_32.lib")
+
+#elif defined(__unix__) || defined(__APPLE__)
+#include <arpa/inet.h>
+#include <fcntl.h>  // For fcntl
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <chrono>
+#include "Posix/PosixParticipant.h"
+#endif
 
 #include <string.h>
 
 namespace RoboidControl {
 
-#pragma region ParticipantRegistry
+ParticipantUDP::ParticipantUDP(int port) {
-
+  this->ipAddress = "0.0.0.0";
-ParticipantRegistry ParticipantUDPGeneric::registry;
-
-RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress,
-                                               unsigned int port) {
-#if !defined(NO_STD)
-  for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
-    if (participant == nullptr)
-      continue;
-    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
-        participant->port == port) {
-      // std::cout << "found participant " << participant->ipAddress << ":"
-      //           << (int)participant->port << std::endl;
-      return participant;
-    }
-  }
-  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
-            << std::endl;
-#endif
-  return nullptr;
-}
-
-RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) {
-#if !defined(NO_STD)
-  for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
-    if (participant == nullptr)
-      continue;
-    if (participant->networkId == participantId)
-      return participant;
-  }
-  std::cout << "Could not find participant " << (int)participantId << std::endl;
-#endif
-  return nullptr;
-}
-
-RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress,
-                                               unsigned int port) {
-  RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port);
-  Add(participant);
-  return participant;
-}
-
-void ParticipantRegistry::Add(RemoteParticipantUDP* participant) {
-  Participant* foundParticipant = Get(participant->networkId);
-  // Get(participant->ipAddress, participant->port);
-
-  if (foundParticipant == nullptr) {
-#if defined(NO_STD)
-    // this->things[this->thingCount++] = thing;
-#else
-    ParticipantRegistry::participants.push_back(participant);
-#endif
-    //   std::cout << "Add participant " << participant->ipAddress << ":"
-    //             << participant->port << "[" << (int)participant->networkId
-    //             << "]\n";
-    //   std::cout << "participants " <<
-    //   ParticipantRegistry::participants.size()
-    //             << "\n";
-    // } else {
-    //   std::cout << "Did not add, existing participant " <<
-    //   participant->ipAddress
-    //             << ":" << participant->port << "[" <<
-    //             (int)participant->networkId
-    //             << "]\n";
-  }
-}
-
-void ParticipantRegistry::Remove(RemoteParticipantUDP* participant) {
-  // participants.remove(participant);
-}
-
-#if defined(NO_STD)
-RemoteParticipantUDP** ParticipantRegistry::GetAll() const {
-  return ParticipantRegistry::participants;
-}
-#else
-const std::list<RemoteParticipantUDP*>& ParticipantRegistry::GetAll() const {
-  return ParticipantRegistry::participants;
-}
-#endif
-
-#pragma endregion ParticipantRegistry
-
-RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) {
-  // make a copy of the ip address string
-  int addressLength = (int)strlen(ipAddress);
-  int stringLength = addressLength + 1;
-  char* addressString = new char[stringLength];
-#if defined(_WIN32) || defined(_WIN64)
-  strncpy_s(addressString, stringLength, ipAddress,
-            addressLength);  // Leave space for null terminator
-#else
-  strncpy(addressString, ipAddress, addressLength);
-#endif
-  addressString[addressLength] = '\0';
-
-  this->ipAddress = addressString;
   this->port = port;
-}
-
-bool RemoteParticipantUDP::Send(IMessage* msg) {
-  // No message is actually sent, because this class has no networking
-  // implementation
-  return false;
-}
-
-#pragma region Init
-
-ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
-    : RemoteParticipantUDP("127.0.0.1", port) {
-  this->name = "ParticipantUDP";
   this->remoteSite = nullptr;
   if (this->port == 0)
     this->isIsolated = true;
-  registry.Add(this);
-
-  this->root = Thing::LocalRoot();  //::LocalParticipant->root;
-  this->root->owner = this;
-  this->root->name = "UDP Root";
-  std::cout << "P2 " << (int)this->root << std::endl;
-  this->Add(this->root);
-
-  Participant::ReplaceLocalParticipant(*this);
 }
 
-ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
+ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
-                                             int port,
+    : Participant("127.0.0.1", localPort) {
-                                             int localPort)
-    : RemoteParticipantUDP("127.0.0.1", localPort) {
-  this->name = "ParticipantUDP";
   if (this->port == 0)
     this->isIsolated = true;
   else
-    this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
+    this->remoteSite = new Participant(ipAddress, port);
-  registry.Add(this);
-
-  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
-  this->root->owner = this;
-  this->root->name = "UDP Root";
-  std::cout << "P1 " << (int)this->root << std::endl;
-  this->Add(this->root);
-
-  Participant::ReplaceLocalParticipant(*this);
 }
 
-void ParticipantUDPGeneric::begin() {
+static ParticipantUDP* isolatedParticipant = nullptr;
-  if (this->isIsolated || this->remoteSite == nullptr)
+
+ParticipantUDP* ParticipantUDP::Isolated() {
+  if (isolatedParticipant == nullptr)
+    isolatedParticipant = new ParticipantUDP(0);
+  return isolatedParticipant;
+}
+
+void ParticipantUDP::begin() {
+  if (this->isIsolated)
     return;
 
   SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
 }
 
-#pragma endregion Init
+void ParticipantUDP::SetupUDP(int localPort,
+                              const char* remoteIpAddress,
+                              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 = static_cast<Posix::ParticipantUDP*>(this);
+  thisPosix->Setup(localPort, remoteIpAddress, remotePort);
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
+#endif
+  this->connected = true;
+}
 
-#pragma region Update
+void ParticipantUDP::Update(unsigned long currentTimeMs) {
-
+  if (currentTimeMs == 0)
-// The update order
+    currentTimeMs = Thing::GetTimeMs();
-// 1. receive external messages
-// 2. update the state
-// 3. send out the updated messages
-void ParticipantUDPGeneric::Update(bool recurse) {
-  unsigned long currentTimeMs = Thing::GetTimeMs();
 
   if (this->isIsolated == false) {
     if (this->connected == false)
@@ -186,101 +88,122 @@ void ParticipantUDPGeneric::Update(bool recurse) {
 
     if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
       ParticipantMsg* msg = new ParticipantMsg(this->networkId);
-
       if (this->remoteSite == nullptr)
         this->Publish(msg);
       else
-        this->Send(msg);
+        this->Send(this->remoteSite, msg);
-
       delete msg;
+
       this->nextPublishMe = currentTimeMs + this->publishInterval;
     }
 
-    //this->ReceiveUDP();
+    this->ReceiveUDP();
   }
 
-  UpdateMyThings();
-  UpdateOtherThings();
-}
-
-void ParticipantUDPGeneric::UpdateMyThings() {
   for (Thing* thing : this->things) {
-    if (thing == nullptr)  // || thing->GetParent() != nullptr)
+    if (thing == nullptr)
       continue;
 
-      // Why don't we do recursive?
+    std::cout << "Update thing " << (int)thing->id << std::endl;
-    // Because when a thing creates a thing in the update,
+    if (this->isIsolated == false) {
-    // that new thing is not sent out (because of hierarchyChanged)
+      PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-    // before it is updated itself: it is immediatedly updated!
+      this->Send(thing->owner, poseMsg);
-    thing->Update(false);
+      BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-
+      this->Send(thing->owner, binaryMsg);
-    if (thing->terminate)
-      this->Remove(thing);
-  }
-}
-
-void ParticipantUDPGeneric::UpdateOtherThings() {
-#if defined(NO_STD)
-  Participant** participants = Participant::registry.GetAll();
-  for (int ix = 0; ix < Participant::registry.count; ix++) {
-    Participant* participant = participants[ix];
-#else
-  for (Participant* participant : registry.GetAll()) {
-#endif
-    if (participant == nullptr || participant == this)
-      continue;
-
-    // Call only the Participant version of the Update.
-    // This is to deal with the function where one of the (remote)
-    // participants is actually a local participant
-    participant->Participant::Update();
-    if (this->isIsolated)
-      continue;
-
-    for (Thing* thing : participant->things) {
-      PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
-      participant->Send(poseMsg);
       delete poseMsg;
-      BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
-      participant->Send(binaryMsg);
-      delete binaryMsg;
     }
+    thing->Update(currentTimeMs, true);
   }
 }
 
-// Update
+void ParticipantUDP::ReceiveUDP() {
-#pragma endregion
+#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
+}
+
+Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) {
+  for (Participant* sender : this->senders) {
+    if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
+      return sender;
+  }
+  return nullptr;
+}
+
+Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
+  // std::cout << "New Participant " << ipAddress << ":" << port << "\n";
+  Participant* participant = new Participant(ipAddress, port);
+#if defined(NO_STD)
+  participant->networkId = this->senderCount;
+  this->senders[this->senderCount++] = participant;
+#else
+  participant->networkId = (unsigned char)this->senders.size();
+  this->senders.push_back(participant);
+#endif
+  return participant;
+}
 
 #pragma region Send
 
-void ParticipantUDPGeneric::SendThingInfo(Participant* remoteParticipant,
+void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
-                                          Thing* thing) {
+                                   Thing* thing) {
   // std::cout << "Send thing info [" << (int)thing->id << "] \n";
   ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-  remoteParticipant->Send(thingMsg);
+  this->Send(remoteParticipant, thingMsg);
   delete thingMsg;
   NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  remoteParticipant->Send(nameMsg);
+  this->Send(remoteParticipant, nameMsg);
   delete nameMsg;
   ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  remoteParticipant->Send(modelMsg);
+  this->Send(remoteParticipant, modelMsg);
   delete modelMsg;
   PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
-  remoteParticipant->Send(poseMsg);
+  this->Send(remoteParticipant, poseMsg);
   delete poseMsg;
-  BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
-  remoteParticipant->Send(binaryMsg);
+  this->Send(remoteParticipant, customMsg);
-  delete binaryMsg;
+  delete customMsg;
 }
 
-bool ParticipantUDPGeneric::Send(IMessage* msg) {
+bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
-  if (this->remoteSite != nullptr)
+  int bufferSize = msg->Serialize(this->buffer);
-    return this->remoteSite->Send(msg);
+  if (bufferSize <= 0)
+    return true;
 
-  return true;
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  return thisWindows->Send(remoteParticipant, bufferSize);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  return thisPosix->Send(remoteParticipant, bufferSize);
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  return thisArduino->Send(remoteParticipant, bufferSize);
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  return thisEspIdf->Send(remoteParticipant, bufferSize);
+#else
+  return false;
+#endif
 }
 
-void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) {
+void ParticipantUDP::PublishThingInfo(Thing* thing) {
   // std::cout << "Publish thing info" << thing->networkId << "\n";
   //  Strange, when publishing, the network id is irrelevant, because it is
   //  connected to a specific site...
@@ -301,31 +224,47 @@ void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) {
   delete customMsg;
 }
 
+bool ParticipantUDP::Publish(IMessage* msg) {
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  return thisWindows->Publish(msg);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  return thisPosix->Publish(msg);
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  return thisArduino->Publish(msg);
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  return thisEspIdf->Publish(msg);
+#else
+  return false;
+#endif
+}
+
 // Send
 #pragma endregion
 
 #pragma region Receive
 
-void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
+void ParticipantUDP::ReceiveData(unsigned char packetSize,
-                                        char* senderIpAddress,
+                                 char* senderIpAddress,
-                                        unsigned int senderPort) {
+                                 unsigned int senderPort) {
-  // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
+  Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
-  //           << std::endl;
-  RemoteParticipantUDP* sender =
-      this->registry.Get(senderIpAddress, senderPort);
   if (sender == nullptr) {
-    sender = this->registry.Add(senderIpAddress, senderPort);
+    sender = this->AddParticipant(senderIpAddress, senderPort);
-#if !defined(NO_STD)
+    std::cout << "New remote participant " << sender->ipAddress << ":"
-    // std::cout << "New remote participant " << sender->ipAddress << ":"
+              << sender->port << std::endl;
-    //           << sender->port << std::endl;
-#endif
   }
 
   ReceiveData(packetSize, sender);
 }
 
-void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
+void ParticipantUDP::ReceiveData(unsigned char bufferSize,
-                                        RemoteParticipantUDP* sender) {
+                                 Participant* sender) {
   unsigned char msgId = this->buffer[0];
   // std::cout << "receive msg " << (int)msgId << "\n";
   // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
@@ -336,8 +275,8 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
       Process(sender, msg);
       delete msg;
     } break;
-    case NetworkIdMsg::id: {
+    case SiteMsg::id: {
-      NetworkIdMsg* msg = new NetworkIdMsg(this->buffer);
+      SiteMsg* msg = new SiteMsg(this->buffer);
       bufferSize -= msg->length;
       Process(sender, msg);
       delete msg;
@@ -377,113 +316,56 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
       Process(sender, msg);
       delete msg;
     } break;
-    case TextMsg::id: {
-      TextMsg* msg = new TextMsg(this->buffer);
-      bufferSize -= msg->length + msg->textLength;
-      Process(sender, msg);
-      delete msg;
-    } break;
-    case DestroyMsg::id: {
-      DestroyMsg* msg = new DestroyMsg(this->buffer);
-      bufferSize -= msg->length;
-      Process(sender, msg);
-      delete msg;
-    } break;
   };
 
-// Check if the buffer has been read completely
+  // Check if the buffer has been read completely
-#if !defined(NO_STD)
   if (bufferSize > 0)
     std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
-#endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
-                                    ParticipantMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
             << "\n";
 #endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
-                                    NetworkIdMsg* msg) {
 #if defined(DEBUG)
-  std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
+  std::cout << this->name << ": process SiteMsg " << (int)this->networkId
             << " -> " << (int)msg->networkId << "\n";
 #endif
 
   if (this->networkId != msg->networkId) {
     this->networkId = msg->networkId;
-
+    // std::cout << this->things.size() << " things\n";
-    std::cout << this->things.size() << " things\n";
     for (Thing* thing : this->things)
       this->SendThingInfo(sender, thing);
   }
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
-                                    InvestigateMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
             << "/" << (int)msg->thingId << "]\n";
 #endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
-                                    ThingMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
             << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
             << (int)msg->parentId << "\n";
 #endif
-  RemoteParticipantUDP* owner = registry.Get(msg->networkId);
-  if (owner == nullptr) {
-    owner = new RemoteParticipantUDP(sender->ipAddress, sender->port);
-    owner->networkId = msg->networkId;
-    registry.Add(owner);
-  }
-
-  Thing* thing = owner->Get(msg->networkId, msg->thingId);
-  if (thing == nullptr) {
-    bool isRemote = (sender->networkId != owner->networkId);
-    thing = ProcessNewThing(owner, msg, isRemote);
-    thing->id = msg->thingId;
-    thing->type = msg->thingType;
-    thing->isRemote = isRemote;
-  }
-
-  if (msg->parentId != 0) {
-    thing->SetParent(owner->Get(msg->networkId, msg->parentId));
-    if (thing->GetParent() == nullptr)
-      std::cout << "Could not find parent" << std::endl;
-  } else
-    thing->SetParent(nullptr);
 }
 
-Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
+void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
-                                              ThingMsg* msg,
-                                              bool isRemote) {
-  switch (msg->thingType) {
-    case Thing::Type::DistanceSensor:
-      return new DistanceSensor(owner->root);
-    case Thing::Type::TouchSensor:
-      return new TouchSensor(owner->root);
-    case Thing::Type::DifferentialDrive:
-      return new DifferentialDrive(owner->root);
-    default:
-      return new Thing(owner->root);
-  }
-}
-
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
-                                    NameMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
             << (int)msg->thingId << "] ";
 #endif
 
-  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;
@@ -498,90 +380,41 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
             nameLength);  // Leave space for null terminator
 #endif
     thingName[nameLength] = '\0';
-    thing->SetName(thingName);
+    thing->name = thingName;
 
-#if !defined(NO_STD)
+    std::cout << thing->name;
-    std::cout << thing->GetName();
-#endif
   }
-#if !defined(NO_STD)
   std::cout << std::endl;
-#endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
-                                    ModelUrlMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
             << "/" << (int)msg->thingId << "]\n";
 #endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
-                                    PoseMsg* msg) {
+#if defined(DEBUG)
-#if !defined(DEBUG) && !defined(NO_STD)
   std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
-            << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
+            << "/" << (int)msg->networkId << "]\n";
 #endif
-  Participant* owner = registry.Get(msg->networkId);
-  if (owner == nullptr)
-    return;
-
-  Thing* thing = owner->Get(msg->networkId, msg->thingId);
-  if (thing == nullptr)
-    return;
-
-  if ((msg->poseType & PoseMsg::Pose_Position) != 0)
-    thing->SetPosition(msg->position);
-  if ((msg->poseType & PoseMsg::Pose_Orientation) != 0)
-    thing->SetOrientation(msg->orientation);
-  if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0)
-    thing->SetLinearVelocity(msg->linearVelocity);
-  if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0)
-    thing->SetAngularVelocity(msg->angularVelocity);
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
-                                    BinaryMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
-            << "/" << (int)msg->thingId << "]\n";
+            << "/" << (int)msg->thingId << "] ";
 #endif
 
-  Participant* owner = registry.Get(msg->networkId);
+  Thing* thing = sender->Get(msg->thingId);
-  if (owner != nullptr) {
-    Thing* thing = owner->Get(msg->networkId, msg->thingId);
-    if (thing != nullptr)
-      thing->ProcessBinary(msg->data);
-#if !defined(NO_STD)
-    else {
-#if defined(DEBUG)
-      std::cout << "  unknown thing [" << (int)msg->networkId << "/"
-                << (int)msg->thingId << "]";
-#endif
-    }
-#endif
-  }
-}
-
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
-                                    TextMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
-            << (int)msg->text << "\n";
-#endif
-}
-
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
-                                    DestroyMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
-            << (int)msg->thingId << "]\n";
-#endif
-
-  Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing != nullptr)
-    this->Remove(thing);
+    thing->ProcessBinary(msg->data);
+  else {
+    std::cout << "  unknown thing [" << (int)msg->networkId << "/"
+              << (int)msg->thingId << "]";
+  }
+  std::cout << std::endl;
 }
 
 // Receive

Participants/ParticipantUDP.h

@@ -1,14 +1,12 @@
 #pragma once
 
 #include "Messages/BinaryMsg.h"
-#include "Messages/DestroyMsg.h"
 #include "Messages/InvestigateMsg.h"
 #include "Messages/ModelUrlMsg.h"
 #include "Messages/NameMsg.h"
-#include "Messages/NetworkIdMsg.h"
 #include "Messages/ParticipantMsg.h"
 #include "Messages/PoseMsg.h"
-#include "Messages/TextMsg.h"
+#include "Messages/SiteMsg.h"
 #include "Messages/ThingMsg.h"
 #include "Participant.h"
 
@@ -31,76 +29,7 @@ namespace RoboidControl {
 
 constexpr int MAX_SENDER_COUNT = 256;
 
-class RemoteParticipantUDP : public Participant {
+/// @brief A local participant is the local device which can communicate with
- public:
- /// @brief Create a new participant with the given communcation info
-  /// @param ipAddress The IP address of the participant
-  /// @param port The UDP port of the participant
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
-  RemoteParticipantUDP(const char* ipAddress, int port);
-
-  /// @brief The Ip Address of a participant.
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
-  const char* ipAddress = "0.0.0.0";
-  /// @brief The port number for UDP communication with the participant.
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
-  unsigned int port = 0;
-
-  bool Send(IMessage* msg) override;
-};
-
-/// @brief class which manages all known participants
-class ParticipantRegistry {
- public:
-  /// @brief Retrieve a participant by its address
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The participant or a nullptr when it could not be found
-  RemoteParticipantUDP* Get(const char* ipAddress, unsigned int port);
-  /// @brief Retrieve a participant by its network ID
-  /// @param networkID The network ID of the participant
-  /// @return The participant or a nullptr when it could not be found
-  RemoteParticipantUDP* Get(unsigned char networkID);
-
-  /// @brief Add a participant with the given details
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The added participant
-  RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port);
-
-  /// @brief Add a participant
-  /// @param participant The participant to add
-  void Add(RemoteParticipantUDP* participant);
-
-  /// @brief Remove a participant
-  /// @param participant The participant to remove
-  void Remove(RemoteParticipantUDP* participant);
-
- private:
-#if defined(NO_STD)
- public:
-  RemoteParticipantUDP** GetAll() const;
-  int count = 0;
-
- private:
-  RemoteParticipantUDP** participants;
-#else
- public:
-  /// @brief Get all participants
-  /// @return All participants
-  const std::list<RemoteParticipantUDP*>& GetAll() const;
-
- private:
-  /// @brief The list of known participants
-  std::list<RemoteParticipantUDP*> participants;
-#endif
-};
-
-/// @brief A participant using UDP communication
-/// A local participant is the local device which can communicate with
 /// other participants It manages all local things and communcation with other
 /// participants. Each application has a local participant which is usually
 /// explicit in the code. An participant can be isolated. In that case it is
@@ -111,42 +40,52 @@ class ParticipantRegistry {
 /// participant is created which can be obtained using
 /// RoboidControl::IsolatedParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
-class ParticipantUDPGeneric : public RemoteParticipantUDP {
+class ParticipantUDP : public Participant {
-#pragma region Init
-
  public:
   /// @brief Create a participant without connecting to a site
   /// @param port The port on which the participant communicates
   /// These participant typically broadcast Participant messages to let site
   /// servers on the local network know their presence. Alternatively they can
   /// broadcast information which can be used directly by other participants.
-  ParticipantUDPGeneric(int port = 7681);
+  ParticipantUDP(int port = 7681);
   /// @brief Create a participant which will try to connect to a site.
   /// @param ipAddress The IP address of the site
   /// @param port The port used by the site
-  /// @param localPort The port used by the local participant
+  ParticipantUDP(const char* ipAddress,
-  ParticipantUDPGeneric(const char* ipAddress,
+                   int port = 7681,
-                        int port = 7681,
+                   int localPort = 7681);
-                        int localPort = 7681);
+  // Note to self: one cannot specify the port used by the local participant
+  // now!!
 
-#pragma endregion Init
+  /// @brief Isolated participant is used when the application is run without
+  /// networking
+  /// @return A participant without networking support
+  static ParticipantUDP* Isolated();
 
-#pragma region Properties
-
- public:
   /// @brief True if the participant is running isolated.
   /// Isolated participants do not communicate with other participants
   bool isIsolated = false;
 
-  /// @brief The remote site when this participant is connected to a site
-  RemoteParticipantUDP* remoteSite = nullptr;
-
   /// The interval in milliseconds for publishing (broadcasting) data on the
   /// local network
   long publishInterval = 3000;  // 3 seconds
 
- protected:
+  /// @brief The name of the participant
-#if !defined(ARDUINO)
+  const char* name = "ParticipantUDP";
+
+  // int localPort = 0;
+
+  /// @brief The remote site when this participant is connected to a site
+  Participant* remoteSite = nullptr;
+
+#if defined(ARDUINO)
+  // const char* remoteIpAddress = nullptr;
+  // unsigned short remotePort = 0;
+  // char* broadcastIpAddress = nullptr;
+
+  // WiFiUDP udp;
+#else
+
 #if defined(__unix__) || defined(__APPLE__)
   int sock;
 #elif defined(_WIN32) || defined(_WIN64)
@@ -154,73 +93,81 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
   sockaddr_in server_addr;
   sockaddr_in broadcast_addr;
 #endif
+
 #endif
- public:
+
   void begin();
   bool connected = false;
 
-#pragma endregion Properties
+  virtual void Update(unsigned long currentTimeMs = 0) override;
-
-#pragma region Update
-
- public:
-  virtual void Update(bool recurse = true) override;
-
- protected:
-  unsigned long nextPublishMe = 0;
-
-  /// @brief Prepare the local things for the next update
-  // virtual void PrepMyThings();
-  virtual void UpdateMyThings();
-  virtual void UpdateOtherThings();
-
-#pragma endregion Update
-
-#pragma region Send
 
   void SendThingInfo(Participant* remoteParticipant, Thing* thing);
   void PublishThingInfo(Thing* thing);
 
-  virtual bool Send(IMessage* msg) override;
+  bool Send(Participant* remoteParticipant, IMessage* msg);
-  virtual bool Publish(IMessage* msg) = 0;
+  bool Publish(IMessage* msg);
 
-#pragma endregion Send
-
-#pragma region Receive
-
- protected:
   void ReceiveData(unsigned char bufferSize,
                    char* senderIpAddress,
                    unsigned int senderPort);
-  void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant);
+  void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
 
-  virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0;
+#if defined(NO_STD)
+  unsigned char senderCount = 0;
+  Participant* senders[MAX_SENDER_COUNT];
+#else
+  std::list<Participant*> senders;
+#endif
 
-  virtual void ReceiveUDP() = 0;
+ protected:
+  unsigned long nextPublishMe = 0;
 
-  virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg);
+  char buffer[1024];
-  virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg);
 
-  virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg);
+  void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
-  virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender,
-                                 ThingMsg* msg,
-                                 bool isRemote);
 
-  virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg);
+  Participant* GetParticipant(const char* ipAddress, int port);
-  virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg);
+  Participant* AddParticipant(const char* ipAddress, int port);
-  virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg);
 
-#pragma endregion Receive
+  void ReceiveUDP();
 
-public:
+  virtual void Process(Participant* sender, ParticipantMsg* msg);
-  static ParticipantRegistry registry;
+  virtual void Process(Participant* sender, SiteMsg* msg);
+  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);
 
+#if !defined(NO_STD)
+//  public:
+//   using ThingConstructor = std::function<Thing*(Participant* participant,
+//                                                 unsigned char networkId,
+//                                                 unsigned char thingId)>;
+
+//   template <typename ThingClass>
+//   void Register(unsigned char thingType) {
+//     thingMsgProcessors[thingType] = [](Participant* participant,
+//                                        unsigned char networkId,
+//                                        unsigned char thingId) {
+//       return new ThingClass(participant, networkId, thingId);
+//     };
+//   };
+
+//   template <typename ThingClass>
+//   void Register2(unsigned char thingType, ThingConstructor f) {
+//     thingMsgProcessors[thingType] = [f](Participant* participant,
+//                                        unsigned char networkId,
+//                                        unsigned char thingId) {
+//       return f(participant, networkId, thingId);
+//     };
+//   };
+
+//  protected:
+//   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
+
+#endif
 };
 
 }  // namespace RoboidControl
-
-#include "EspIdf/EspIdfParticipant.h"

Participants/SiteServer.cpp

@@ -9,89 +9,58 @@
 
 namespace RoboidControl {
 
-#pragma region Init
+SiteServer::SiteServer(int port) {
-
-SiteServer::SiteServer(int port) : ParticipantUDPGeneric(port) {
   this->name = "Site Server";
   this->publishInterval = 0;
 
-  //SetupUDP(port, ipAddress, 0);
+  this->ipAddress = "0.0.0.0";
-}
+  this->port = port;
 
-#pragma endregion Init
-
-#pragma region Update
-
-void SiteServer::UpdateMyThings() {
-  for (Thing* thing : this->things) {
-    if (thing == nullptr)
-      continue;
-
-    thing->Update(true);
-
-    if (this->isIsolated == false) {
-      // Send to all other participants
 #if defined(NO_STD)
-      Participant** participants = Participant::registry.GetAll();
+  this->senders[this->senderCount++] = this;
-      for (int ix = 0; ix < Participant::registry.count; ix++) {
-        Participant* participant = participants[ix];
 #else
-      for (Participant* participant : registry.GetAll()) {
+  this->senders.push_back(this);
 #endif
-        if (participant == nullptr || participant == this)
-          continue;
 
-        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+  SetupUDP(port, ipAddress, 0);
-        participant->Send(poseMsg);
+
-        delete poseMsg;
+#if !defined(NO_STD)
-        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+  // Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
-        participant->Send(binaryMsg);
+#endif
-        delete binaryMsg;
-      }
-    }
-  }
 }
 
-#pragma endregion Update
+void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
-
+  if (msg->networkId == 0) {
-#pragma region Receive
-
-void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
-  if (msg->networkId != sender->networkId) {
     // std::cout << this->name << " received New Client -> " <<
     // sender->ipAddress
     //           << ":" << (int)sender->port << "\n";
-    NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
+    SiteMsg* msg = new SiteMsg(sender->networkId);
-    sender->Send(msg);
+    this->Send(sender, msg);
     delete msg;
   }
 }
 
-void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {}
+void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
 
-void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* 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) {
-    // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
+    // #if defined(NO_STD)
-    // Thing::Reconstruct(sender, msg->thingType, msg->thingId);
+    new Thing(sender, (Thing::Type)msg->thingType,
-    //thing = new Thing(msg->thingType, sender->root);
+              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
   }
-  thing->id = msg->thingId;
-
-  if (msg->parentId != 0) {
-    thing->SetParent(Get(msg->networkId, msg->parentId));
-    if (thing->IsRoot())
-    // if (thing->GetParent() != nullptr)
-#if defined(NO_STD)
-      ;
-#else
-      std::cout << "Could not find parent [" << (int)msg->networkId << "/"
-                << (int)msg->parentId << "]\n";
-#endif
-  } else
-    thing->SetParent(Thing::LocalRoot());
 }
 
-#pragma endregion Receive
-
 }  // namespace RoboidControl

Participants/SiteServer.h

@@ -2,43 +2,45 @@
 
 #include "ParticipantUDP.h"
 
-// #if !defined(NO_STD)
+#if !defined(NO_STD)
-// #include <functional>
+#include <functional>
-// #include <memory>
+#include <memory>
-// #include <unordered_map>
+#include <unordered_map>
-// #endif
+#endif
 
 namespace RoboidControl {
 
 /// @brief A participant is device which can communicate with other participants
-class SiteServer : public ParticipantUDPGeneric {
+class SiteServer : public ParticipantUDP {
-  
-#pragma region Init
-
  public:
-  /// @brief Create a new site server
-  /// @param port The port of which to receive the messages
   SiteServer(int port = 7681);
 
-#pragma endregion Init
+  // virtual void Update(unsigned long currentTimeMs = 0) override;
 
-#pragma region Update
+// #if !defined(NO_STD)
-
+//   template <typename ThingClass>
-  virtual void UpdateMyThings() override;
+//   void Register(unsigned char thingType) {
-
+//     thingMsgProcessors[thingType] = [](Participant* participant,
-#pragma endregion Update
+//                                        unsigned char networkId,
-
+//                                        unsigned char thingId) {
-#pragma region Receive
+//       return new ThingClass(participant, networkId, thingId);
+//     };
+//   };
+// #endif
 
  protected:
   unsigned long nextPublishMe = 0;
 
-  virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) override;
+  virtual void Process(Participant* sender, ParticipantMsg* msg) override;
-  virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override;
+  virtual void Process(Participant* sender, SiteMsg* msg) override;
-  virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override;
+  virtual void Process(Participant* sender, ThingMsg* msg) override;
-
-#pragma endregion Receive
 
+// #if !defined(NO_STD)
+//   using ThingConstructor = std::function<Thing*(Participant* participant,
+//                                                 unsigned char networkId,
+//                                                 unsigned char thingId)>;
+//   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
+// #endif
 };
 
 }  // namespace RoboidControl

Posix/PosixParticipant.cpp

@@ -1,5 +1,4 @@
 #include "PosixParticipant.h"
-#if defined(__unix__) || defined(__APPLE__)
 
 #if defined(__unix__) || defined(__APPLE__)
 #include <arpa/inet.h>
@@ -10,8 +9,9 @@
 #endif
 
 namespace RoboidControl {
+namespace Posix {
 
-void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+void Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #if defined(__unix__) || defined(__APPLE__)
 
   // Create a UDP socket
@@ -63,7 +63,7 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
 #endif
 }
 
-void ParticipantUDP::Receive() {
+void Receive() {
 #if defined(__unix__) || defined(__APPLE__)
   sockaddr_in client_addr;
   socklen_t len = sizeof(client_addr);
@@ -74,9 +74,9 @@ void ParticipantUDP::Receive() {
     unsigned int sender_port = ntohs(client_addr.sin_port);
 
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    // RoboidControl::Participant* remoteParticipant = this->Get(sender_ipAddress, sender_port);
+    // RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
     // if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
+    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
     //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
     //   //           << "\n";
     //   // std::cout << "New remote participant " << remoteParticipant->ipAddress
@@ -90,7 +90,7 @@ void ParticipantUDP::Receive() {
 #endif
 }
 
-bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
+bool Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(__unix__) || defined(__APPLE__)
   // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
   //           << "\n";
@@ -113,7 +113,7 @@ bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferS
   return true;
 }
 
-bool ParticipantUDP::Publish(IMessage* msg) {
+bool Publish(IMessage* msg) {
 #if defined(__unix__) || defined(__APPLE__)
   int bufferSize = msg->Serialize(this->buffer);
   if (bufferSize <= 0)
@@ -132,6 +132,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   return true;
 }
 
+}  // namespace Posix
 }  // namespace RoboidControl
-
-#endif

Posix/PosixParticipant.h

@@ -1,24 +1,17 @@
 #pragma once
-#if defined(__unix__) || defined(__APPLE__)
 
 #include "Participants/ParticipantUDP.h"
 
 namespace RoboidControl {
+namespace Posix {
 
-class ParticipantUDP : public ParticipantUDPGeneric {
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
-
- protected:
-#if defined(__unix__) || defined(__APPLE__)
-  sockaddr_in remote_addr;
-  sockaddr_in server_addr;
-  sockaddr_in broadcast_addr;
-#endif
 };
 
+}  // namespace Posix
 }  // namespace RoboidControl
-#endif

README.md

@@ -14,60 +14,5 @@ Supporting:
 # Basic components
 
 - RoboidControl::Thing
-- RoboidControl::Participant
+- RoboidControl::LocalParticipant
-
+- RoboidControl::SiteServer
-# Installation
-
-## Core code
-
-The repository uses cmake for building. You can place it in a subfolder of your project and include it in you `CMakeLists.txt`.
-For example if the library is placed in the subfolder `roboidcontrol`:
-```
-# Add the path to Roboid Control
-add_subdirectory(roboidcontrol)
-
-# Your source files/executable
-add_executable(my_executable main.cpp) 
-
-# Link against RoboidControl
-target_link_libraries(my_executable RoboidControl)
-```
-
-## Arduino (PlatformIO)
-
-Arduino is only supported in combination with PlatformIO. The Arduino IDE is not (yet?) supported.
-
-The best way to include support for Roboid Control in PlatformIO is
-to clone the Roboid Control for C++ repository into a subfolder of the /lib folder.
-Alternatively you can download the zip file and unpack it as a subfolder of the /lib folder.
-
-## ESP-IDF
-
-The best way to include support for Roboid Control in PlatformIO is
-to clone the Roboid Control for C++ repository into a subfolder of the /components folder.
-Alternatively you can download the zip file and unpack it as a subfolder of the /components folder.
-
-Make sure you have included RoboidControl as a component in your top-level CMakeLists.txt, for example:
-```
-list(APPEND EXTRA_COMPONENT_DIRS
-    components/RoboidControl    
-    )
-```
-
-# Get Started
-
-## Core C++ Examples
-
-This repository contains examples in the `examples` folder. You can build these using cmake.
-
-For example, to build the BB2A example:
-``` 
-cmake -B build -D BUILD_EXAMPLE_BB2A=ON
-cmake --build build
-```
-The resulting executable is then `build/examples/Debug/BB2A.exe`
-
-## Arduino (PlatformIO) Examples
-
-Specific examples for the Arduino platform are found in the `Arduino\examples` folder.
-To use them you should create a new project in PlatformIO and then copy the example code to your project.

Testing/Temporary/LastTest.log

@@ -1,3 +0,0 @@
-Start testing: Jun 17 17:17 W. Europe Summer Time
-----------------------------------------------------------
-End testing: Jun 17 17:17 W. Europe Summer Time

Thing.cpp

@@ -5,7 +5,6 @@
 #include "Participants/IsolatedParticipant.h"
 
 #include <string.h>
-// #include <iostream>
 
 #if defined(ARDUINO)
 #include "Arduino.h"
@@ -18,78 +17,72 @@
 
 namespace RoboidControl {
 
-#pragma region Init
+// LocalParticipant* Thing::CheckHiddenParticipant() {
+//   if (isolatedParticipant == nullptr)
+//     isolatedParticipant = new LocalParticipant(0);
+//   return isolatedParticipant;
+// }
 
-Thing* Thing::LocalRoot() {
+Thing::Thing(int thingType)
-  Participant* p = Participant::LocalParticipant;
+    : Thing(IsolatedParticipant::Isolated(), thingType) {}
-  Thing* localRoot = p->root;
-  return localRoot;
-}
 
-// Only use this for root things
+Thing::Thing(Participant* owner, Type thingType, unsigned char thingId)
-Thing::Thing(Participant* owner) {
+    : Thing(owner, (unsigned char)thingType, thingId) {}
-  this->type = Type::Root;
-  this->name = "Root";
-
-  this->position = Spherical::zero;
-  this->positionUpdated = true;
-  this->orientation = SwingTwist::identity;
-  this->orientationUpdated = true;
-  this->hierarchyChanged = true;
-
-  this->linearVelocity = Spherical::zero;
-  this->angularVelocity = Spherical::zero;
 
+Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
   this->owner = owner;
-  this->owner->Add(this);
+  this->id = thingId;
-  std::cout << this->owner->name << ": New root thing " << std::endl;
+  this->type = thingType;
-}
+  this->networkId = 0;
-
-void Thing::CreateRoot(Participant* owner) {
-  owner->root = new Thing(owner);
-}
-
-Thing::Thing(Thing* parent) {
-  this->type = Type::Undetermined;
 
   this->position = Spherical::zero;
-  this->positionUpdated = true;
   this->orientation = SwingTwist::identity;
-  this->orientationUpdated = true;
-  this->hierarchyChanged = true;
 
   this->linearVelocity = Spherical::zero;
   this->angularVelocity = Spherical::zero;
 
-  this->owner = parent->owner;
+  // std::cout << "add thing [" << (int)this->id << "] to owner "
+  //           << this->owner->ipAddress << ":" << this->owner->port << std::endl;
   this->owner->Add(this, true);
-  this->SetParent(parent);
-
-  std::cout << this->owner->name << ": New thing for " << parent->name
-            << std::endl;
 }
 
-Thing::~Thing() {
+// Thing::Thing(Participant* owner,
-  std::cout << "Destroy thing " << this->name << std::endl;
+//             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() {
+  // Thing::Remove(this);
 }
 
-#pragma endregion Init
+Thing* Thing::FindThing(const char* name) {
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    Thing* child = this->children[childIx];
+    if (child == nullptr || child->name == nullptr)
+      continue;
 
-void Thing::SetName(const char* name) {
+    if (strcmp(child->name, name) == 0)
-  this->name = name;
+      return child;
-  this->nameChanged = true;
+
+    Thing* foundChild = child->FindThing(name);
+    if (foundChild != nullptr)
+      return foundChild;
+  }
+  return nullptr;
 }
 
-const char* Thing::GetName() const {
-  return this->name;
-}
-
-void Thing::SetModel(const char* url) {
-  this->modelUrl = url;
-}
-
-#pragma region Hierarchy
-
 void Thing::SetParent(Thing* parent) {
   if (parent == nullptr) {
     Thing* parentThing = this->parent;
@@ -98,21 +91,18 @@ void Thing::SetParent(Thing* parent) {
     this->parent = nullptr;
   } else
     parent->AddChild(this);
-  this->hierarchyChanged = true;
 }
 
-bool Thing::IsRoot() const {
+void Thing::SetParent(Thing* root, const char* name) {
-  return this == LocalRoot() || this->parent == nullptr;
+  Thing* thing = root->FindThing(name);
+  if (thing != nullptr)
+    this->SetParent(thing);
 }
 
 Thing* Thing::GetParent() {
   return this->parent;
 }
 
-Thing* Thing::GetChildByIndex(unsigned char ix) {
-  return this->children[ix];
-}
-
 void Thing::AddChild(Thing* child) {
   unsigned char newChildCount = this->childCount + 1;
   Thing** newChildren = new Thing*[newChildCount];
@@ -152,7 +142,7 @@ Thing* Thing::RemoveChild(Thing* child) {
     }
   }
 
-  child->parent = Thing::LocalRoot();
+  child->parent = nullptr;
 
   delete[] this->children;
   this->children = newChildren;
@@ -161,7 +151,7 @@ Thing* Thing::RemoveChild(Thing* child) {
   return child;
 }
 
-Thing* Thing::GetChild(unsigned char id, bool recurse) {
+Thing* Thing::GetChild(unsigned char id, bool recursive) {
   for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
     Thing* child = this->children[childIx];
     if (child == nullptr)
@@ -169,8 +159,8 @@ Thing* Thing::GetChild(unsigned char id, bool recurse) {
     if (child->id == id)
       return child;
 
-    if (recurse) {
+    if (recursive) {
-      Thing* foundChild = child->GetChild(id, recurse);
+      Thing* foundChild = child->GetChild(id, recursive);
       if (foundChild != nullptr)
         return foundChild;
     }
@@ -178,25 +168,57 @@ Thing* Thing::GetChild(unsigned char id, bool recurse) {
   return nullptr;
 }
 
-Thing* Thing::FindChild(const char* name, bool recurse) {
+Thing* Thing::GetChildByIndex(unsigned char ix) {
-  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+  return this->children[ix];
-    Thing* child = this->children[childIx];
-    if (child == nullptr || child->name == nullptr)
-      continue;
-
-    if (strcmp(child->name, name) == 0)
-      return child;
-
-    Thing* foundChild = child->FindChild(name);
-    if (foundChild != nullptr)
-      return foundChild;
-  }
-  return nullptr;
 }
 
-#pragma endregion Hierarchy
+void Thing::SetModel(const char* url) {
+  this->modelUrl = url;
+}
 
-#pragma region Pose
+unsigned long Thing::GetTimeMs() {
+#if defined(ARDUINO)
+  return millis();
+#else
+  auto now = std::chrono::steady_clock::now();
+  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+      now.time_since_epoch());
+  return static_cast<unsigned long>(ms.count());
+#endif
+}
+
+void Thing::Update(bool recursive) {
+  Update(GetTimeMs(), recursive);
+}
+
+void Thing::Update(unsigned long currentTimeMs, bool recursive) {
+  // 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];
+      if (child == nullptr)
+        continue;
+      child->Update(currentTimeMs, recursive);
+    }
+  }
+}
+
+void Thing::UpdateThings(unsigned long currentTimeMs) {
+  IsolatedParticipant::Isolated()->Update(currentTimeMs);
+}
+
+int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
+  (void)buffer;
+  (void)ix;
+  return 0;
+}
+void Thing::ProcessBinary(char* bytes) {
+  (void)bytes;
+};
 
 void Thing::SetPosition(Spherical position) {
   this->position = position;
@@ -216,10 +238,8 @@ SwingTwist Thing::GetOrientation() {
 }
 
 void Thing::SetLinearVelocity(Spherical linearVelocity) {
-  if (this->linearVelocity.distance != linearVelocity.distance) {
+  this->linearVelocity = linearVelocity;
-    this->linearVelocity = linearVelocity;
+  this->linearVelocityUpdated = true;
-    this->linearVelocityUpdated = true;
-  }
 }
 
 Spherical Thing::GetLinearVelocity() {
@@ -227,60 +247,12 @@ Spherical Thing::GetLinearVelocity() {
 }
 
 void Thing::SetAngularVelocity(Spherical angularVelocity) {
-  if (this->angularVelocity.distance != angularVelocity.distance) {
+  this->angularVelocity = angularVelocity;
-    this->angularVelocity = angularVelocity;
+  this->angularVelocityUpdated = true;
-    this->angularVelocityUpdated = true;
-  }
 }
 
 Spherical Thing::GetAngularVelocity() {
   return this->angularVelocity;
 }
 
-#pragma endregion Pose
-
-#pragma region Update
-
-unsigned long Thing::GetTimeMs() {
-#if defined(ARDUINO)
-  unsigned long ms = millis();
-  return ms;
-#else
-  auto now = std::chrono::steady_clock::now();
-  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
-      now.time_since_epoch());
-  return static_cast<unsigned long>(ms.count());
-#endif
-}
-
-void Thing::Update(bool recursive) {
-  this->positionUpdated = false;
-  this->orientationUpdated = false;
-  this->linearVelocityUpdated = false;
-  this->angularVelocityUpdated = false;
-  this->hierarchyChanged = false;
-  this->nameChanged = false;
-
-  if (recursive) {
-     std::cout << "# children: " << (int)this->childCount << std::endl;
-    for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-      Thing* child = this->children[childIx];
-      if (child == nullptr)
-        continue;
-      child->Update(recursive);
-    }
-  }
-}
-
-#pragma endregion Update
-
-int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
-  (void)buffer;
-  (void)ix;
-  return 0;
-}
-void Thing::ProcessBinary(char* bytes) {
-  (void)bytes;
-};
-
 }  // namespace RoboidControl

Thing.h

@@ -10,7 +10,7 @@
 namespace RoboidControl {
 
 class Participant;
-class ParticipantUDPGeneric;
+class ParticipantUDP;
 
 #define THING_STORE_SIZE 256
 // IMPORTANT: values higher than 256 will need to change the Thing::id type
@@ -20,122 +20,71 @@ class ParticipantUDPGeneric;
 class Thing {
  public:
   /// @brief Predefined thing types
-  struct Type {
+  enum Type {
-    static const unsigned char Undetermined = 0x00;
+    Undetermined,
-    // Sensor
+    // Sensor,
-    static const unsigned char Switch = 0x01;
+    Switch,
-    static const unsigned char DistanceSensor = 0x02;
+    DistanceSensor,
-    static const unsigned char DirectionalSensor = 0x03;
+    DirectionalSensor,
-    static const unsigned char TemperatureSensor = 0x04;
+    TemperatureSensor,
-    static const unsigned char TouchSensor = 0x05;
+    TouchSensor,
-    // Motor
+    // Motor,
-    static const unsigned char ControlledMotor = 0x06;
+    ControlledMotor,
-    static const unsigned char UncontrolledMotor = 0x07;
+    UncontrolledMotor,
-    static const unsigned char Servo = 0x08;
+    Servo,
-    static const unsigned char RelativeEncoder = 0x19;
     // Other
-    static const unsigned char Root = 0x10;
+    Roboid,
-    static const unsigned char Roboid = 0x09;
+    Humanoid,
-    static const unsigned char Humanoid = 0x0A;
+    ExternalSensor,
-    static const unsigned char ExternalSensor = 0x08;
-    static const unsigned char Animator = 0x0C;
-    static const unsigned char DifferentialDrive = 0x0D;
   };
 
-#pragma region Init
+  /// @brief Create a new thing using an implicit local participant
+  /// @param thingType The type of 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,
+        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, 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,
+  //       unsigned char networkId,
+  //       unsigned char thingId,
+  //       Type thingType = Type::Undetermined);
 
- private:
+  /// @brief The participant managing this thing
- public:
+  Participant* owner;
-  /// @brief Create a new Thing
+  /// @brief The network ID of this thing
-  /// @param parent (optional) The parent thing
+  /// @note This field will likely disappear in future versions
-  /// The owner will be the same as the owner of the parent thing, it will
+  unsigned char networkId = 0;
-  /// be Participant::LocalParticipant if the parent is not specified. A thing
-  /// without a parent will be connected to the root thing.
-  Thing(Thing* parent = LocalRoot());
-
- private:
-  /// @brief Constructor to create a root thing
-  /// @param owner The participant who will own this root thing
-  /// @remarks This function is private because CreateRoot() should be used
-  /// instead
-  Thing(Participant* owener);
-
- public:
-  /// @brief Destructor for a Thing
-  ~Thing();
-
-  /// @brief Create a root thing for a participant
-  /// @param owner The participant who will own this root thing
-  static void CreateRoot(Participant* owner);
-
-  /// @brief The root thing for the local participant
-  /// @return The root thing for the local participant
-  static Thing* LocalRoot();
-
-#pragma endregion Init
-
- public:
-  /// @brief Terminated things are no longer updated
-  bool terminate = false;
-
-#pragma region Properties
-
- public:
   /// @brief The ID of the thing
   unsigned char id = 0;
 
   /// @brief The type of Thing
   /// This can be either a Thing::Type of a byte value for custom types
-  unsigned char type = Type::Undetermined;
+  unsigned char type = 0;
 
-  /// @brief Is this a remote thing?
+  /// @brief Find a thing by name
-  /// A remote thing is owned by other participant
+  /// @param name Rhe name of the thing
-  /// and is not simulated by the local participant
+  /// @return The found thing or nullptr when nothing is found
-  bool isRemote = false;
+  Thing* FindThing(const char* name);
 
-  /// @brief The participant owning this thing
+  /// @brief Sets the parent Thing
-  Participant* owner = nullptr;
+  /// @param parent The Thing which should become the parnet
-
+  /// @remark This is equivalent to calling parent->AddChild(this);
-  /// @brief The name of the thing
+  virtual void SetParent(Thing* parent);
-  const char* name = nullptr;
+  void SetParent(Thing* root, const char* name);
-
+  /// @brief Gets the parent Thing
-  void SetName(const char* name);
-  const char* GetName() const;
-  bool nameChanged = false;
-
-  /// @brief Sets the location from where the 3D model of this Thing can be
-  /// loaded from
-  /// @param url The url of the model
-  /// @remark Although the roboid implementation is not dependent on the model,
-  /// the only official supported model formats are .png (sprite), .gltf and .glb
-  void SetModel(const char* url);
-
-  /// @brief An URL pointing to the location where a model of the thing can be
-  /// found
-  const char* modelUrl = nullptr;
-
-#pragma endregion Properties
-
-#pragma region Hierarchy
-
-  /// @brief Sets the parent of this Thing
-  /// @param parent The Thing which should become the parent
-  void SetParent(Thing* parent);
-  /// @brief Gets the parent of this Thing
   /// @return The parent Thing
   Thing* GetParent();
 
-  /// @brief Check if this is a root thing
-  /// @return True is this thing is a root
-  bool IsRoot() const;
-
-  /// @brief The number of children
-  unsigned char childCount = 0;
-  /// @brief Get a child by index
-  /// @param ix The child index
-  /// @return The found thing of nullptr when nothing is found
-  Thing* GetChildByIndex(unsigned char ix);
-
   /// @brief Add a child Thing to this Thing
   /// @param child The Thing which should become a child
   /// @remark When the Thing is already a child, it will not be added again
@@ -145,46 +94,49 @@ class Thing {
   /// @return The removed child or nullptr if the child could not be found
   Thing* RemoveChild(Thing* child);
 
+  /// @brief The number of children
+  unsigned char childCount = 0;
   /// @brief Get a child by thing Id
   /// @param id The thing ID to find
-  /// @param recurse Look recursively through all descendants
+  /// @param recursive Look recursively through all descendants
   /// @return The found thing of nullptr when nothing is found
-  Thing* GetChild(unsigned char id, bool recurse = false);
+  Thing* GetChild(unsigned char id, bool recursive = false);
+  /// @brief Get a child by index
+  /// @param ix The child index
+  /// @return The found thing of nullptr when nothing is found
+  Thing* GetChildByIndex(unsigned char ix);
 
-  /// @brief Find a thing by name
+ protected:
-  /// @param name The name of the thing
-  /// @param recurse Look recursively through all descendants
-  /// @return The found thing or nullptr when nothing is found
-  Thing* FindChild(const char* name, bool recurse = true);
-
-  /// @brief Indicator that the hierarchy of the thing has changed
-  bool hierarchyChanged = true;
-
- private:
   Thing* parent = nullptr;
   Thing** children = nullptr;
 
-#pragma endregion Hierarchy
-
-#pragma region Pose
-
  public:
+  /// @brief The name of the thing
+  const char* name = nullptr;
+  /// @brief An URL pointing to the location where a model of the thing can be
+  /// found
+  const char* modelUrl = nullptr;
+  /// @brief The scale of the model (deprecated I think)
+  float modelScale = 1;
+
   /// @brief Set the position of the thing
   /// @param position The new position in local space, in meters
   void SetPosition(Spherical position);
   /// @brief Get the position of the thing
   /// @return The position in local space, in meters
   Spherical GetPosition();
-  /// @brief Boolean indicating that the thing has an updated position
-  bool positionUpdated = false;
-
   /// @brief Set the orientation of the thing
   /// @param orientation The new orientation in local space
   void SetOrientation(SwingTwist orientation);
   /// @brief Get the orientation of the thing
   /// @return The orienation in local space
   SwingTwist GetOrientation();
-  /// @brief Boolean indicating the thing has an updated orientation
+  /// @brief The scale of the thing (deprecated I think)
+  // float scale = 1;  // assuming uniform scale
+
+  /// @brief boolean indicating if the position was updated
+  bool positionUpdated = false;
+  /// @brief boolean indicating if the orientation was updated
   bool orientationUpdated = false;
 
   /// @brief Set the linear velocity of the thing
@@ -194,59 +146,57 @@ class Thing {
   /// @brief Get the linear velocity of the thing
   /// @return The linear velocity in local space, in meters per second
   virtual Spherical GetLinearVelocity();
-  /// @brief Boolean indicating the thing has an updated linear velocity
-  bool linearVelocityUpdated = false;
-
   /// @brief Set the angular velocity of the thing
   /// @param angularVelocity the new angular velocity in local space
   virtual void SetAngularVelocity(Spherical angularVelocity);
   /// @brief Get the angular velocity of the thing
   /// @return The angular velocity in local space
   virtual Spherical GetAngularVelocity();
-  /// @brief Boolean indicating the thing has an updated angular velocity
+  bool linearVelocityUpdated = false;
   bool angularVelocityUpdated = false;
 
  private:
-  /// @brief The position of the thing in local space, in meters
+  /// @brief The position in local space
   /// @remark When this Thing has a parent, the position is relative to the
   /// parent's position and orientation
   Spherical position;
-  /// @brief The orientation of the thing in local space
+  /// @brief The orientation in local space
   /// @remark When this Thing has a parent, the orientation is relative to the
   /// parent's orientation
   SwingTwist orientation;
 
-  /// @brief The linear velocity of the thing in local space, in meters per
+  /// @brief The linear velocity in local space
-  /// second
   Spherical linearVelocity;
-  /// @brief The angular velocity of the thing in local space, in degrees per
+  /// @brief The angular velocity in local spze
-  /// second
   Spherical angularVelocity;
 
-#pragma endregion Pose
-
-#pragma region Update
-
  public:
-  //virtual void PrepareForUpdate();
+  /// @brief Terminated things are no longer updated
+  void Terminate();
 
-  /// @brief Updates the state of the thing
+  /// @brief Sets the location from where the 3D model of this Thing can be
-  /// @param recurse When true, this will Update the descendants recursively
+  /// loaded from
-  virtual void Update(bool recurse = false);
+  /// @param url The url of the model
+  /// @remark Although the roboid implementation is not dependent on the model,
+  /// the only official supported model format is .obj
+  void SetModel(const char* url);
 
-  /// @brief Get the current time in milliseconds
-  /// @return The current time in milliseconds
   static unsigned long GetTimeMs();
 
-#pragma endregion Update
+  void Update(bool recursive = false);
+
+  /// @brief Updates the state of the thing
+  /// @param currentTimeMs The current clock time in milliseconds
+  virtual void Update(unsigned long currentTimeMs, bool recursive = false);
+
+  static void UpdateThings(unsigned long currentTimeMs);
 
- public:
   /// @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
-  /// @return The size of the binary data
+  /// @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
+  // /// @brief FUnction used to process binary data received for this thing
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes);
 };

Things/ControlledMotor.cpp

@@ -1,68 +0,0 @@
-#include "ControlledMotor.h"
-#include "LinearAlgebra/FloatSingle.h"
-
-namespace RoboidControl {
-
-ControlledMotor::ControlledMotor(Motor* motor,
-                                 RelativeEncoder* encoder,
-                                 Thing* parent)
-    : Motor(parent), motor(motor), encoder(encoder) {
-  this->type = Type::ControlledMotor;
-  //encoder->SetParent(null);
-  //  Thing parent = motor.GetParent();
-  //  this->SetParent(parent);
-  this->integral = 0;
-}
-
-void ControlledMotor::SetTargetVelocity(float velocity) {
-  this->targetVelocity = velocity;
-  this->rotationDirection =
-      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
-}
-
-void ControlledMotor::Update(bool recurse) {
-  unsigned long currentTimeMs = GetTimeMs();
-  float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
-  this->lastUpdateTime = currentTimeMs;
-  if (timeStep <= 0)
-    return;
-
-  // encoder->Update(false);
-
-  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
-
-  float error = this->targetVelocity - this->actualVelocity;
-  float p_term = error * pidP;
-  this->integral += error * timeStep;
-  float i_term = pidI * this->integral;
-  float d_term = pidD * (error - this->lastError) / timeStep;
-  this->lastError = error;
-
-  float output = p_term + i_term + d_term;
-  // std::cout << "target " << this->targetVelocity << " actual "
-  //           << this->actualVelocity << " output = " << output << std::endl;
-  // float acceleration =
-  //     error * timeStep * pidP;  // Just P is used at this moment
-  // std::cout << "motor acc. " << acceleration << std::endl;
-
-  // float newTargetVelocity = motor->targetVelocity + acceleration;
-  output = LinearAlgebra::Float::Clamp(output, -1, 1);
-  motor->SetTargetVelocity(output);  // or something like that
-  //motor->Update(false);
-}
-
-// float ControlledMotor::GetActualVelocity() {
-//   return (int)rotationDirection * encoder->rotationSpeed;
-// }
-
-// bool ControlledMotor::Drive(float distance) {
-//   if (!driving) {
-//     targetDistance = distance;
-//     startDistance = encoder->GetDistance();
-//     driving = true;
-//   }
-//   float totalDistance = encoder->GetDistance() - startDistance;
-//   bool completed = totalDistance > targetDistance;
-//   return completed;
-// }
-}  // namespace RoboidControl

Things/ControlledMotor.h

@@ -1,40 +0,0 @@
-#pragma once
-
-#include "Motor.h"
-#include "RelativeEncoder.h"
-
-namespace RoboidControl {
-
-/// @brief A motor with speed control
-/// It uses a feedback loop from an encoder to regulate the speed
-/// The speed is measured in revolutions per second.
-class ControlledMotor : public Motor {
- public:
-  ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());
-
-  float pidP = 0.5;
-  float pidD = 0;
-  float pidI = 0.2;
-
-  /// @brief The actual velocity in revolutions per second
-  float actualVelocity;
-
-  enum Direction { Forward = 1, Reverse = -1 };
-  Direction rotationDirection;
-
-  virtual void Update(bool recurse = false) override;
-
-  /// @brief Set the target verlocity for the motor controller
-  /// @param speed the target velocity in revolutions per second
-  virtual void SetTargetVelocity(float velocity) override;
-
-  Motor* motor;
-  RelativeEncoder* encoder;
-
- protected:
-  float integral = 0;
-  float lastError = 0;
-  float lastUpdateTime;
-};
-
-}  // namespace RoboidControl

Things/DifferentialDrive.cpp

@@ -1,29 +1,10 @@
 #include "DifferentialDrive.h"
 
-#include "Messages/LowLevelMessages.h"
-
 namespace RoboidControl {
+DifferentialDrive::DifferentialDrive() : Thing() {}
 
-DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent) {
+RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
-  this->type = Type::DifferentialDrive;
+    : Thing(participant) {}
-  this->name = "Differential drive";
-
-  this->leftWheel = new Motor(this);
-  this->leftWheel->name = "Left motor";
-
-  this->rightWheel = new Motor(this);
-  this->rightWheel->name = "Right motor";
-}
-
-DifferentialDrive::DifferentialDrive(Motor* leftMotor,
-                                     Motor* rightMotor,
-                                     Thing* parent)
-    : Thing(parent) {
-  this->type = Type::DifferentialDrive;
-  this->name = "Differential drive";
-  this->leftWheel = leftMotor;
-  this->rightWheel = rightMotor;
-}
 
 void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
                                            float wheelSeparation) {
@@ -40,66 +21,51 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
     this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-// Motor& DifferentialDrive::GetMotorLeft() {
+void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
-//   return *this->leftWheel;
-// }
-
-// Motor& DifferentialDrive::GetMotorRight() {
-//   return *this->rightWheel;
-// }
-
-void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
   float distance = this->wheelSeparation / 2;
-  this->leftWheel = &leftMotor;
+  this->leftWheel = leftWheel;
-  this->leftWheel->SetPosition(Spherical(distance, Direction::left));
+  ;
+  if (leftWheel != nullptr)
+    this->leftWheel->SetPosition(Spherical(distance, Direction::left));
 
-  this->rightWheel = &rightMotor;
+  this->rightWheel = rightWheel;
-  this->rightWheel->SetPosition(Spherical(distance, Direction::right));
+  if (rightWheel != nullptr)
+    this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-void DifferentialDrive::SetWheelVelocity(float velocityLeft,
+void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
-                                         float velocityRight) {
-  // if (this->leftWheel != nullptr)
-  //   this->leftWheel->SetAngularVelocity(Spherical(velocityLeft,
-  //   Direction::left));
-  // if (this->rightWheel != nullptr)
-  //   this->rightWheel->SetAngularVelocity(
-  //       Spherical(velocityRight, Direction::right));
   if (this->leftWheel != nullptr)
-    this->leftWheel->SetTargetVelocity(velocityLeft);
+    this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
   if (this->rightWheel != nullptr)
-    this->rightWheel->SetTargetVelocity(velocityRight);
+    this->rightWheel->SetAngularVelocity(
+        Spherical(speedRight, Direction::right));
 }
 
-void DifferentialDrive::Update(bool recursive) {
+void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
-  if (this->linearVelocityUpdated) {
+  if (this->linearVelocityUpdated == false)
-    // this assumes forward velocity only....
+    return;
-    float linearVelocity = this->GetLinearVelocity().distance;
+  // this assumes forward velocity only....
+  float linearVelocity = this->GetLinearVelocity().distance;
 
-    Spherical angularVelocity = this->GetAngularVelocity();
+  Spherical angularVelocity = this->GetAngularVelocity();
-    float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
+  float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
-    // Determine the rotation direction
+  // Determine the rotation direction
-    if (angularVelocity.direction.horizontal.InDegrees() < 0)
+  if (angularVelocity.direction.horizontal.InDegrees() < 0)
-      angularSpeed = -angularSpeed;
+    angularSpeed = -angularSpeed;
 
-    // wheel separation can be replaced by this->leftwheel->position->distance
+  // wheel separation can be replaced by this->leftwheel->position->distance
-    float speedLeft =
+  float speedLeft =
-        (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
+      (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
-        this->wheelRadius * Rad2Deg;
+      this->wheelRadius * Rad2Deg;
-    float speedRight =
+  float speedRight =
-        (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
+      (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
-        this->wheelRadius * Rad2Deg;
+      this->wheelRadius * Rad2Deg;
 
-    this->SetWheelVelocity(speedLeft, speedRight);
+  this->SetWheelVelocity(speedLeft, speedRight);
-  }
+  Thing::Update(currentMs, recursive);
-  Thing::Update(recursive);
+  // std::cout << "lin. speed " << linearVelocity << "   ang. speed " <<
-}
+  // angularVelocity.distance << "   left wheel  "
-
+  //         << speedLeft << "   right wheel " << speedRight << "\n";
-int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
-  data[(*ix)++] = this->leftWheel->id;
-  data[(*ix)++] = this->rightWheel->id;
-  LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
-  return 4;
 }
 
 }  // namespace RoboidControl

Things/DifferentialDrive.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include "Thing.h"
-#include "Motor.h"
 
 namespace RoboidControl {
 
@@ -10,13 +9,11 @@ namespace RoboidControl {
 /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
 class DifferentialDrive : public Thing {
  public:
-  /// @brief Create a new child differential drive
+  /// @brief Create a differential drive without networking support
-  /// @param parent The parent thing
+  DifferentialDrive();
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
+  /// @brief Create a differential drive with networking support
-  /// an ID
+  /// @param participant The local participant
-  DifferentialDrive(Thing* parent = Thing::LocalRoot());
+  DifferentialDrive(Participant* participant);
-
-  DifferentialDrive(Motor* leftMotor, Motor* rightMotor, Thing* parent = Thing::LocalRoot());
 
   /// @brief Configures the dimensions of the drive
   /// @param wheelDiameter The diameter of the wheels in meters
@@ -26,41 +23,35 @@ class DifferentialDrive : public Thing {
   /// linear and angular velocity.
   /// @sa SetLinearVelocity SetAngularVelocity
   void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
-
-  // Motor& GetMotorLeft();
-  // Motor& GetMotorRight();
   /// @brief Congures the motors for the wheels
   /// @param leftWheel The motor for the left wheel
   /// @param rightWheel The motor for the right wheel
-  void SetMotors(Motor& leftMotor, Motor& rightMotor);
+  void SetMotors(Thing* leftWheel, Thing* rightWheel);
 
   /// @brief Directly specify the speeds of the motors
   /// @param speedLeft The speed of the left wheel in degrees per second.
   /// Positive moves the robot in the forward direction.
   /// @param speedRight The speed of the right wheel in degrees per second.
   /// Positive moves the robot in the forward direction.
+
   void SetWheelVelocity(float speedLeft, float speedRight);
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long)
-  virtual void Update(bool recursive = true) override;
+  virtual void Update(unsigned long currentMs, bool recursive = true) override;
-
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
-  // virtual void ProcessBinary(char* bytes) override;
-
-    /// @brief The left wheel
-  Motor* leftWheel = nullptr;
-  /// @brief The right wheel
-  Motor* rightWheel = nullptr;
 
  protected:
   /// @brief The radius of a wheel in meters
-  float wheelRadius = 0.0f;
+  float wheelRadius = 1.0f;
   /// @brief The distance between the wheels in meters
-  float wheelSeparation = 0.0f;
+  float wheelSeparation = 1.0f;
 
   /// @brief Convert revolutions per second to meters per second
   float rpsToMs = 1.0f;
 
+  /// @brief The left wheel
+  Thing* leftWheel = nullptr;
+  /// @brief The right wheel
+  Thing* rightWheel = nullptr;
 };
 
 }  // namespace RoboidControl

Things/DigitalSensor.cpp

@@ -2,17 +2,8 @@
 
 namespace RoboidControl {
 
-DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent) {
+//DigitalSensor::DigitalSensor() {}
-  this->type = Type::Switch;
-}
 
-int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
+DigitalSensor::DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant) {}
-  bytes[(*ix)++] = state ? 1 : 0;
-  return 1;
-}
-
-void DigitalSensor::ProcessBinary(char* bytes) {
-  this->state |= (bytes[0] == 1);
-}
 
 }  // namespace RoboidControl

Things/DigitalSensor.h

@@ -7,31 +7,15 @@ namespace RoboidControl {
 /// @brief A digital (on/off, 1/0, true/false) sensor
 class DigitalSensor : public Thing {
  public:
-  /// @brief Create a digital sensor without communication abilities
-  //DigitalSensor();
-  /// @brief Create a digital sensor for a participant
-  /// @param owner The owning participant
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  DigitalSensor(Participant* owner = nullptr); //, unsigned char thingId = 0);
-  /// @brief Create a new child digital sensor
-  /// @param parent The parent thing
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  // DigitalSensor(Thing* parent); //, unsigned char thingId = 0);
-  DigitalSensor(Thing* parent = Thing::LocalRoot());
-
   /// @brief The sigital state
   bool state = 0;
 
-  /// @brief Function used to generate binary data for this digital sensor
+  /// @brief The default constructor
-  /// @param buffer The byte array for thw binary data
+  //DigitalSensor();
-  /// @param ix The starting position for writing the binary data
+  /// @brief Create a temperature sensor with the given ID
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  /// @param networkId The network ID of the sensor
-  /// @brief Function used to process binary data received for this digital
+  /// @param thingId The ID of the thing
-  /// sensor
+  DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
-  /// @param bytes The binary data to process
-  virtual void ProcessBinary(char* bytes) override;
 };
 
 }  // namespace RoboidControl

Things/DistanceSensor.cpp

@@ -1,29 +0,0 @@
-#include "DistanceSensor.h"
-
-#include "Messages/LowLevelMessages.h"
-
-namespace RoboidControl {
-
-DistanceSensor::DistanceSensor(Thing* parent) : Thing(parent) {
-  this->type = Type::DistanceSensor;
-  this->name = "Distance sensor";
-}
-
-float DistanceSensor::GetDistance() {
-  if (this->externalDistance < this->internalDistance)
-    return this->externalDistance;
-  else
-    return this->internalDistance;
-}
-
-int DistanceSensor::GenerateBinary(char* bytes, unsigned char* ix) {
-  LowLevelMessages::SendFloat16(bytes, ix, this->internalDistance);
-  return *ix;
-}
-
-void DistanceSensor::ProcessBinary(char* bytes) {
-  unsigned char ix = 0;
-  this->externalDistance = LowLevelMessages::ReceiveFloat16(bytes, &ix);
-}
-
-}  // namespace RoboidControl

Things/DistanceSensor.h

@@ -1,41 +0,0 @@
-#pragma once
-
-#if !NO_STD
-#include <limits>
-#endif
-
-#include "Thing.h"
-
-namespace RoboidControl {
-
-/// @brief A sensor measuring distance
-class DistanceSensor : public Thing {
- public:
-  /// @brief Create a new child touch sensor
-  /// @param parent The parent thing
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  DistanceSensor(Thing* parent = Thing::LocalRoot());
-
-  /// @brief Get the current distance
-  float GetDistance();
-
-  /// @brief Function used to generate binary data for this sensor
-  /// @param buffer The byte array for thw binary data
-  /// @param ix The starting position for writing the binary data
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
-  /// @brief Function used to process binary data received for this sensor
-  /// @param bytes The binary data to process
-  virtual void ProcessBinary(char* bytes) override;
-
- protected:
-#if ARDUNIO
-  float internalDistance = INFINITY;
-  float externalDistance = INFINITY;
-#else
-  float internalDistance = std::numeric_limits<double>::infinity();
-  float externalDistance = std::numeric_limits<double>::infinity();
-#endif
-};
-
-}  // namespace RoboidControl

Things/Motor.cpp

@@ -1,34 +0,0 @@
-#include "Motor.h"
-
-#include "Messages/BinaryMsg.h"
-#include "Participant.h"
-
-namespace RoboidControl {
-
-Motor::Motor(Thing* parent) : Thing(parent) {
-  this->type = Type::UncontrolledMotor;
-}
-
-void Motor::SetTargetVelocity(float targetSpeed) {
-  if (targetSpeed != this->targetVelocity) {
-    this->targetVelocity = targetSpeed;
-
-    if (this->owner->networkId != 0) {
-      // in other word: if we are connected...
-      BinaryMsg* binaryMsg = new BinaryMsg(this->owner->networkId, this);
-      this->owner->Send(binaryMsg);
-      delete binaryMsg;
-    }
-  }
-}
-
-float Motor::GetTargetVelocity() {
-  return this->targetVelocity;
-}
-
-int Motor::GenerateBinary(char* data, unsigned char* ix) {
-  data[(*ix)++] = this->targetVelocity * 127.0f;
-  return 1;
-}
-
-}  // namespace RoboidControl

Things/Motor.h

@@ -1,25 +0,0 @@
-#pragma once
-
-#include "Thing.h"
-
-namespace RoboidControl {
-
-class Motor : public Thing {
- public:
-  Motor(Thing* parent = Thing::LocalRoot());
-
-  /// @brief Motor turning direction
-  enum class Direction { Clockwise = 1, CounterClockwise = -1 };
-  /// @brief The forward turning direction of the motor
-  Direction direction;
-
-  virtual void SetTargetVelocity(float velocity);  // -1..0..1
-  virtual float GetTargetVelocity();
-
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
-
- protected:
-  float targetVelocity = 0;
-};
-
-}  // namespace RoboidControl

Things/RelativeEncoder.cpp

@@ -1,22 +0,0 @@
-#include "RelativeEncoder.h"
-
-namespace RoboidControl {
-
-RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(parent) {
-  this->type = Type::RelativeEncoder;
-}
-
-float RelativeEncoder::GetRotationSpeed() {
-  return rotationSpeed;
-}
-
-int RelativeEncoder::GenerateBinary(char* data, unsigned char* ix) {
-  data[(*ix)++] = (unsigned char)(this->rotationSpeed * 127);
-  return 1;
-}
-
-void RelativeEncoder::ProcessBinary(char* data) {
-  this->rotationSpeed = (float)data[0] / 127;
-}
-
-}  // namespace RoboidControl

Things/RelativeEncoder.h

@@ -1,39 +0,0 @@
-#pragma once
-
-#include "Thing.h"
-
-namespace RoboidControl {
-
-/// @brief An Incremental Encoder measures the rotations of an axle using a
-/// rotary sensor. Some encoders are able to detect direction, while others can
-/// not.
-class RelativeEncoder : public Thing {
- public:
-  /// @brief Creates a sensor which measures distance from pulses
-  /// @param pulsesPerRevolution The number of pulse edges which make a
-  /// full rotation
-  /// @param distancePerRevolution The distance a wheel travels per full
-  /// rotation
-  //RelativeEncoder(Participant* owner);
-  // RelativeEncoder(Thing* parent);
-  RelativeEncoder(Thing* parent = Thing::LocalRoot());
-
-  /// @brief Get the rotation speed 
-  /// @return The speed in revolutions per second
-  virtual float GetRotationSpeed();
-  float rotationSpeed = 0;
-
-  /// @brief Function used to generate binary data for this touch sensor
-  /// @param buffer The byte array for thw binary data
-  /// @param ix The starting position for writing the binary data
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
-  /// @brief Function used to process binary data received for this touch sensor
-  /// @param bytes The binary data to process
-  virtual void ProcessBinary(char* bytes) override;
-
-protected:
-    /// @brief rotation speed in revolutions per second
-    //float _rotationSpeed;
-};
-
-}  // namespace RoboidControl

Things/TemperatureSensor.cpp

@@ -4,9 +4,9 @@
 
 namespace RoboidControl {
 
-TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent) {
+TemperatureSensor::TemperatureSensor(Participant* participant,
-  this->type = Type::TemperatureSensor;
+                                     unsigned char thingId)
-}
+    : Thing(participant, Type::TemperatureSensor, thingId) {}
 
 void TemperatureSensor::SetTemperature(float temp) {
   this->temperature = temp;

Things/TemperatureSensor.h

@@ -7,17 +7,15 @@ namespace RoboidControl {
 /// @brief A temperature sensor
 class TemperatureSensor : public Thing {
  public:
+  /// @brief The measured temperature
+  float temperature = 0;
+
   /// @brief The default constructor
   //TemperatureSensor();
   /// @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 thingId);
+  TemperatureSensor(Participant* participant, unsigned char thingId);
-  // TemperatureSensor(Thing* parent);
-  TemperatureSensor(Thing* parent = Thing::LocalRoot());
-
-      /// @brief The measured temperature
-      float temperature = 0;
 
   /// @brief Manually override the measured temperature
   /// @param temperature The new temperature

Things/TouchSensor.cpp

@@ -1,36 +1,23 @@
 #include "TouchSensor.h"
 
 namespace RoboidControl {
+TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}
 
-TouchSensor::TouchSensor(Thing* parent) : Thing(parent) {
+TouchSensor::TouchSensor(Participant* participant)
-  this->type = Type::TouchSensor;
+    : Thing(participant, Thing::Type::TouchSensor) {}
-  this->name = "Touch sensor";
-}
 
-bool TouchSensor::IsTouching() {
+TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
-  return this->internalTouch || this->externalTouch;
+  this->SetParent(parent);
-}
-
-// void TouchSensor::PrepareForUpdate() {
-//   //this->internalTouch = this->externalTouch;
-// }
-
-void TouchSensor::Update(bool recursive) {
-  Thing::Update(recursive);
 }
 
 int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
-  std::cout << "BinaryMsg Touch " << this->internalTouch << std::endl;
+  return 0;
-  bytes[(*ix)++] = this->internalTouch ? 1 : 0;
-  return 1;
 }
 
 void TouchSensor::ProcessBinary(char* bytes) {
-  this->externalTouch = (bytes[0] == 1);
+  // if (bytes[0] == 1)
-  if (this->externalTouch)
+  //   std::cout << this->name << " is Touching something!\n";
-    std::cout << "touching!\n";
+  this->touchedSomething |= (bytes[0] == 1);
-  else
-    std::cout << "not touching\n";
 }
 
 }  // namespace RoboidControl

Things/TouchSensor.h

@@ -6,31 +6,29 @@ namespace RoboidControl {
 
 /// @brief A sensor which can detect touches
 class TouchSensor : public Thing {
-// When finishing this release (0.3), I notice that this is equivalent to a digital sensor
  public:
-  /// @brief Create a new child touch sensor
+  /// @brief Create a touch sensor with isolated participant
-  /// @param parent The parent thing
+  TouchSensor();
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
+  /// @brief Create a touch sensor
-  /// an ID
+  TouchSensor(Participant* participant);
-  TouchSensor(Thing* parent = Thing::LocalRoot());
+  /// @brief Create a temperature sensor with the given ID
+  /// @param networkId The network ID of the sensor
+  /// @param thingId The ID of the thing
+  TouchSensor(Thing* parent);
+  // TouchSensor(RemoteParticipant* participant, unsigned char networkId,
+  // unsigned char thingId);
 
   /// @brief Value which is true when the sensor is touching something, false
   /// otherwise
-  bool IsTouching();
+  bool touchedSomething = false;
 
-  //virtual void PrepareForUpdate() override;
+  /// @brief Function to create a binary message with the temperature
-  virtual void Update(bool recursive) override;
-
-  /// @brief Function used to generate binary data for this touch sensor
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
   int GenerateBinary(char* bytes, unsigned char* ix) override;
-  /// @brief Function used to process binary data received for this touch sensor
+  /// @brief Function to extract the temperature received in the binary message
-  /// @param bytes The binary data to process
+  /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes) override;
-protected:
-  bool externalTouch = false;
-  bool internalTouch = false;
 };
 
 }  // namespace RoboidControl

Windows/WindowsParticipant.cpp

@@ -1,5 +1,4 @@
 #include "WindowsParticipant.h"
-#if defined(_WIN32) || defined(_WIN64)
 
 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
@@ -8,6 +7,7 @@
 #endif
 
 namespace RoboidControl {
+namespace Windows {
 
 void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #if defined(_WIN32) || defined(_WIN64)
@@ -85,9 +85,9 @@ void ParticipantUDP::Receive() {
     unsigned int sender_port = ntohs(client_addr.sin_port);
 
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    // RoboidControl::ParticipantUDP* remoteParticipant = this->Get(sender_ipAddress, sender_port);
+    // RoboidControl::ParticipantUDP* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
     // if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
+    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
     //   // std::cout << "New sender " << sender_ipAddress << ":"
     //   //           << sender_port << "\n";
     //   // std::cout << "New remote participant " <<
@@ -102,7 +102,7 @@ void ParticipantUDP::Receive() {
 #endif // _WIN32 || _WIN64
 }
 
-bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(_WIN32) || defined(_WIN64)
   char ip_str[INET_ADDRSTRLEN];
   inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@@ -142,6 +142,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   return true;
 }
 
+}  // namespace Windows
 }  // namespace RoboidControl
-
-#endif

Windows/WindowsParticipant.h

@@ -1,15 +1,15 @@
 #pragma once
-#if defined(_WIN32) || defined(_WIN64)
 
 #include "Participants/ParticipantUDP.h"
 
 namespace RoboidControl {
+namespace Windows {
 
-class ParticipantUDP : public ParticipantUDPGeneric {
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:
@@ -18,5 +18,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
   #endif
 };
 
+}  // namespace Windows
 }  // namespace RoboidControl
-#endif

examples/CMakeLists.txt

@@ -1,8 +0,0 @@
-# examples/CMakeLists.txt
-
-# Check if the options are enabled and add the corresponding examples
-if(BUILD_EXAMPLE_BB2A)
-    add_executable(BB2A BB2A/main.cpp)  # Adjust the path as necessary
-
-    target_link_libraries(BB2A RoboidControl)
-endif()

examples/README.md

@@ -1 +0,0 @@
-Important: this folder has to be named 'examples' exactly to maintain compatibility with Arduino

test/CMakeLists.txt

@@ -1,30 +0,0 @@
-# Unit test configuration
-
-add_compile_definitions(GTEST)
-include(FetchContent)
-FetchContent_Declare(
-    googletest
-    DOWNLOAD_EXTRACT_TIMESTAMP ON
-    URL https://github.com/google/googletest/archive/refs/heads/main.zip
-)
-
-# For Windows: Prevent overriding the parent project's compiler/linker settings
-set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
-FetchContent_MakeAvailable(googletest)
-
-file(GLOB_RECURSE test_srcs *.cc)
-message(STATUS "Test sources: ${test_srcs}")
-add_executable(
-    RoboidControlTest
-    ${test_srcs}
-)
-message(STATUS "RoboidControlTest target created")
-target_link_libraries(
-    RoboidControlTest
-    gtest_main
-    RoboidControl
-    LinearAlgebra
-)
-
-include(GoogleTest)
-gtest_discover_tests(RoboidControlTest)

test/participant_test.cc

@@ -4,75 +4,111 @@
 // not supported using Visual Studio 2022 compiler...
 #include <gtest/gtest.h>
 
+#include <chrono>
+#include <thread>
+// #include <ws2tcpip.h>
+
+#include "Participant.h"
 #include "Participants/SiteServer.h"
 #include "Thing.h"
 
-#include <chrono>
-#include <thread>
-
 using namespace RoboidControl;
 
-TEST(Participant, Participant) {
+// Function to get the current time in milliseconds as unsigned long
-  ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7682);
+unsigned long get_time_ms() {
-
+  auto now = std::chrono::steady_clock::now();
-  unsigned long milliseconds = Thing::GetTimeMs();
+  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
-  unsigned long startTime = milliseconds;
+      now.time_since_epoch());
-  while (milliseconds < startTime + 7000) {
+  return static_cast<unsigned long>(ms.count());
-    participant->Update();
-    
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    milliseconds = Thing::GetTimeMs();
-  }
-  SUCCEED();
 }
 
-TEST(Participant, SiteServer) {
+// class RoboidControlSuite : public ::testing::test {
-  SiteServer* siteServer = new SiteServer();
+//   TEST_F(RoboidControlSuite, HiddenParticipant) {
-
+//     Thing thing = Thing();
-  unsigned long milliseconds = Thing::GetTimeMs();
-  unsigned long startTime = milliseconds;
-  while (milliseconds < startTime + 7000) {
-    siteServer->Update();
-    
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    milliseconds = Thing::GetTimeMs();
-  }
-  SUCCEED();
-}
-
-TEST(Participant, SiteParticipant) {
-  SiteServer* siteServer = new SiteServer(7681);
-  ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682);
-
-  unsigned long milliseconds = Thing::GetTimeMs();
-  unsigned long startTime = milliseconds;
-  while (milliseconds < startTime + 7000) {
-    siteServer->Update();
-    participant->Update();
-
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    milliseconds = Thing::GetTimeMs();
-  }
-  SUCCEED();
-}
-
-TEST(Participant, ThingMsg) {
-  SiteServer* siteServer = new SiteServer(7681);
-  ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682);
-  Thing* thing = new Thing();
-  thing->SetName("First Thing");
-  thing->SetModel("https://passer.life/extras/ant.jpg");
   
-  unsigned long milliseconds = Thing::GetTimeMs();
+//     unsigned long milliseconds = get_time_ms();
-  unsigned long startTime = milliseconds;
+//     unsigned long startTime = milliseconds;
-  while (milliseconds < startTime + 7000) {
+//     while (milliseconds < startTime + 1000) {
-    siteServer->Update();
+//       Thing.Update(milliseconds);
-    participant->Update();
+  
+//       milliseconds = get_time_ms();
+//     }
+//     ASSERT_EQ(1, 1);
+//   }
+// }
 
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+class ParticipantSuite : public ::testing::Test {
-    milliseconds = Thing::GetTimeMs();
+protected:
+  // SetUp and TearDown can be used to set up and clean up before/after each
+  // test
+  void SetUp() override {
+    // Initialize test data here
   }
-  SUCCEED();
+
-}
+  void TearDown() override {
+    // Clean up test data here
+  }
+};
+
+
+
+// TEST_F(ParticipantSuite, ParticipantUDP) {
+//   ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681);
+
+//   unsigned long milliseconds = get_time_ms();
+//   unsigned long startTime = milliseconds;
+//   while (milliseconds < startTime + 1000) {
+//     participant->Update(milliseconds);
+
+//     milliseconds = get_time_ms();
+//   }
+//   ASSERT_EQ(1, 1);
+// }
+
+// TEST_F(ParticipantSuite, SiteServer) {
+//   SiteServer site = SiteServer(7681);
+
+//   unsigned long milliseconds = get_time_ms();
+//   unsigned long startTime = milliseconds;
+//   while (milliseconds < startTime + 1000) {
+//     site.Update(milliseconds);
+
+//     milliseconds = get_time_ms();
+//   }
+//   ASSERT_EQ(1, 1);
+// }
+
+// TEST_F(ParticipantSuite, SiteParticipant) {
+//   SiteServer site = SiteServer(7681);
+//   ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681);
+
+//   unsigned long milliseconds = get_time_ms();
+//   unsigned long startTime = milliseconds;
+//   while (milliseconds < startTime + 1000) {
+//     site.Update(milliseconds);
+//     participant.Update(milliseconds);
+
+//     std::this_thread::sleep_for(std::chrono::milliseconds(100));
+//     milliseconds = get_time_ms();
+//   }
+//   ASSERT_EQ(1, 1);
+// }
+
+// TEST_F(ParticipantSuite, Thing) {
+//   SiteServer site = SiteServer(7681);
+//   ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681);
+//   Thing thing = Thing(&participant);
+
+//   unsigned long milliseconds = get_time_ms();
+//   unsigned long startTime = milliseconds;
+//   while (milliseconds < startTime + 1000) {
+//     site.Update(milliseconds);
+//     participant.Update(milliseconds);
+
+//     std::this_thread::sleep_for(std::chrono::milliseconds(100));
+//     milliseconds = get_time_ms();
+//   }
+//   ASSERT_EQ(1, 1);
+// }
 
 #endif

test/thing_test.cc

@@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
   unsigned long milliseconds = Thing::GetTimeMs();
   unsigned long startTime = milliseconds;
   while (milliseconds < startTime + 1000) {
-    thing->Update();
+    Thing::UpdateThings(milliseconds);
 
     milliseconds = Thing::GetTimeMs();
   }
@@ -23,13 +23,13 @@ TEST(RoboidControlSuite, HiddenParticipant) {
 }
 
 TEST(RoboidControlSuite, IsolatedParticipant) {
-  ParticipantUDP* participant = new ParticipantUDP(0);
+  ParticipantUDP* participant = ParticipantUDP::Isolated();
-  Thing* thing = new Thing();
+  Thing* thing = new Thing(participant);
 
   unsigned long milliseconds = Thing::GetTimeMs();
   unsigned long startTime = milliseconds;
   while (milliseconds < startTime + 1000) {
-    participant->Update();
+    participant->Update(milliseconds);
 
     milliseconds = Thing::GetTimeMs();
   }