Add typed object receiver

NetworkPerception.cpp

@@ -9,6 +9,8 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
 
   if (buffer[0] == NetworkSync::PoseMsg) {
     ReceiveObject(buffer, roboid);
+  } else if (buffer[0] == NetworkSync::PoseTypeMsg) {
+    ReceiveTypedObject(buffer, roboid);
   }
 }
 
@@ -76,3 +78,30 @@ void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) {
     roboid->perception->AddTrackedObject(this, position);
   }
 }
+
+#include "../../../src/LighthouseTracker.h"
+#include <Arduino.h>
+
+void NetworkPerception::ReceiveTypedObject(unsigned char *data,
+                                           Roboid *roboid) {
+  unsigned char objectType = data[1];
+  unsigned char objectId = data[2];
+  if (objectType == 0x02) { // lighthouse
+    // We require position and orientation for lighthouses
+    if (data[3] != NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)
+      return;
+
+    Vector3 worldPosition = ReceiveVector3(data, 4);
+    Vector3 worldAngles = ReceiveVector3(data, 16);
+    Quaternion worldOrientation = Quaternion::Euler(worldAngles);
+
+    // roboid->perception->AddTrackedObject(this, objectType, worldPosition,
+    //                                      worldOrientation);
+    Sensor *sensor =
+        roboid->perception->FindSensorOfType(0x82); // Lighthouse type sensor
+    LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
+    lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
+  }
+
+  Serial.printf("Typed Object received\n");
+}

NetworkPerception.h

@@ -13,6 +13,7 @@ public:
 
 protected:
   void ReceiveObject(unsigned char *data, Roboid *roboid);
+  void ReceiveTypedObject(unsigned char *data, Roboid *roboid);
 
   Int32 ReceiveInt32(unsigned char *data, int startIndex);
   float ReceiveFloat100(unsigned char *data, int startIndex);

NetworkSync.h

@@ -20,6 +20,7 @@ public:
 
   /// @brief The id of a Pose message
   static const char PoseMsg = 0x10;
+  static const char PoseTypeMsg = 0x11;
   /// @brief A bit pattern for the pose, stating that this message contains a
   /// position in world coordinates
   static const char Pose_Position = 0x01;
@@ -42,8 +43,8 @@ public:
 
   void PublishTrackedObjects(SendBuffer sendBuffer, InterestingThing **objects);
 
-  virtual void SendPosition(Vector3 worldPosition) {};
-  virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
+  virtual void SendPosition(Vector3 worldPosition){};
+  virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation){};
 
 protected:
   NetworkPerception *networkPerception;

Perception.cpp

@@ -247,6 +247,11 @@ void Perception::AddTrackedObject(Sensor *sensor, Spherical position) {
   }
 }
 
+// void Perception::AddTrackedObject(Sensor *sensor, uint8_t objectType,
+//                                   Vector3 position, Quaternion orientation) {
+
+//                                   }
+
 unsigned char Perception::TrackedObjectCount() {
   unsigned char objectCount = 0;
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {

Roboid.cpp

@@ -68,8 +68,8 @@ void Roboid::SetPosition(Vector3 newWorldPosition) {
   this->worldPosition = newWorldPosition;
 
   if (networkSync != nullptr)
-    // networkSync->SendPosition(this->worldPosition);
-    networkSync->SendPose(this->worldPosition, this->worldOrientation);
+    networkSync->SendPosition(this->worldPosition);
+  // networkSync->SendPose(this->worldPosition, this->worldOrientation);
 }
 
 void Roboid::SetOrientation(Quaternion worldOrientation) {