Add initial plane object support

NetworkPerception.cpp

@@ -36,6 +36,21 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
   }
 }
 
+void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
+  unsigned char poseType = data[3];
+  if ((poseType & NetworkSync::Pose_Position) == 0 |
+      (poseType & NetworkSync::Pose_Orientation) == 0)
+
+    // both position and orientation are required
+    return;
+
+  Vector3 worldPosition = ReceiveVector3(data, 4);
+  Vector3 worldAngles = ReceiveVector3(data, 16);
+  Quaternion worldOrientation = Quaternion::Euler(worldAngles);
+
+  roboid->perception->AddTrackedObject(this, position, 0x80);
+}
+
 void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char objectId = data[2];
@@ -45,6 +60,9 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
     networkId = 0x00;
   printf("Received pose message [%d/%d]\n", networkId, objectId);
 
+  if (objectId == 0x80)
+    return ReceivePlane(data, roboid);
+
   if ((poseType & NetworkSync::Pose_Position) != 0) {
     Vector3 worldPosition = ReceiveVector3(data, 4);
     if (objectId == 0) {
@@ -92,46 +110,6 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
       }
     }
   }
-
-  /*
-    Vector3 worldAngles = ReceiveVector3(data, 16);
-    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);
-    }
-  */
 }
 
 // HACK!!!

NetworkPerception.h

@@ -16,6 +16,8 @@ protected:
   void ReceivePoseMsg(unsigned char *data, Roboid *roboid);
   void ReceiveTypedObject(unsigned char *data, Roboid *roboid);
 
+  void ReceivePlane(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);

TrackedObject.cpp

@@ -7,6 +7,7 @@ InterestingThing::InterestingThing(Sensor *sensor, Polar position) {
   this->confidence = maxConfidence;
   this->sensor = sensor;
   this->position = Spherical(position);
+  this->updated = true;
 }
 
 InterestingThing::InterestingThing(Sensor *sensor, Spherical position,
@@ -16,6 +17,7 @@ InterestingThing::InterestingThing(Sensor *sensor, Spherical position,
   this->sensor = sensor;
   this->position = position;
   this->orientation = orientation;
+  this->updated = true;
 }
 
 bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) {
@@ -54,10 +56,12 @@ bool InterestingThing::DegradeConfidence(float deltaTime) {
 void InterestingThing::Refresh(Polar position) {
   this->position = Spherical(position);
   this->confidence = maxConfidence;
+  this->updated = true;
 }
 
 void InterestingThing::Refresh(Spherical position, Quaternion orientation) {
   this->position = position;
   this->orientation = orientation;
   this->confidence = maxConfidence;
+  this->updated = true;
 }

TrackedObject.h

@@ -72,6 +72,7 @@ public:
 
   unsigned char type = 0x00;
   unsigned char confidence;
+  bool updated = false;
 
 protected:
   static constexpr unsigned char maxConfidence = 255;