Improve perception

NetworkPerception.cpp

@@ -2,16 +2,18 @@
 
 #include "NetworkSync.h"
 
-// #define DEBUG true
+#define RC_DEBUG true
-#if DEBUG
+#if RC_DEBUG
 #include <Arduino.h>
 #endif
 
 void NetworkPerception::ProcessPacket(Roboid* roboid,
                                       unsigned char* buffer,
                                       int packetsize) {
+#if RC_DEBUG
   // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
   //        buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]);
+#endif
 
   switch (buffer[0]) {
     case NetworkSync::CreateMsg:
@@ -49,7 +51,7 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data,
   unsigned char networkId = data[1];
   unsigned char objectId = data[2];
 
-#if DEBUG
+#if RC_DEBUG
   printf("Received InvestigateMsg [%d/%d]\n", networkId, objectId);
 #endif
 
@@ -103,8 +105,8 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) {
   //        worldPosition.y, worldPosition.z, roboidPosition.x,
   //        roboidPosition.y, roboidPosition.z, position.distance,
   //        (float)position.horizontalAngle, (float)position.verticalAngle);
-  // printf("Received plane %f (%f %f)\n", position.distance,
+  printf("Received plane %f (%f %f)\n", position.distance,
-  //        (float)position.horizontalAngle, (float)position.verticalAngle);
+         position.horizontalAngle.ToFloat(), position.verticalAngle.ToFloat());
   roboid->perception->AddTrackedObject(this, position, 0x80, networkId);
 }
 
@@ -135,6 +137,10 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
   if (networkId == roboid->networkSync->networkId)
     networkId = 0x00;
 
+#if RC_DEBUG
+  printf("Received PoseMsg [%d/%d]\n", networkId, objectId);
+#endif
+
   if (objectId == 0x80)
     return ReceivePlane(data, roboid);
   else if (objectId == 0x81)

NetworkSync.cpp

@@ -1,6 +1,6 @@
 #include "NetworkSync.h"
 
-// #define RC_DEBUG 1
+#define RC_DEBUG 1
 
 #ifdef RC_DEBUG
 #include <Arduino.h>
@@ -54,7 +54,7 @@ void NetworkSync::NewObject(InterestingThing* thing) {
   SendBuffer(ix);
 
 #ifdef RC_DEBUG
-  printf("Sent CreateMsg %d %d\n", buffer[1], buffer[2]);
+  printf("Sent CreateMsg [%d/%d] %d\n", networkId, buffer[1], buffer[2]);
 #endif
 
   // PublishTrackedObject(roboid, obj);
@@ -148,6 +148,10 @@ void NetworkSync::SendPose(Roboid* roboid, bool recurse) {
   SendQuat32(buffer, &ix, roboid->GetOrientation());
   SendBuffer(ix);
 
+#if RC_DEBUG
+  printf("Sent PoseMsg [%d/%d]\n", networkId, buffer[1]);
+#endif
+
   if (recurse) {
     for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) {
       Thing* child = roboid->GetChild(childIx);
@@ -166,6 +170,10 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) {
   SendQuat32(buffer, &ix, thing->orientation);
   SendBuffer(ix);
 
+#if RC_DEBUG
+  printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]);
+#endif
+
   if (recurse) {
     for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
       Thing* child = thing->GetChild(childIx);
@@ -224,16 +232,22 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
   buffer[ix++] = PoseMsg;     // Position2DMsg;
   buffer[ix++] = object->id;  // objectId;
   buffer[ix++] = Pose_Position | Pose_Orientation;
+  SendSpherical(buffer, &ix, Spherical(worldPosition));
+  SendQuat32(buffer, &ix, worldOrientation);
   // SendPolar(buffer, &ix, polar); // 3 bytes
-  SendVector3(buffer, &ix, worldPosition);
+  // SendVector3(buffer, &ix, worldPosition);
   // SendQuat32(buffer, &ix, worldOrientation);
   SendBuffer(ix);
 
+#if RC_DEBUG
+  printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], object->type);
+#endif
+
   object->updated = false;
 
-#if DEBUG
+#if RC_DEBUG
-  printf("PublishTrackedObj [%d/%d] (%f %f)\n", object->networkId, buffer[1],
+  // printf("PublishTrackedObj [%d/%d] (%f %f)\n", object->networkId, buffer[1],
-         worldPosition.Right(), worldPosition.Forward());
+  //        worldPosition.Right(), worldPosition.Forward());
 #endif
 }
 

Perception.cpp

@@ -193,6 +193,7 @@ bool Perception::ObjectNearby(float direction, float range) {
   }
   return false;
 }
+#include <Arduino.h>
 
 // #include <WifiSync.h>
 // This function is deprecated
@@ -203,7 +204,7 @@ void Perception::AddTrackedObject(Sensor* sensor,
   Spherical16 sPos =
       Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
   Quaternion orientation = Quaternion::identity;
-  AddTrackedObject(sensor, sPos, orientation, thingType, thingType, networkId);
+  AddTrackedObject(sensor, sPos, orientation, 0x00, thingType, networkId);
   /*
     InterestingThing *obj = new InterestingThing(sensor, position);
     obj->type = thingType;
@@ -300,14 +301,10 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
     // Do we see the same object?
     else {
       if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
-        // if ((float)thing->position.horizontalAngle !=
-        //     (float)this->trackedObjects[thingIx]->position.horizontalAngle)
-        //   this->roboid->networkSync->SendText("Update\0");
-        // else
-        //   this->roboid->networkSync->SendText("Refresh\0");
         this->trackedObjects[thingIx]->Refresh(
             thing->position, thing->orientation.ToQuaternion());
         delete thing;
+
         return this->trackedObjects[thingIx];
       }
       // Is this the fartest object we see?
@@ -347,15 +344,16 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
 
 InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
                                                unsigned char networkId,
-                                               unsigned char objectId,
+                                               unsigned char thingId,
-                                               Spherical16 position,
+                                               Spherical position,
                                                Quaternion orientation) {
-  InterestingThing* thing = FindTrackedObject(networkId, objectId);
+  InterestingThing* thing = FindTrackedObject(networkId, thingId);
   if (thing == nullptr) {
-    thing =
+    thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId,
-        AddTrackedObject(sensor, position, orientation, objectId, networkId);
+                             networkId);
+    // Don't we set this above already?
     thing->networkId = networkId;
-    thing->id = objectId;
+    thing->id = thingId;
     thing->type = 0xFF;  // unknown
   }
   return thing;

Perception.h

@@ -91,13 +91,13 @@ class Perception {
   /// the roboid
   void AddTrackedObject(Sensor* sensor,
                         Polar position,
-                        unsigned char objectType = 0x00,
+                        unsigned char objectType = 0xFF,
                         unsigned char networkId = 0x00);
   InterestingThing* AddTrackedObject(
       Sensor* sensor,
-      Spherical16 position,
+      Spherical position,
       Quaternion orientation = Quaternion::identity,
-      unsigned char objectType = 0x00,
+      unsigned char objectType = 0xFF,
       unsigned char objectId = 0x00,
       unsigned networkId = 0x00);
 
@@ -105,7 +105,7 @@ class Perception {
       Sensor* sensor,
       unsigned char networkId,
       unsigned char objectId,
-      Spherical16 position,
+      Spherical position,
       Quaternion orientation = Quaternion::identity);
 
   bool IsInteresting(float distance);