Added Sphere perception

NetworkPerception.cpp

@@ -107,6 +107,30 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
   roboid->perception->AddTrackedObject(this, position, 0x80, networkId);
 }
 
+void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
+  unsigned char networkId = data[1];
+
+  float radius = ReceiveFloat100(data, 3);
+
+  Vector3 worldPosition = ReceiveVector3(data, 7);
+  Vector3 roboidPosition = roboid->GetPosition();
+  Vector3 deltaPosition = worldPosition - roboidPosition;
+
+  Quaternion roboidOrientation = roboid->GetOrientation();
+  Vector3 localPosition =
+      Quaternion::Inverse(roboidOrientation) * deltaPosition;
+
+  Spherical position = Spherical(localPosition);
+
+  roboid->perception->AddTrackedObject(this, position, Quaternion::identity,
+                                       0x81, 0x81, networkId);
+  roboid->networkSync->SendText("Received Sphere\n\0");
+  if ((float)position.horizontalAngle == 0)
+    roboid->networkSync->SendText("Zero hor angle\0");
+  if ((float)position.verticalAngle == 0)
+    roboid->networkSync->SendText("Zero vertical angle\0");
+}
+
 void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char objectId = data[2];
@@ -118,6 +142,8 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
 
   if (objectId == 0x80)
     return ReceivePlane(data, roboid);
+  else if (objectId = 0x81)
+    return ReceiveSphere(data, roboid);
 
   if ((poseType & NetworkSync::Pose_Position) != 0) {
     Vector3 worldPosition = ReceiveVector3(data, 4);

NetworkPerception.h

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

Perception.cpp

@@ -189,9 +189,14 @@ bool Perception::ObjectNearby(float direction, float range) {
 }
 
 // #include <WifiSync.h>
+// This function is deprecated
 void Perception::AddTrackedObject(Sensor *sensor, Polar position,
                                   unsigned char thingType,
                                   unsigned char networkId) {
+  Spherical sPos = Spherical(position);
+  Quaternion orientation = Quaternion::identity;
+  AddTrackedObject(sensor, sPos, orientation, thingType, thingType, networkId);
+  /*
     InterestingThing *obj = new InterestingThing(sensor, position);
     obj->type = thingType;
 
@@ -232,10 +237,8 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
       obj->networkId = networkId;
       obj->id = lastObjectId++; // availableSlotIx + 1;
   #ifdef RC_DEBUG2
-    printf("%d: new tracked object [%d/%d]\n", availableSlotIx, obj->networkId,
+      printf("%d: new tracked object [%d/%d]\n", availableSlotIx,
-           obj->id);
+  obj->networkId, obj->id); #endif if (roboid->networkSync != nullptr) {
-#endif
-    if (roboid->networkSync != nullptr) {
         roboid->networkSync->NewObject(obj);
         // ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
       }
@@ -266,6 +269,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
       // No available slot, delete trackedobject
       delete obj;
     }
+  */
 }
 
 InterestingThing *
@@ -273,6 +277,8 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
                              Quaternion orientation, unsigned char thingType,
                              unsigned char thingId, unsigned networkId) {
   InterestingThing *thing = new InterestingThing(sensor, position, orientation);
+  if (thingId != 0x00)
+    thing->id = thingId;
   thing->type = thingType;
 
   unsigned char farthestObjIx = 0;
@@ -284,11 +290,11 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
     // Do we see the same object?
     else {
       if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
-#ifdef RC_DEBUG2
+        if ((float)thing->position.horizontalAngle !=
-        // Serial.print((int)this->trackedObjects[objIx]->id);
+            (float)this->trackedObjects[thingIx]->position.horizontalAngle)
-        // Serial.println(": update tracked object");
+          this->roboid->networkSync->SendText("Update\0");
-#endif
+        else
-
+          this->roboid->networkSync->SendText("Refresh\0");
         this->trackedObjects[thingIx]->Refresh(thing->position,
                                                thing->orientation);
         delete thing;
@@ -311,12 +317,6 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
     thing->networkId = networkId;
     if (thingId == 0x00)
       thing->id = lastObjectId++; // availableSlotIx + 1;
-    else
-      thing->id = thingId;
-#ifdef RC_DEBUG2
-    printf("%d: new tracked object {%d/%d}\n", availableSlotIx, obj->networkId,
-           obj->id);
-#endif
     return thing;
   }
   // If this object is closer than the farthest object, then replace it
@@ -327,17 +327,8 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position,
     thing->networkId = networkId;
     if (thingId == 0x00)
       thing->id = lastObjectId++; // availableSlotIx + 1;
-    else
-      thing->id = thingId;
-#ifdef RC_DEBUG2
-    printf("%d: replaced tracked object {%d/%d}\n", farthestObjIx,
-           obj->networkId, obj->id);
-#endif
     return thing;
   } else {
-#ifdef RC_DEBUG2
-    printf("%d: delete tracked object {%d/%d}\n", -1, obj->networkId, obj->id);
-#endif
     // No available slot, delete trackedobject
     delete thing;
     return nullptr;