Performance improvement

DifferentialDrive.cpp

@@ -47,7 +47,6 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
   };
 }
 
-#include <Arduino.h>
 void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
   float leftSpeed =
       Float::Clamp(forward + yaw, -1, 1); // revolutions per second

NetworkPerception.cpp

@@ -20,7 +20,6 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
   }
 }
 
-#include <Arduino.h>
 void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char objectId = data[2];
@@ -37,6 +36,7 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
 }
 
 void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
+  unsigned char networkId = data[1];
   unsigned char poseType = data[3];
   if ((poseType & NetworkSync::Pose_Position) == 0 |
       (poseType & NetworkSync::Pose_Orientation) == 0)
@@ -50,7 +50,7 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
 
   float distance = deltaPosition.magnitude();
   if (roboid->perception->IsInteresting(distance) == false) {
-    printf("  plane not interesting\n");
+    // printf("  plane not interesting\n");
     return;
   }
 
@@ -67,9 +67,9 @@ 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);
+  //        (float)position.horizontalAngle, (float)position.verticalAngle);
-  roboid->perception->AddTrackedObject(this, position, 0x80);
+  roboid->perception->AddTrackedObject(this, position, 0x80, networkId);
 }
 
 void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
@@ -93,7 +93,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
 
       float distance = Vector3::Distance(roboidPosition, worldPosition);
       if (roboid->perception->IsInteresting(distance) == false) {
-        printf("  not interesting\n");
+        // printf("  not interesting\n");
         return;
       }
 

Perception.cpp

@@ -6,7 +6,7 @@
 
 #include <math.h>
 
-#define RC_DEBUG2
+// #define RC_DEBUG2
 #ifdef RC_DEBUG2
 #include <Arduino.h>
 #endif
@@ -76,27 +76,31 @@ float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
                                       horizontalAngle);
   if (abs(deltaAngle) < abs(range)) {
     // distance = distance
-    printf("   plane distance = %f (%f-%f)+%f=%f", distance,
+    // printf("   plane distance = %f (%f-%f)+%f=%f", distance,
-           (float)plane->position.horizontalAngle, horizontalAngle, range,
+    //        (float)plane->position.horizontalAngle, horizontalAngle, range,
-           deltaAngle);
+    //        deltaAngle);
 
   } else if (deltaAngle < -range) {
     float angle = deltaAngle + range;
-    printf("   plane distance < %f (%f-%f)+%f=%f %f", distance,
+    // printf("   plane distance < %f (%f-%f)+%f=%f %f", distance,
-           (float)plane->position.horizontalAngle, horizontalAngle, range,
+    //        (float)plane->position.horizontalAngle, horizontalAngle, range,
-           angle, cosf(angle * Angle::Deg2Rad));
+    //        angle, cosf(angle * Angle::Deg2Rad));
     if (angle > -90)
       distance = distance / cosf(angle * Angle::Deg2Rad);
+    else
+      distance = 9999; // infinity?
 
   } else if (deltaAngle > range) {
     float angle = deltaAngle - range;
-    printf("   plane distance > %f (%f-%f)-%f=%f %f", distance,
+    // printf("   plane distance > %f (%f-%f)-%f=%f %f", distance,
-           (float)plane->position.horizontalAngle, horizontalAngle, range,
+    //        (float)plane->position.horizontalAngle, horizontalAngle, range,
-           angle, cosf(angle * Angle::Deg2Rad));
+    //        angle, cosf(angle * Angle::Deg2Rad));
     if (angle < 90)
       distance = distance / cosf(angle * Angle::Deg2Rad);
+    else
+      distance = 9999; // infinity?
   }
-  printf("    distance = %f\n", distance);
+  // printf("    distance = %f\n", distance);
   return distance;
 }
 
@@ -186,7 +190,8 @@ bool Perception::ObjectNearby(float direction, float range) {
 
 #include <WifiSync.h>
 void Perception::AddTrackedObject(Sensor *sensor, Polar position,
-                                  unsigned char thingType) {
+                                  unsigned char thingType,
+                                  unsigned char networkId) {
   InterestingThing *obj = new InterestingThing(sensor, position);
   obj->type = thingType;
 
@@ -224,10 +229,11 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
   if (availableSlotIx >= 0) { //< maxObjectCount) {
     // a slot is available
     this->trackedObjects[availableSlotIx] = obj;
-    obj->networkId = 0x00;
+    obj->networkId = networkId;
     obj->id = lastObjectId++; // availableSlotIx + 1;
 #ifdef RC_DEBUG2
-    printf("%d: new tracked object [/%d]\n", availableSlotIx, obj->id);
+    printf("%d: new tracked object [%d/%d]\n", availableSlotIx, obj->networkId,
+           obj->id);
 #endif
     roboid->networkSync->NewObject(obj);
     ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
@@ -237,7 +243,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->networkId = networkId;
     obj->id = lastObjectId++; // availableSlotIx + 1;
 #ifdef RC_DEBUG2
     // Serial.print((int)obj->id);

Perception.h

@@ -88,7 +88,8 @@ public:
   /// @param position The position of the sensor in polar coordinates local to
   /// the roboid
   void AddTrackedObject(Sensor *sensor, Polar position,
-                        unsigned char objectType = 0x00);
+                        unsigned char objectType = 0x00,
+                        unsigned char networkId = 0x00);
   InterestingThing *
   AddTrackedObject(Sensor *sensor, Spherical position,
                    Quaternion orientation = Quaternion::identity,

TrackedObject.cpp

@@ -20,24 +20,33 @@ InterestingThing::InterestingThing(Sensor *sensor, Spherical position,
   this->updated = true;
 }
 
-#include <Arduino.h>
+// #include <Arduino.h>
 bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) {
   if (id != 0 && id == otherObj->id)
     return true;
   if (type != otherObj->type)
     return false;
-  printf(" %f (%f %f) is same as %f (%f %f)\n", otherObj->position.distance,
+
-         (float)otherObj->position.horizontalAngle,
+  float equalDistance = equalityDistance;
-         (float)otherObj->position.verticalAngle, position.distance,
+  float equalAngle = equalityAngle;
-         (float)position.horizontalAngle, (float)position.verticalAngle);
+  if (otherObj->type == 0x80 || otherObj->id == 0x80) {
-  if (fabsf(position.distance - otherObj->position.distance) > equalityDistance)
+    equalDistance = 0.1;
+    equalAngle = 10;
+  }
+  // printf(" %d %f (%f %f) is same as %f (%f %f)\n", otherObj->type,
+  //        otherObj->position.distance,
+  //        (float)otherObj->position.horizontalAngle,
+  //        (float)otherObj->position.verticalAngle, position.distance,
+  //        (float)position.horizontalAngle, (float)position.verticalAngle);
+  if (fabsf(position.distance - otherObj->position.distance) > equalDistance)
     return false;
   if (fabsf(position.horizontalAngle - otherObj->position.horizontalAngle) >
-      equalityAngle)
+      equalAngle)
     return false;
   if (fabsf(position.verticalAngle - otherObj->position.verticalAngle) >
-      equalityAngle)
+      equalAngle)
     return false;
+  // printf(" -> yes ");
   return true;
 }