Update object location based on roboid pose

Perception.cpp

@@ -175,10 +175,10 @@ void Perception::AddPerceivedObject(Polar position) {
   PerceivedObject *obj = new PerceivedObject(position);
   // objCount = PerceivedObjectCount();
   // printf("perc obj count %d\n");
-  AddPerceivedObject(obj);
-}
+  //   AddPerceivedObject(obj);
+  // }
 
-void Perception::AddPerceivedObject(PerceivedObject *obj) {
+  // void Perception::AddPerceivedObject(PerceivedObject *obj) {
   unsigned char farthestObjIx = 0;
   unsigned char availableSlotIx = 0;
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
@@ -188,16 +188,22 @@ void Perception::AddPerceivedObject(PerceivedObject *obj) {
       availableSlotIx = objIx;
     }
     // Do we see the same object?
-    else if (obj->IsTheSameAs(this->perceivedObjects[objIx])) {
-      printf("[%d] Updating...\n", objIx);
-      this->perceivedObjects[objIx]->Refresh(obj->position, obj->radius);
-      return;
-    }
-    // Is this the fartest object we see?
-    else if (this->perceivedObjects[farthestObjIx] == nullptr ||
-             (this->perceivedObjects[objIx]->position.distance >
-              this->perceivedObjects[farthestObjIx]->position.distance)) {
-      farthestObjIx = objIx;
+    else {
+      printf("(%d) my %f %f =^= received %f %f\n", objIx,
+             this->perceivedObjects[objIx]->position.distance,
+             this->perceivedObjects[objIx]->position.angle,
+             obj->position.distance, obj->position.angle);
+      if (obj->IsTheSameAs(this->perceivedObjects[objIx])) {
+        printf("[%d] Updating...\n", objIx);
+        this->perceivedObjects[objIx]->Refresh(obj->position, obj->radius);
+        return;
+      }
+      // Is this the fartest object we see?
+      else if (this->perceivedObjects[farthestObjIx] == nullptr ||
+               (this->perceivedObjects[objIx]->position.distance >
+                this->perceivedObjects[farthestObjIx]->position.distance)) {
+        farthestObjIx = objIx;
+      }
     }
   }
 
@@ -253,4 +259,34 @@ void Perception::Update(float currentTimeMs) {
   }
   if (this->perceivedObjects[0] != nullptr) {
   }
+}
+
+void Perception::UpdatePose(Polar translation) {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    PerceivedObject *obj = perceivedObjects[objIx];
+    if (obj == nullptr)
+      continue;
+
+    Polar newPosition = obj->position - translation;
+    obj->position = newPosition;
+  }
+}
+
+void Perception::UpdatePose(Quaternion rotation) {
+  // only rotation around vertical axis is supported for now
+  float rotationAngle;
+  Vector3 rotationAxis;
+  rotation.ToAngleAxis(&rotationAngle, &rotationAxis);
+  // Make sure rotation axis is positive
+  if (rotationAxis.y < 0)
+    rotationAngle = -rotationAngle;
+
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    PerceivedObject *obj = perceivedObjects[objIx];
+    if (obj == nullptr)
+      continue;
+
+    float updatedAngle = Angle::Normalize(obj->position.angle - rotationAngle);
+    obj->position.angle = updatedAngle;
+  }
 }

Perception.h

@@ -2,6 +2,7 @@
 
 #include "Placement.h"
 #include "Polar.h"
+#include "Quaternion.h"
 #include "Sensor.h"
 
 namespace Passer {
@@ -95,13 +96,16 @@ public:
   // Object Perception
 
   void AddPerceivedObject(Polar position);
-  void AddPerceivedObject(PerceivedObject *obj);
+  // void AddPerceivedObject(PerceivedObject *obj);
   unsigned char PerceivedObjectCount();
   PerceivedObject **GetPerceivedObjects();
 
   // mainly used for confidence update
   void Update(float currentTimeMs);
 
+  void UpdatePose(Polar translation);
+  void UpdatePose(Quaternion rotation);
+
   float nearbyDistance = 0.3F;
 
 protected:

Propulsion.cpp

@@ -1,7 +1,10 @@
 #include "Propulsion.h"
+#include "Roboid.h"
 
 #include "FloatSingle.h"
 
+#include <Arduino.h>
+
 Propulsion::Propulsion() {
   this->placement = nullptr;
   this->motorCount = 0;
@@ -46,12 +49,21 @@ float Propulsion::GetAngularVelocity() { return 0; }
 
 Vector3 Propulsion::GetPosition() { return this->worldPosition; }
 
-Quaternion Propulsion::GetOrientation() { return Quaternion::identity; }
+Quaternion Propulsion::GetOrientation() { return this->worldOrientation; }
 
-void Propulsion::SetPosition(Vector3 worldPosition) {
-  this->worldPosition = worldPosition;
+void Propulsion::SetPosition(Vector3 newWorldPosition) {
+  Vector3 translation = newWorldPosition - this->worldPosition;
+  float distance = translation.magnitude();
+  float angle = Vector3::SignedAngle(this->worldOrientation * Vector3::forward,
+                                     translation, Vector3::up);
+  Polar polarTranslation = Polar(angle, distance);
+  roboid->perception->UpdatePose(polarTranslation);
+  this->worldPosition = newWorldPosition;
 }
 
 void Propulsion::SetOrientation(Quaternion worldOrientation) {
+  Quaternion delta =
+      Quaternion::Inverse(this->worldOrientation) * worldOrientation;
+  roboid->perception->UpdatePose(delta);
   this->worldOrientation = worldOrientation;
 }

Propulsion.h

@@ -9,6 +9,8 @@
 namespace Passer {
 namespace RoboidControl {
 
+class Roboid;
+
 /// @brief The Propulsion module for a Roboid is used to move the Roboid in
 /// space
 ///
@@ -70,6 +72,8 @@ public:
   virtual void SetPosition(Vector3 worldPosition);
   virtual void SetOrientation(Quaternion worldOrientation);
 
+  Roboid *roboid = nullptr;
+
 protected:
   /// @brief The number of motors used for Propulsion
   unsigned int motorCount = 0;

Roboid.cpp

@@ -5,6 +5,7 @@ Roboid::Roboid() {}
 Roboid::Roboid(Perception *perception, Propulsion *propulsion) {
   this->perception = perception;
   this->propulsion = propulsion;
+  propulsion->roboid = this;
 }
 
 void Roboid::Update(float currentTimeMs) {