Synchronizing perception

NetworkPerception.cpp

@@ -7,40 +7,82 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
   // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
   //        buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]);
 
-  if (buffer[0] == NetworkSync::PoseMsg) {
+  switch (buffer[0]) {
-    ReceiveObject(buffer, roboid);
+  case NetworkSync::CreateMsg:
-  } else if (buffer[0] == NetworkSync::PoseTypeMsg) {
+    ReceiveCreateMsg(buffer, roboid);
+    break;
+  case NetworkSync::PoseMsg:
+    ReceivePoseMsg(buffer, roboid);
+    break;
+  case NetworkSync::PoseTypeMsg:
     ReceiveTypedObject(buffer, roboid);
+    break;
   }
 }
 
-Int32 NetworkPerception::ReceiveInt32(unsigned char *data, int startIndex) {
+#include <Arduino.h>
-  Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 |
+void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
-                  (UInt32)(data[startIndex + 2]) << 16 |
+  unsigned char networkId = data[1];
-                  (UInt32)(data[startIndex + 1]) << 8 |
+  unsigned char objectId = data[2];
-                  (UInt32)(data[startIndex + 0]));
+  unsigned char objectType = data[3];
-  return a;
+  if (networkId == roboid->networkSync->networkId)
+    return;
+
+  // printf("Received create message [%d/%d]\n", networkId, objectId);
+  InterestingThing *thing =
+      roboid->perception->FindTrackedObject(networkId, objectId);
+  if (thing != nullptr) {
+    thing->type = objectType;
+  }
 }
 
-float NetworkPerception::ReceiveFloat100(unsigned char *data, int startIndex) {
+void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
-  Int32 intValue = ReceiveInt32(data, startIndex);
+  unsigned char networkId = data[1];
-  float f = (float)intValue / 100.0F;
+  unsigned char objectId = data[2];
-  return f;
+  unsigned char poseType = data[3];
-}
+  printf("Received pose message [%d/%d]\n", networkId, objectId);
 
-Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) {
+  if ((poseType & NetworkSync::Pose_Position) != 0) {
-  float x = ReceiveFloat100(data, startIndex);
+    Vector3 worldPosition = ReceiveVector3(data, 4);
-  float y = ReceiveFloat100(data, startIndex + 4);
+    if (objectId == 0) {
-  float z = ReceiveFloat100(data, startIndex + 8);
+      roboid->SetPosition(worldPosition);
-  Vector3 v = Vector3(x, y, z);
+    } else {
-  return v;
+      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);
 
-void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) {
+      Vector3 localPosition =
-  unsigned char objectId = data[1];
+          Quaternion::Inverse(myOrientation) * (worldPosition - myPosition);
-  // char poseType = data[2];
+
-  Vector3 worldPosition = ReceiveVector3(data, 3);
+      // float distance = Vector3::Distance(myPosition, worldPosition);
-  Vector3 worldAngles = ReceiveVector3(data, 15);
+      // 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);
+      InterestingThing *thing = roboid->perception->AddTrackedObject(
+          this, networkId, objectId, position);
+      if (thing->type == 0xFF) {
+        // Unknown thing
+        roboid->networkSync->SendInvestigateThing(thing);
+      }
+    }
+  }
+
+  /*
+    Vector3 worldAngles = ReceiveVector3(data, 16);
     Quaternion worldOrientation = Quaternion::Euler(worldAngles);
     Vector3 worldDirection = worldOrientation * Vector3::forward;
     if (objectId == 0) {
@@ -77,6 +119,7 @@ void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) {
 
       roboid->perception->AddTrackedObject(this, position);
     }
+  */
 }
 
 // HACK!!!
@@ -100,8 +143,31 @@ void NetworkPerception::ReceiveTypedObject(unsigned char *data,
     // roboid->perception->AddTrackedObject(this, objectType, worldPosition,
     //                                      worldOrientation);
     // Sensor *sensor =
-    //     roboid->perception->FindSensorOfType(0x82); // Lighthouse type sensor
+    //     roboid->perception->FindSensorOfType(0x82); // Lighthouse type
+    //     sensor
     // LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
     // lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
   }
 }
+
+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;
+}

NetworkPerception.h

@@ -12,7 +12,8 @@ public:
   void ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize);
 
 protected:
-  void ReceiveObject(unsigned char *data, Roboid *roboid);
+  void ReceiveCreateMsg(unsigned char *data, Roboid *roboid);
+  void ReceivePoseMsg(unsigned char *data, Roboid *roboid);
   void ReceiveTypedObject(unsigned char *data, Roboid *roboid);
 
   Int32 ReceiveInt32(unsigned char *data, int startIndex);

NetworkSync.cpp

@@ -9,6 +9,22 @@
 #include "LinearAlgebra/Angle8.h"
 #include "LinearAlgebra/Spherical.h"
 
+void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
+  networkPerception->ProcessPacket(roboid, buffer, bytecount);
+
+  switch (buffer[0]) {
+  case NetworkIdMsg:
+    // this->networkId = NetworkIdMsg;
+    ReceiveNetworkId();
+    break;
+  }
+}
+
+void NetworkSync::ReceiveNetworkId() {
+  this->networkId = buffer[1];
+  printf("Received network Id %d\n", this->networkId);
+}
+
 void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
                               const Vector3 v) {
   SendSingle100(data, *startIndex, v.x);
@@ -118,12 +134,25 @@ void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex,
   }
 }
 
-void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
+// static unsigned char buffer[100];
+
+void NetworkSync::SendBuffer(unsigned char bufferSize) {}
+
+void NetworkSync::PublishClient() {
+  unsigned char ix = 0;
+  buffer[ix++] = ClientMsg;
+  buffer[ix++] = 0; // No network ID
+  SendBuffer(ix);
+}
+
+void NetworkSync::PublishTrackedObjects(Buffer sendBuffer,
                                         InterestingThing **objects) {
   for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
     InterestingThing *obj = objects[objIx];
     if (obj == nullptr)
       continue;
+    if (obj->networkId != 0x00)
+      continue; // object is external
     // if (obj->sensor->type == Thing::ExternalType)
     //   continue;
 
@@ -133,7 +162,7 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
   }
 }
 
-void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
+void NetworkSync::PublishTrackedObject(Buffer sendBuffer,
                                        InterestingThing *object) {
   Vector2 worldPosition2 =
       Vector2::Rotate(Vector2::forward * object->position.distance,
@@ -162,7 +191,7 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
 #endif
 }
 
-void NetworkSync::PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId,
+void NetworkSync::PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
                                         InterestingThing *object) {
   const UInt16 bufferSize = 4 + 12;
   UInt8 buffer[bufferSize] = {
@@ -177,7 +206,7 @@ void NetworkSync::PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId,
   sendBuffer(buffer, bufferSize);
 }
 
-void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
+void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
   Polar velocity = roboid->propulsion->GetVelocity();
   Vector2 worldVelocity2 =
       Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
@@ -214,8 +243,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
 #endif
 }
 
-void NetworkSync::SendDestroyObject(SendBuffer sendBuffer,
+void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) {
-                                    InterestingThing *obj) {
 #ifdef RC_DEBUG
   Serial.print("Send Destroy ");
   Serial.println((int)obj->id);
@@ -224,3 +252,12 @@ void NetworkSync::SendDestroyObject(SendBuffer sendBuffer,
   sendBuffer(buffer, 2);
 #endif
 }
+
+void NetworkSync::SendInvestigateThing(InterestingThing *thing) {
+  // printf("Investigate [%d/%d]\n", thing->networkId, thing->id);
+  unsigned char ix = 0;
+  buffer[ix++] = InvestigateMsg;
+  buffer[ix++] = thing->networkId;
+  buffer[ix++] = thing->id;
+  SendBuffer(ix);
+}

NetworkSync.h

@@ -11,6 +11,8 @@ namespace RoboidControl {
 /// @brief Interface for synchronizaing state between clients across a network
 class NetworkSync {
 public:
+  unsigned char networkId;
+
   /// @brief Retreive and send the roboid state
   /// @param roboid The roboid for which the state is updated
   virtual void NetworkUpdate(Roboid *roboid) = 0;
@@ -19,39 +21,53 @@ public:
   virtual void DestroyObject(InterestingThing *obj) = 0;
   virtual void NewObject(InterestingThing *obj){};
   /// @brief The id of a Pose message
-  static const char PoseMsg = 0x10;
+  static const unsigned char PoseMsg = 0x10;
-  static const char PoseTypeMsg = 0x11;
+  static const unsigned char PoseTypeMsg = 0x11;
-  static const char RelativePoseMsg = 0x12;
+  static const unsigned char RelativePoseMsg = 0x12;
   /// @brief A bit pattern for the pose, stating that this message contains a
   /// position in world coordinates
-  static const char Pose_Position = 0x01;
+  static const unsigned char Pose_Position = 0x01;
   /// @brief A bit pattern for the pose, stating that this message contains an
   /// orientation in world coordinates
-  static const char Pose_Orientation = 0x02;
+  static const unsigned char Pose_Orientation = 0x02;
   /// @brief A bit pattern for the pose, stating that this messsage contains a
   /// linear velocity in world coordinates
-  static const char Pose_LinearVelocity = 0x04;
+  static const unsigned char Pose_LinearVelocity = 0x04;
   /// @brief A bit pattern for the pose, stating that this message contains an
   /// angular velocity in world coordinates
   static const char Pose_AngularVelocity = 0x08;
 
-  static const char DestroyMsg = 0x20;
+  static const unsigned char DestroyMsg = 0x20;
 
-  typedef void (*SendBuffer)(UInt8 *buffer, UInt16 bufferSize);
+  static const unsigned char AngVelocity2DMsg = 0x46;
+  static const unsigned char Position2DMsg = 0x61;
+  static const unsigned char Velocity2DMsg = 0x62;
+  static const unsigned char CreateMsg = 0x80;
+  static const unsigned char InvestigateMsg = 0x81;
+  static const unsigned char ClientMsg = 0xA0;
+  static const unsigned char NetworkIdMsg = 0xA1;
 
-  void SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid);
+  typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
-  void SendDestroyObject(SendBuffer sendBuffer, InterestingThing *obj);
+
+  void ReceiveMessage(Roboid *roboid, unsigned char bytecount);
+
+  void ReceiveNetworkId();
+
+  void SendInvestigateThing(InterestingThing *thing);
+
+  void SendPoseMsg(Buffer sendBuffer, Roboid *roboid);
+  void SendDestroyObject(Buffer sendBuffer, InterestingThing *obj);
   void PublishNewObject();
 
-  void PublishTrackedObjects(SendBuffer sendBuffer, InterestingThing **objects);
+  void PublishTrackedObjects(Buffer sendBuffer, InterestingThing **objects);
 
   virtual void SendPosition(Vector3 worldPosition){};
   virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation){};
 
 protected:
   NetworkPerception *networkPerception;
-  void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object);
+  void PublishTrackedObject(Buffer sendBuffer, InterestingThing *object);
-  void PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId,
+  void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
                              InterestingThing *object);
 
   void SendSingle100(unsigned char *data, unsigned int startIndex, float value);
@@ -73,6 +89,11 @@ protected:
   // void SendSpherical32(unsigned char *data, int startIndex, Spherical s);
   void SendQuat32(unsigned char *data, unsigned char *startIndex,
                   const Quaternion q);
+
+  unsigned char buffer[32];
+  virtual void SendBuffer(unsigned char bufferSize);
+
+  void PublishClient();
 };
 
 } // namespace RoboidControl

Perception.cpp

@@ -163,6 +163,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
   if (availableSlotIx < maxObjectCount) {
     // a slot is available
     this->trackedObjects[availableSlotIx] = obj;
+    obj->networkId = 0x00;
     obj->id = availableSlotIx + 1;
 #ifdef RC_DEBUG2
     Serial.print((int)obj->id);
@@ -176,6 +177,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
            this->trackedObjects[farthestObjIx]->position.distance) {
     delete this->trackedObjects[farthestObjIx];
     this->trackedObjects[farthestObjIx] = obj;
+    obj->networkId = 0x00;
     obj->id = availableSlotIx + 1;
 #ifdef RC_DEBUG2
     Serial.print((int)obj->id);
@@ -228,6 +230,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
   if (availableSlotIx < maxObjectCount) {
     // a slot is available
     this->trackedObjects[availableSlotIx] = obj;
+    obj->networkId = 0x00;
     obj->id = availableSlotIx + 1;
 #ifdef RC_DEBUG2
     Serial.print((int)obj->id);
@@ -240,6 +243,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
            this->trackedObjects[farthestObjIx]->position.distance) {
     delete this->trackedObjects[farthestObjIx];
     this->trackedObjects[farthestObjIx] = obj;
+    obj->networkId = 0x00;
     obj->id = availableSlotIx + 1;
 #ifdef RC_DEBUG2
     Serial.print((int)obj->id);
@@ -257,6 +261,21 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
   }
 }
 
+InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
+                                               unsigned char networkId,
+                                               unsigned char objectId,
+                                               Spherical position,
+                                               Quaternion orientation) {
+  InterestingThing *thing = FindTrackedObject(networkId, objectId);
+  if (thing == nullptr) {
+    thing = AddTrackedObject(sensor, position, orientation);
+    thing->networkId = networkId;
+    thing->id = objectId;
+    thing->type = 0xFF; // unknown
+  }
+  return thing;
+}
+
 InterestingThing *Perception::FindTrackedObject(char objectId) {
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
     InterestingThing *thing = this->trackedObjects[objIx];
@@ -269,6 +288,20 @@ InterestingThing *Perception::FindTrackedObject(char objectId) {
   return nullptr;
 }
 
+InterestingThing *Perception::FindTrackedObject(unsigned char networkId,
+                                                unsigned char objectId) {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+
+    if (thing->networkId == networkId && thing->id == objectId) {
+      return thing;
+    }
+  }
+  return nullptr;
+}
+
 unsigned char Perception::TrackedObjectCount() {
   unsigned char objectCount = 0;
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
@@ -282,7 +315,7 @@ InterestingThing **Perception::GetTrackedObjects() {
   return this->trackedObjects;
 }
 
-unsigned char Perception::GetThingsOfType(unsigned char objectType,
+unsigned char Perception::ThingsOfType(unsigned char objectType,
                                        InterestingThing *buffer[],
                                        unsigned char bufferSize) {
   unsigned char thingCount = 0;

Perception.h

@@ -91,11 +91,19 @@ public:
   AddTrackedObject(Sensor *sensor, Spherical position,
                    Quaternion orientation = Quaternion::identity);
 
+  InterestingThing *
+  AddTrackedObject(Sensor *sensor, unsigned char networkId,
+                   unsigned char objectId, Spherical position,
+                   Quaternion orientation = Quaternion::identity);
+
   InterestingThing *FindTrackedObject(char objectId);
+  InterestingThing *FindTrackedObject(unsigned char networkId,
+                                      unsigned char objectId);
 
   /// @brief Retrieve the number of objects currently being tracked by the
   /// roboid
-  /// @return The object of objects, which is always lower than maxObjectCount
+  /// @return The object of objects, which is always lower than
+  /// maxObjectCount
   unsigned char TrackedObjectCount();
   /// @brief Retreive the objects currently tracked by the roboid
   /// @return An array of current objects
@@ -104,7 +112,7 @@ public:
   /// currently being tracked.
   InterestingThing **GetTrackedObjects();
 
-  unsigned char GetThingsOfType(unsigned char objectType,
+  unsigned char ThingsOfType(unsigned char objectType,
                              InterestingThing *buffer[],
                              unsigned char bufferSize);
 

TrackedObject.h

@@ -58,8 +58,9 @@ public:
   /// coordinated position->angle = 30 and position->angle = 27 can be the same.
   static constexpr float equalityAngle = 5.0F;
 
+  unsigned char networkId;
   /// @brief The id of the tracked object
-  char id;
+  unsigned char id;
 
   char parentId = 0;
   /// @brief The current position of the object