Optimise sending of messages

Arduino/Things/DigitalInput.cpp

@@ -29,7 +29,7 @@ DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
     : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
 
 void DigitalInput::TouchSensor::Update(bool recursive) {
-  this->touchedSomething = digitalInput.isLow;
+  this->internalTouch = digitalInput.isLow;
 }
 
 #pragma endregion Touch sensor
@@ -98,8 +98,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) {
   this->pulseFrequency = pulseCount / timeStep;
   this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
 
-  std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
+  // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
-            << " timestep " << timeStep << std::endl;
+  //           << " timestep " << timeStep << std::endl;
 
   this->lastUpdateTime = currentTimeMs;
 }

Arduino/Things/UltrasonicSensor.cpp

@@ -65,7 +65,7 @@ UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
 void UltrasonicSensor::TouchSensor::Update(bool recursive) {
   RoboidControl::TouchSensor::Update(recursive);
   this->ultrasonic.Update(false);
-  this->touchedSomething |= (this->ultrasonic.distance > 0 &&
+  this->internalTouch |= (this->ultrasonic.distance > 0 &&
                             this->ultrasonic.distance <= this->touchDistance);
 }
 

Messages/LowLevelMessages.cpp

@@ -5,6 +5,83 @@
 
 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) {
@@ -42,58 +119,5 @@ 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

@@ -11,6 +11,10 @@ class LowLevelMessages {
   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);
 

Messages/PoseMsg.cpp

@@ -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::ReceiveQuat32(buffer, &ix);
+  this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
   // linearVelocity
   // angularVelocity
 }
@@ -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::SendQuat32(buffer, &ix, this->orientation);
+    LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
   if ((this->poseType & Pose_LinearVelocity) != 0)
     LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
   if ((this->poseType & Pose_AngularVelocity) != 0)

Participant.cpp

@@ -62,7 +62,7 @@ bool Participant::Send(IMessage* msg) {
     return true;
 
   // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
-  //           << " to " << remoteParticipant->ipAddress << std::endl;
+  //           << " to " << this->ipAddress << std::endl;
 
 #if defined(_WIN32) || defined(_WIN64)
   Windows::ParticipantUDP* thisWindows =

Participants/ParticipantUDP.cpp

@@ -97,7 +97,7 @@ void ParticipantUDP::Update() {
       if (this->remoteSite == nullptr)
         this->Publish(msg);
       else
-        this->remoteSite->Send(msg);
+        this->Send(msg);
 
       delete msg;
 
@@ -127,19 +127,19 @@ void ParticipantUDP::UpdateMyThings() {
       continue;
 
     // std::cout << thing->name << "\n";
-    if (thing->hierarchyChanged) {
+    // if (thing->hierarchyChanged) {
-      if (!(this->isIsolated || this->networkId == 0)) {
+    //   if (!(this->isIsolated || this->networkId == 0)) {
-        ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
+    //     ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-        this->remoteSite->Send(thingMsg);
+    //     this->remoteSite->Send(thingMsg);
-        delete thingMsg;
+    //     delete thingMsg;
 
-        if (thing->nameChanged) {
+    //     if (thing->nameChanged) {
-          NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+    //       NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-          this->remoteSite->Send(nameMsg);
+    //       this->remoteSite->Send(nameMsg);
-          delete nameMsg;
+    //       delete nameMsg;
-        }
+    //     }
-      }
+    //   }
-    }
+    // }
 
     // std::cout << "B\n";
     // Why don't we do recursive?
@@ -149,26 +149,26 @@ void ParticipantUDP::UpdateMyThings() {
     thing->Update(false);
 
     // std::cout << "C\n";
-    if (!(this->isIsolated || this->networkId == 0)) {
+    // if (!(this->isIsolated || this->networkId == 0)) {
-      if (thing->terminate) {
+    //   if (thing->terminate) {
-        DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
+    //     DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
-        this->remoteSite->Send(destroyMsg);
+    //     this->remoteSite->Send(destroyMsg);
-        delete destroyMsg;
+    //     delete destroyMsg;
-      } else {
+    //   } else {
-        //  Send to remote site
+    //     //  Send to remote site
-        if (thing->nameChanged) {
+    //     if (thing->nameChanged) {
-          NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+    //       NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-          this->remoteSite->Send(nameMsg);
+    //       this->remoteSite->Send(nameMsg);
-          delete nameMsg;
+    //       delete nameMsg;
-        }
+    //     }
-        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+    //     PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-        this->remoteSite->Send(poseMsg);
+    //     this->remoteSite->Send(poseMsg);
-        delete poseMsg;
+    //     delete poseMsg;
-        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+    //     BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-        this->remoteSite->Send(binaryMsg);
+    //     this->remoteSite->Send(binaryMsg);
-        delete binaryMsg;
+    //     delete binaryMsg;
-      }
+    //   }
-    }
+    // }
     // std::cout << "D\n";
     if (thing->terminate)
       this->Remove(thing);
@@ -230,34 +230,10 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
   delete binaryMsg;
 }
 
-// bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
+bool ParticipantUDP::Send(IMessage* msg) {
-//   int bufferSize = msg->Serialize(this->buffer);
+  if (this->remoteSite != nullptr)
-//   if (bufferSize <= 0)
+    this->remoteSite->Send(msg);
-//     return true;
+}
-
-//   // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
-//   //           << " to " << remoteParticipant->ipAddress << std::endl;
-
-// #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 ParticipantUDP::PublishThingInfo(Thing* thing) {
   // std::cout << "Publish thing info" << thing->networkId << "\n";

Participants/ParticipantUDP.h

@@ -77,7 +77,6 @@ public:
   long publishInterval = 3000;  // 3 seconds
 
  protected:
-  char buffer[1024];
 
 #if !defined(ARDUINO)
 #if defined(__unix__) || defined(__APPLE__)
@@ -114,7 +113,7 @@ public:
   void SendThingInfo(Participant* remoteParticipant, Thing* thing);
   void PublishThingInfo(Thing* thing);
 
-  //bool Send(Participant* remoteParticipant, IMessage* msg);
+  virtual bool Send(IMessage* msg) override;
   bool Publish(IMessage* msg);
 
 #pragma endregion Send

Things/ControlledMotor.cpp

@@ -39,8 +39,8 @@ void ControlledMotor::Update(bool recurse) {
   this->lastError = error;
 
   float output = p_term + i_term + d_term;
-  std::cout << "target " << this->targetVelocity << " actual "
+  // std::cout << "target " << this->targetVelocity << " actual "
-            << this->actualVelocity << " output = " << output << std::endl;
+  //           << 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;

Things/Motor.cpp

@@ -1,5 +1,8 @@
 #include "Motor.h"
 
+#include "Messages/BinaryMsg.h"
+#include "Participant.h"
+
 namespace RoboidControl {
 
 Motor::Motor(Thing* parent) : Thing(parent) {
@@ -7,7 +10,20 @@ Motor::Motor(Thing* parent) : Thing(parent) {
 }
 
 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) {

Things/Motor.h

@@ -14,11 +14,11 @@ class Motor : public Thing {
   Direction direction;
 
   virtual void SetTargetVelocity(float velocity);  // -1..0..1
+  virtual float GetTargetVelocity();
 
   int GenerateBinary(char* bytes, unsigned char* ix) override;
-  // virtual void ProcessBinary(char* bytes) override;
 
- //protected:
+ protected:
   float targetVelocity = 0;
 };
 

Things/TouchSensor.cpp

@@ -7,8 +7,12 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent) {
   this->name = "Touch sensor";
 }
 
+bool TouchSensor::IsTouching() {
+  return this->internalTouch || this->externalTouch;
+}
+
 void TouchSensor::PrepareForUpdate() {
-  this->touchedSomething = this->externalTouch;
+  //this->internalTouch = this->externalTouch;
 }
 
 void TouchSensor::Update(bool recursive) {
@@ -16,7 +20,8 @@ void TouchSensor::Update(bool recursive) {
 }
 
 int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
-  bytes[(*ix)++] = this->touchedSomething ? 1 : 0;
+  std::cout << "BinaryMsg Touch " << this->internalTouch << std::endl;
+  bytes[(*ix)++] = this->internalTouch ? 1 : 0;
   return 1;
 }
 

Things/TouchSensor.h

@@ -16,7 +16,7 @@ class TouchSensor : public Thing {
 
   /// @brief Value which is true when the sensor is touching something, false
   /// otherwise
-  bool touchedSomething = false;
+  bool IsTouching();
 
   virtual void PrepareForUpdate() override;
   virtual void Update(bool recursive) override;
@@ -30,6 +30,7 @@ class TouchSensor : public Thing {
   virtual void ProcessBinary(char* bytes) override;
 protected:
   bool externalTouch = false;
+  bool internalTouch = false;
 };
 
 }  // namespace RoboidControl

examples/BB2B.cpp

@@ -27,10 +27,10 @@ int main() {
   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.touchedSomething) ? -600.0f : 600.0f;
+    float leftWheelSpeed = (touchRight.internalTouch) ? -600.0f : 600.0f;
     // The right wheel does the same, but instead is controlled by
     // touches on the left side
-    float rightWheelSpeed = (touchLeft.touchedSomething) ? -600.0f : 600.0f;
+    float rightWheelSpeed = (touchLeft.internalTouch) ? -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);