Improved network sync

DifferentialDrive.cpp

@@ -25,7 +25,7 @@ void DifferentialDrive::SetDimensions(float wheelDiameter,
                                       float wheelSeparation) {
   this->wheelDiameter = wheelDiameter;
   this->wheelSeparation = wheelSeparation;
-  this->rpsToMs = wheelDiameter * Angle::PI;
+  this->rpsToMs = wheelDiameter * Angle::pi;
 
   float distance = this->wheelSeparation / 2;
   this->motors[0]->position.distance = distance;
@@ -73,7 +73,7 @@ Polar DifferentialDrive::GetVelocity() {
   Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
   float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
-  // float rpsToMs = wheelDiameter * Angle::PI;
+  // float rpsToMs = wheelDiameter * Angle::pi;
 
   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
   rightSpeed = rightSpeed * rpsToMs; // in meters per second
@@ -85,19 +85,22 @@ Polar DifferentialDrive::GetVelocity() {
   return velocity;
 }
 
+#include <Arduino.h>
+
 float DifferentialDrive::GetAngularVelocity() {
   Motor *leftMotor = motors[0];
   Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
   float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
-  // float rpsToMs = wheelDiameter * Angle::PI;
+
+  // float rpsToMs = wheelDiameter * Angle::pi;
   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
   rightSpeed = rightSpeed * rpsToMs; // in meters per second
 
   float angularSpeed = (rightSpeed - leftSpeed) / 2;
-  float angularDistance = wheelSeparation / 2 * Angle::PI;
+  float angularDistance = wheelSeparation / 2 * Angle::pi;
   float rotationsPerSecond = angularSpeed / angularDistance;
-  float degreesPerSecond = 360 * rotationsPerSecond;
+  float degreesPerSecond = Angle::Normalize(360 * rotationsPerSecond);
   float angularVelocity = degreesPerSecond;
 
   return angularVelocity;

NetworkPerception.cpp

@@ -0,0 +1,78 @@
+#include "NetworkPerception.h"
+
+#include "RoboidControl/NetworkSync.h"
+
+void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
+                                      int packetsize) {
+  // printf("packet received, type = 0x%02x %d %d %d %d\n", buffer[0],
+  // buffer[2], buffer[3], buffer[4], buffer[5]);
+
+  if (buffer[0] == NetworkSync::PoseMsg) {
+    ReceiveObject(buffer, roboid);
+  }
+}
+
+Int32 NetworkPerception::ReceiveInt32(unsigned char *data, int startIndex) {
+  Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 |
+                  (UInt32)(data[startIndex + 2]) << 16 |
+                  (UInt32)(data[startIndex + 1]) << 8 |
+                  (UInt32)(data[startIndex + 0]));
+  return a;
+}
+
+float NetworkPerception::ReceiveFloat100(unsigned char *data, int startIndex) {
+  Int32 intValue = ReceiveInt32(data, startIndex);
+  float f = (float)intValue / 100.0F;
+  return f;
+}
+
+Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) {
+  float x = ReceiveFloat100(data, startIndex);
+  float y = ReceiveFloat100(data, startIndex + 4);
+  float z = ReceiveFloat100(data, startIndex + 8);
+  Vector3 v = Vector3(x, y, z);
+  return v;
+}
+
+void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) {
+  unsigned char objectId = data[1];
+  char poseType = data[2];
+  Vector3 worldPosition = ReceiveVector3(data, 3);
+  Vector3 worldAngles = ReceiveVector3(data, 15);
+  Quaternion worldOrientation = Quaternion::Euler(worldAngles);
+  Vector3 worldDirection = worldOrientation * Vector3::forward;
+  if (objectId == 0) {
+    roboid->SetPosition(worldPosition);
+    roboid->SetOrientation(worldOrientation);
+    // printf("Received pose (%f, %f) (%f, %f)\n", worldPosition.x,
+    //        worldPosition.z, worldDirection.x, worldDirection.z);
+
+  } else {
+    Vector3 myPosition = roboid->GetPosition(); // Vector3::zero;
+    Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition);
+    Quaternion myOrientation = roboid->GetOrientation();
+    Vector3 myDirection = myOrientation * Vector3::forward;
+    Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection);
+
+    Vector3 localPosition =
+        Quaternion::Inverse(myOrientation) * (worldPosition - myPosition);
+
+    // float distance = Vector3::Distance(myPosition, worldPosition);
+    // float angle = Vector3::SignedAngle(myDirection, localPosition,
+    // Vector3::up);
+    float distance = Vector3::Magnitude(localPosition);
+    float angle =
+        Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
+
+    Polar position = Polar(angle, distance);
+
+    // int objCount = roboid->perception->TrackedObjectCount();
+
+    // printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
+    // worldPosition.x,
+    //        worldPosition.z, localPosition.x, localPosition.z, distance,
+    //        angle);
+
+    roboid->perception->AddTrackedObject(this, position);
+  }
+}

NetworkPerception.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include "RoboidControl/Roboid.h"
+#include "RoboidControl/Sensor.h"
+#include "RoboidControl/Types.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+class NetworkPerception : public Sensor {
+public:
+  void ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize);
+
+protected:
+  void ReceiveObject(unsigned char *data, Roboid *roboid);
+
+  Int32 ReceiveInt32(unsigned char *data, int startIndex);
+  float ReceiveFloat100(unsigned char *data, int startIndex);
+  Vector3 ReceiveVector3(unsigned char *data, int startIndex);
+};
+
+} // namespace RoboidControl
+} // namespace Passer

NetworkSync.cpp

@@ -1,20 +1,115 @@
 #include "NetworkSync.h"
 
+#ifdef RC_DEBUG
+#include <Arduino.h>
+#endif
+
 void NetworkSync::SendVector3(unsigned char *data, int startIndex, Vector3 v) {
-  SendFloat100(data, startIndex, v.x);
+  SendSingle100(data, startIndex, v.x);
-  SendFloat100(data, startIndex + 4, v.y);
+  SendSingle100(data, startIndex + 4, v.y);
-  SendFloat100(data, startIndex + 8, v.z);
+  SendSingle100(data, startIndex + 8, v.z);
 }
 
-void NetworkSync::SendFloat100(unsigned char *data, int startIndex,
+void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
                                 float value) {
   // Sends a float with truncated 2 decimal precision
-#ifdef ARDUINO_AVR_UNO
+  Int32 intValue = value * 100;
-  long intValue = value * 100;
+  SendInt32(data, startIndex, intValue);
-#else
+  // for (unsigned char ix = 0; ix < 4; ix++) {
-  int intValue = value * 100;
+  //   data[startIndex + ix] = ((unsigned char *)&intValue)[ix];
-#endif
+  // }
+}
+
+void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) {
   for (unsigned char ix = 0; ix < 4; ix++) {
-    data[startIndex + ix] = ((unsigned char *)&intValue)[ix];
+    data[startIndex + ix] = ((unsigned char *)&value)[ix];
   }
 }
+
+void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
+                                        TrackedObject **objects) {
+  for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
+    TrackedObject *obj = objects[objIx];
+    if (obj == nullptr)
+      continue;
+    // if (obj->sensor->type == Thing::ExternalType)
+    //   continue;
+
+    // tmp
+    obj->id = objIx;
+    PublishTrackedObject(sendBuffer, obj);
+  }
+}
+
+void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
+                                       TrackedObject *object) {
+  Vector2 worldPosition2 = Vector2::Rotate(
+      Vector2::forward * object->position.distance, -object->position.angle);
+  Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
+
+#ifdef RC_DEBUG
+  Serial.print("Send Pose ");
+  Serial.print((int)object->id);
+  Serial.print("  Position  ");
+  Serial.print(worldPosition3.x);
+  Serial.print(", ");
+  Serial.print(worldPosition3.y);
+  Serial.print(", ");
+  Serial.println(worldPosition3.z);
+#else
+  UInt16 bufferSize = 3 + 12;
+  UInt8 buffer[bufferSize] = {
+      PoseMsg,
+      object->id,
+      Pose_Position,
+  };
+  SendVector3(buffer, 3, worldPosition3);
+  sendBuffer(buffer, bufferSize);
+#endif
+}
+
+void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
+  Polar velocity = roboid->propulsion->GetVelocity();
+  Vector2 worldVelocity2 =
+      Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
+  Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y);
+
+  float angularVelocity = roboid->propulsion->GetAngularVelocity();
+  Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);
+
+#ifdef RC_DEBUG
+  Serial.print("Send Pose 0  LinearVelocity  ");
+  Serial.print(worldVelocity3.x);
+  Serial.print(", ");
+  Serial.print(worldVelocity3.y);
+  Serial.print(", ");
+  Serial.print(worldVelocity3.z);
+  Serial.print("  AngularVelocity ");
+  Serial.print(worldAngularVelocity.x);
+  Serial.print(", ");
+  Serial.print(worldAngularVelocity.y);
+  Serial.print(", ");
+  Serial.println(worldAngularVelocity.z);
+#else
+  const unsigned int bufferSize = 3 + 12 + 12;
+  unsigned char buffer[bufferSize] = {
+      PoseMsg,
+      0, // objectId;
+      Pose_LinearVelocity | Pose_AngularVelocity,
+  };
+  SendVector3(buffer, 3, worldVelocity3);
+  SendVector3(buffer, 15, worldAngularVelocity);
+
+  sendBuffer(buffer, bufferSize);
+#endif
+}
+
+void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj) {
+#ifdef RC_DEBUG
+  Serial.print("Send Destroy ");
+  Serial.println((int)obj->id);
+#else
+  unsigned char buffer[2] = {DestroyMsg, (unsigned char)obj->id};
+  sendBuffer(buffer, 2);
+#endif
+}

NetworkSync.h

@@ -1,7 +1,9 @@
 #pragma once
 
+#include "NetworkPerception.h"
 #include "Perception.h"
 #include "Roboid.h"
+#include "Types.h"
 
 namespace Passer {
 namespace RoboidControl {
@@ -33,8 +35,20 @@ public:
 
   static const char DestroyMsg = 0x20;
 
+  typedef void (*SendBuffer)(UInt8 *buffer, UInt16 bufferSize);
+
+  void SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid);
+  void SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj);
+
+  void PublishTrackedObjects(SendBuffer sendBuffer, TrackedObject **objects);
+
+protected:
+  NetworkPerception *networkPerception;
+  void PublishTrackedObject(SendBuffer sendBuffer, TrackedObject *object);
+
   void SendVector3(unsigned char *data, int startIndex, Vector3 v);
-  void SendFloat100(unsigned char *data, int startIndex, float value);
+  void SendSingle100(unsigned char *data, int startIndex, float value);
+  void SendInt32(unsigned char *data, int startIndex, Int32 value);
 };
 
 } // namespace RoboidControl

Perception.cpp

@@ -6,6 +6,14 @@
 
 #include <math.h>
 
+#ifdef RC_DEBUG2
+#include <Arduino.h>
+#endif
+
+unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
+                                              // number of object which can
+                                              // be tracked by a human
+
 Perception::Perception() {
   this->trackedObjects = new TrackedObject *[maxObjectCount];
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
@@ -30,6 +38,22 @@ Sensor *Perception::GetSensor(unsigned int sensorId) {
   return sensor;
 }
 
+unsigned int Perception::AddSensor(Sensor *newSensor) {
+  unsigned int newSensorCount = this->sensorCount + 1;
+  Sensor **newSensors = new Sensor *[newSensorCount];
+  for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
+    newSensors[sensorIx] = sensors[sensorIx];
+
+  unsigned int sensorId = this->sensorCount;
+  newSensors[sensorId] = newSensor;
+
+  Sensor **oldSensors = this->sensors;
+  this->sensors = newSensors;
+  this->sensorCount = newSensorCount;
+  delete[] oldSensors;
+  return sensorId;
+}
+
 Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
     Sensor *sensor = this->sensors[sensorIx];
@@ -107,6 +131,11 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
     // Do we see the same object?
     else {
       if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
+#ifdef RC_DEBUG2
+        Serial.print((int)this->trackedObjects[objIx]->id);
+        Serial.println(": update tracked object");
+#endif
+
         this->trackedObjects[objIx]->Refresh(obj->position);
         delete obj;
         return;
@@ -125,13 +154,28 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
   if (availableSlotIx < maxObjectCount) {
     // a slot is available
     this->trackedObjects[availableSlotIx] = obj;
+    obj->id = availableSlotIx;
+#ifdef RC_DEBUG2
+    Serial.print((int)obj->id);
+    Serial.println(": new tracked object");
+#endif
+
   }
   // If this object is closer than the farthest object, then replace it
   else if (obj->position.distance <
            this->trackedObjects[farthestObjIx]->position.distance) {
     delete this->trackedObjects[farthestObjIx];
     this->trackedObjects[farthestObjIx] = obj;
+    obj->id = availableSlotIx;
+#ifdef RC_DEBUG2
+    Serial.print((int)obj->id);
+    Serial.println(": replaced tracked object");
+#endif
   } else {
+#ifdef RC_DEBUG2
+    Serial.print((int)obj->id);
+    Serial.println(": delete tracked object");
+#endif
     // No available slot, delete trackedobject
     delete obj;
   }

Perception.h

@@ -26,6 +26,8 @@ public:
   /// @brief The roboid of this perception system
   Roboid *roboid = nullptr;
 
+  unsigned int AddSensor(Sensor *sensor);
+
   /// @brief Get the number of Sensors
   /// @return The number of sensors, zero when no sensors are present
   unsigned int GetSensorCount();
@@ -130,7 +132,7 @@ public:
 
   float lastUpdateTimeMs = 0;
 
-  unsigned char maxObjectCount = 7; // 7 is typically the maximum
+  static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
                                        // number of object which can
                                        // be tracked by a human
   TrackedObject **trackedObjects;

Roboid.cpp

@@ -2,9 +2,17 @@
 
 #include "NetworkSync.h"
 
-Roboid::Roboid() {}
+#ifdef RC_DEBUG
+#include <Arduino.h>
+#endif
 
-Roboid::Roboid(Perception *perception, Propulsion *propulsion) {
+Roboid::Roboid() {
+#ifdef RC_DEBUG
+  Serial.begin(115200);
+#endif
+}
+
+Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() {
   this->perception = perception;
   perception->roboid = this;
   this->propulsion = propulsion;

Types.h

@@ -0,0 +1,22 @@
+#pragma once
+
+using UInt8 = unsigned char;
+using SInt8 = signed char;
+using Int8 = SInt8;
+
+using UInt16 = unsigned short;
+using SInt16 = signed short;
+using Int16 = SInt16;
+
+#ifdef ARDUINO_AVR_UNO
+using UInt32 = unsigned long;
+using SInt32 = signed long;
+#else
+using UInt32 = unsigned int;
+using SInt32 = signed int;
+#endif
+using Int32 = SInt32;
+
+using UInt64 = unsigned long long;
+using SInt64 = signed long long;
+using Int64 = SInt64;