Plane update improvement

NetworkPerception.cpp

@@ -45,9 +45,30 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
     return;
 
   Vector3 worldPosition = ReceiveVector3(data, 4);
-  Vector3 worldAngles = ReceiveVector3(data, 16);
+  Vector3 roboidPosition = roboid->GetPosition();
-  Quaternion worldOrientation = Quaternion::Euler(worldAngles);
+  Vector3 deltaPosition = worldPosition - roboidPosition;
 
+  float distance = deltaPosition.magnitude();
+  if (roboid->perception->IsInteresting(distance) == false) {
+    printf("  plane not interesting\n");
+    return;
+  }
+
+  Quaternion roboidOrientation = roboid->GetOrientation();
+  Vector3 localPosition =
+      Quaternion::Inverse(roboidOrientation) * deltaPosition;
+  // float angle =
+  //     Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
+
+  // Polar position = Polar(angle, distance);
+  Spherical position = Spherical(localPosition);
+
+  // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x,
+  //        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,
+         (float)position.horizontalAngle, (float)position.verticalAngle);
   roboid->perception->AddTrackedObject(this, position, 0x80);
 }
 
@@ -58,7 +79,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
   // printf("My network id == %d\n", roboid->networkSync->networkId);
   if (networkId == roboid->networkSync->networkId)
     networkId = 0x00;
-  printf("Received pose message [%d/%d]\n", networkId, objectId);
+  // printf("Received pose message [%d/%d]\n", networkId, objectId);
 
   if (objectId == 0x80)
     return ReceivePlane(data, roboid);

NetworkSync.cpp

@@ -9,6 +9,8 @@
 #include "LinearAlgebra/Angle8.h"
 #include "LinearAlgebra/Spherical.h"
 
+NetworkSync::NetworkSync() { buffer = new unsigned char[32]; }
+
 void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
   networkPerception->ProcessPacket(roboid, buffer, bytecount);
 

NetworkSync.h

@@ -11,6 +11,8 @@ namespace RoboidControl {
 /// @brief Interface for synchronizaing state between clients across a network
 class NetworkSync {
 public:
+  NetworkSync();
+
   unsigned char networkId;
 
   /// @brief Retreive and send the roboid state
@@ -19,7 +21,7 @@ public:
   /// @brief Inform that the given object is no longer being tracked
   /// @param obj
   virtual void DestroyObject(InterestingThing *obj) = 0;
-  virtual void NewObject(InterestingThing *obj){};
+  virtual void NewObject(InterestingThing *obj) {};
   /// @brief The id of a Pose message
   static const unsigned char PoseMsg = 0x10;
   static const unsigned char PoseTypeMsg = 0x11;
@@ -61,8 +63,8 @@ public:
 
   void PublishTrackedObjects(Buffer sendBuffer, InterestingThing **objects);
 
-  virtual void SendPosition(Vector3 worldPosition){};
+  virtual void SendPosition(Vector3 worldPosition) {};
-  virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation){};
+  virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
 
 protected:
   NetworkPerception *networkPerception;
@@ -90,7 +92,7 @@ protected:
   void SendQuat32(unsigned char *data, unsigned char *startIndex,
                   const Quaternion q);
 
-  unsigned char buffer[32];
+  unsigned char *buffer; // =[32];
   virtual void SendBuffer(unsigned char bufferSize);
 
   void PublishClient();

Perception.cpp

@@ -69,6 +69,37 @@ Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
   return nullptr;
 }
 
+float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
+                       float range) {
+  float distance = plane->position.distance;
+  float deltaAngle = Angle::Normalize((float)plane->position.horizontalAngle -
+                                      horizontalAngle);
+  if (abs(deltaAngle) < abs(range)) {
+    // distance = distance
+    printf("   plane distance = %f (%f-%f)+%f=%f", distance,
+           (float)plane->position.horizontalAngle, horizontalAngle, range,
+           deltaAngle);
+
+  } else if (deltaAngle < -range) {
+    float angle = deltaAngle + range;
+    printf("   plane distance < %f (%f-%f)+%f=%f %f", distance,
+           (float)plane->position.horizontalAngle, horizontalAngle, range,
+           angle, cosf(angle * Angle::Deg2Rad));
+    if (angle > -90)
+      distance = distance / cosf(angle * Angle::Deg2Rad);
+
+  } else if (deltaAngle > range) {
+    float angle = deltaAngle - range;
+    printf("   plane distance > %f (%f-%f)-%f=%f %f", distance,
+           (float)plane->position.horizontalAngle, horizontalAngle, range,
+           angle, cosf(angle * Angle::Deg2Rad));
+    if (angle < 90)
+      distance = distance / cosf(angle * Angle::Deg2Rad);
+  }
+  printf("    distance = %f\n", distance);
+  return distance;
+}
+
 float Perception::GetDistance(float horizontalDirection, float range) {
   float minDistance = INFINITY;
   if (range < 0)
@@ -78,7 +109,11 @@ float Perception::GetDistance(float horizontalDirection, float range) {
     InterestingThing *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
-    if (obj->position.horizontalAngle > horizontalDirection - range &&
+
+    if (obj->type == 0x080) { // plane
+      float planeDistance = GetPlaneDistance(obj, horizontalDirection, range);
+      minDistance = fminf(minDistance, planeDistance);
+    } else if (obj->position.horizontalAngle > horizontalDirection - range &&
                obj->position.horizontalAngle < horizontalDirection + range) {
 
       minDistance = fminf(minDistance, obj->position.distance);
@@ -87,6 +122,31 @@ float Perception::GetDistance(float horizontalDirection, float range) {
   return minDistance;
 }
 
+float Perception::GetDistanceOfType(unsigned char thingType,
+                                    float horizontalAngle, float range) {
+  float minDistance = INFINITY;
+  if (range < 0)
+    range = -range;
+
+  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
+    InterestingThing *thing = trackedObjects[thingIx];
+    if (thing == nullptr)
+      continue;
+    if (thing->type != thingType)
+      continue;
+
+    if (thing->type == 0x080) { // plane
+      float planeDistance = GetPlaneDistance(thing, horizontalAngle, range);
+      minDistance = fminf(minDistance, planeDistance);
+    } else if (thing->position.horizontalAngle > horizontalAngle - range &&
+               thing->position.horizontalAngle < horizontalAngle + range) {
+
+      minDistance = fminf(minDistance, thing->position.distance);
+    }
+  }
+  return minDistance;
+}
+
 float Perception::GetDistance(float horizontalDirection,
                               float verticalDirection, float range) {
   float minDistance = INFINITY;
@@ -428,32 +488,50 @@ void Perception::Update(float currentTimeMs) {
   }
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    InterestingThing *obj = trackedObjects[objIx];
+    InterestingThing *thing = trackedObjects[objIx];
-    if (obj == nullptr)
+    if (thing == nullptr)
       continue;
 
-    if (obj->DegradeConfidence(deltaTime) == false) {
+    if (thing->DegradeConfidence(deltaTime) == false) {
       // delete obj
       if (roboid != nullptr && roboid->networkSync != nullptr) {
-        roboid->networkSync->DestroyObject(obj);
+        roboid->networkSync->DestroyObject(thing);
       }
       this->trackedObjects[objIx] = nullptr;
-      delete obj;
+      delete thing;
     }
   }
 }
 
 void Perception::UpdatePose(Polar translation) {
-  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
-    InterestingThing *obj = trackedObjects[objIx];
+    InterestingThing *thing = trackedObjects[thingIx];
-    if (obj == nullptr)
+    if (thing == nullptr)
       continue;
 
-    // We only support translations on polars at this moment...
+    // We only support translations in the horizontal plane at this moment...
     // This needs Spherical operator- to be implemented to work in 3d space
-    Polar horizontalPosition = obj->position.ProjectOnHorizontalPlane();
+    if (thing->type == 0x80) { // plane
+      printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
+             (float)thing->position.horizontalAngle,
+             (float)thing->position.verticalAngle);
+      // Update the closest point to the plane
+      float angle = (float)thing->position.horizontalAngle + translation.angle;
+      angle = abs(angle);
+
+      float deltaDistance = translation.distance * cosf(angle * Angle::Deg2Rad);
+      printf(" | translate %f %f %f | ", (float)translation.distance,
+             (float)angle, deltaDistance);
+      thing->position.distance -= deltaDistance;
+      printf("-> %f (%f %f)\n", thing->position.distance,
+             (float)thing->position.horizontalAngle,
+             (float)thing->position.verticalAngle);
+    } else {
+      Polar horizontalPosition =
+          Polar(thing->position); // obj->position.ProjectOnHorizontalPlane();
       Spherical newPosition = Spherical(horizontalPosition - translation);
-    obj->position = newPosition;
+      thing->position = newPosition;
+    }
   }
 }
 
@@ -466,13 +544,21 @@ void Perception::UpdatePose(Quaternion rotation) {
   if (rotationAxis.y < 0)
     rotationAngle = -rotationAngle;
 
-  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
-    InterestingThing *obj = trackedObjects[objIx];
+    InterestingThing *thing = trackedObjects[thingIx];
-    if (obj == nullptr)
+    if (thing == nullptr)
       continue;
 
+    printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
+           (float)thing->position.horizontalAngle,
+           (float)thing->position.verticalAngle);
+    printf("| rotate %f  | ", rotationAngle);
     float updatedAngle =
-        Angle::Normalize(obj->position.horizontalAngle - rotationAngle);
+        Angle::Normalize(thing->position.horizontalAngle - rotationAngle);
-    obj->position.horizontalAngle = updatedAngle;
+    thing->position.horizontalAngle = updatedAngle;
+
+    printf("-> %f (%f %f) \n", thing->position.distance,
+           (float)thing->position.horizontalAngle,
+           (float)thing->position.verticalAngle);
   }
 }

Perception.h

@@ -45,6 +45,8 @@ public:
   /// @param range The range in which objects should be looked for
   /// @return The distance to the closest object in meters
   float GetDistance(float direction, float range = 10.0F);
+  float GetDistanceOfType(unsigned char thingType, float horizontalAngle,
+                          float range = 10.0F);
   /// @brief Gets the distance to the closest object
   /// @param horizontalDirection The direction in the horizontal plane to look
   /// for objects

Roboid.cpp

@@ -15,6 +15,7 @@ Roboid::Roboid() {
   this->perception = new Perception();
   this->perception->roboid = this;
   this->propulsion = nullptr;
+  this->networkSync = nullptr;
   this->actuationRoot = nullptr;
   this->worldPosition = Vector3::zero;
   // this->worldOrientation = Quaternion::identity;
@@ -43,6 +44,7 @@ Roboid::Roboid(Perception *perception, ServoMotor *actuationRoot) : Roboid() {
 
 Roboid::Roboid(ServoMotor *actuationRoot) : actuationRoot(actuationRoot) {}
 
+#include <Arduino.h>
 void Roboid::Update(float currentTimeMs) {
   if (perception != nullptr)
     perception->Update(currentTimeMs);
@@ -65,6 +67,7 @@ void Roboid::Update(float currentTimeMs) {
 
   if (actuationRoot != nullptr)
     actuationRoot->Update(currentTimeMs);
+
   lastUpdateTimeMs = currentTimeMs;
 }
 

TrackedObject.cpp

@@ -20,11 +20,16 @@ InterestingThing::InterestingThing(Sensor *sensor, Spherical position,
   this->updated = true;
 }
 
+#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)otherObj->position.verticalAngle, position.distance,
+         (float)position.horizontalAngle, (float)position.verticalAngle);
   if (fabsf(position.distance - otherObj->position.distance) > equalityDistance)
     return false;
   if (fabsf(position.horizontalAngle - otherObj->position.horizontalAngle) >