Renamed TrackedObject to InterestingThing

NetworkSync.cpp

@@ -27,9 +27,9 @@ void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) {
 }
 
 void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
-                                        TrackedObject **objects) {
+                                        InterestingThing **objects) {
   for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
-    TrackedObject *obj = objects[objIx];
+    InterestingThing *obj = objects[objIx];
     if (obj == nullptr)
       continue;
     // if (obj->sensor->type == Thing::ExternalType)
@@ -42,9 +42,10 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
 }
 
 void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
-                                       TrackedObject *object) {
+                                       InterestingThing *object) {
-  Vector2 worldPosition2 = Vector2::Rotate(
+  Vector2 worldPosition2 =
-      Vector2::forward * object->position.distance, -object->position.angle);
+      Vector2::Rotate(Vector2::forward * object->position.magnitude,
+                      -object->position.horizontalAngle);
   Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
 
 #ifdef RC_DEBUG
@@ -57,7 +58,7 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
   Serial.print(", ");
   Serial.println(worldPosition3.z);
 #else
-  UInt16 bufferSize = 3 + 12;
+  const UInt16 bufferSize = 3 + 12;
   UInt8 buffer[bufferSize] = {
       PoseMsg,
       object->id,
@@ -104,7 +105,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
 #endif
 }
 
-void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj) {
+void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, InterestingThing *obj) {
 #ifdef RC_DEBUG
   Serial.print("Send Destroy ");
   Serial.println((int)obj->id);

NetworkSync.h

@@ -16,7 +16,7 @@ public:
   virtual void NetworkUpdate(Roboid *roboid) = 0;
   /// @brief Inform that the given object is no longer being tracked
   /// @param obj
-  virtual void DestroyObject(TrackedObject *obj) = 0;
+  virtual void DestroyObject(InterestingThing *obj) = 0;
 
   /// @brief The id of a Pose message
   static const char PoseMsg = 0x10;
@@ -38,13 +38,13 @@ public:
   typedef void (*SendBuffer)(UInt8 *buffer, UInt16 bufferSize);
 
   void SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid);
-  void SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj);
+  void SendDestroyObject(SendBuffer sendBuffer, InterestingThing *obj);
 
-  void PublishTrackedObjects(SendBuffer sendBuffer, TrackedObject **objects);
+  void PublishTrackedObjects(SendBuffer sendBuffer, InterestingThing **objects);
 
 protected:
   NetworkPerception *networkPerception;
-  void PublishTrackedObject(SendBuffer sendBuffer, TrackedObject *object);
+  void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object);
 
   void SendVector3(unsigned char *data, int startIndex, Vector3 v);
   void SendSingle100(unsigned char *data, int startIndex, float value);

Perception.cpp

@@ -15,7 +15,7 @@ unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
                                               // be tracked by a human
 
 Perception::Perception() {
-  this->trackedObjects = new TrackedObject *[maxObjectCount];
+  this->trackedObjects = new InterestingThing *[maxObjectCount];
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
     this->trackedObjects[objIx] = nullptr;
 }
@@ -27,7 +27,7 @@ Perception::Perception(Sensor **sensors, unsigned int sensorCount)
   for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
     this->sensors[sensorIx] = sensors[sensorIx];
 
-  this->trackedObjects = new TrackedObject *[maxObjectCount];
+  this->trackedObjects = new InterestingThing *[maxObjectCount];
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
     this->trackedObjects[objIx] = nullptr;
 }
@@ -68,19 +68,19 @@ Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
   return nullptr;
 }
 
-float Perception::GetDistance(float direction, float range) {
+float Perception::GetDistance(float horizontalDirection, float range) {
   float minDistance = INFINITY;
   if (range < 0)
     range = -range;
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    TrackedObject *obj = trackedObjects[objIx];
+    InterestingThing *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
-    if (obj->position.angle > direction - range &&
+    if (obj->position.horizontalAngle > horizontalDirection - range &&
-        obj->position.angle < direction + range) {
+        obj->position.horizontalAngle < horizontalDirection + range) {
 
-      minDistance = fminf(minDistance, obj->position.distance);
+      minDistance = fminf(minDistance, obj->position.magnitude);
     }
   }
   return minDistance;
@@ -93,13 +93,13 @@ float Perception::GetDistance(float horizontalDirection,
     range = -range;
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    TrackedObject *obj = trackedObjects[objIx];
+    InterestingThing *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
-    if (obj->position.angle > horizontalDirection - range &&
+    if (obj->position.horizontalAngle > horizontalDirection - range &&
-        obj->position.angle < horizontalDirection + range) {
+        obj->position.horizontalAngle < horizontalDirection + range) {
 
-      minDistance = fminf(minDistance, obj->position.distance);
+      minDistance = fminf(minDistance, obj->position.magnitude);
     }
   }
 
@@ -110,13 +110,13 @@ bool Perception::ObjectNearby(float direction, float range) {
     range = -range;
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    TrackedObject *obj = trackedObjects[objIx];
+    InterestingThing *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
 
-    if (obj->position.angle > direction - range &&
+    if (obj->position.horizontalAngle > direction - range &&
-        obj->position.angle < direction + range) {
+        obj->position.horizontalAngle < direction + range) {
-      if (obj->position.distance <= nearbyDistance)
+      if (obj->position.magnitude <= nearbyDistance)
         return true;
     }
   }
@@ -124,7 +124,7 @@ bool Perception::ObjectNearby(float direction, float range) {
 }
 
 void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
-  TrackedObject *obj = new TrackedObject(sensor, position);
+  InterestingThing *obj = new InterestingThing(sensor, position);
 
   unsigned char farthestObjIx = 0;
   unsigned char availableSlotIx = 0;
@@ -146,8 +146,8 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
       }
       // Is this the fartest object we see?
       else if (this->trackedObjects[farthestObjIx] == nullptr ||
-               (this->trackedObjects[objIx]->position.distance >
+               (this->trackedObjects[objIx]->position.magnitude >
-                this->trackedObjects[farthestObjIx]->position.distance)) {
+                this->trackedObjects[farthestObjIx]->position.magnitude)) {
         farthestObjIx = objIx;
       }
     }
@@ -166,8 +166,70 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
 
   }
   // If this object is closer than the farthest object, then replace it
-  else if (obj->position.distance <
+  else if (obj->position.magnitude <
-           this->trackedObjects[farthestObjIx]->position.distance) {
+           this->trackedObjects[farthestObjIx]->position.magnitude) {
+    delete this->trackedObjects[farthestObjIx];
+    this->trackedObjects[farthestObjIx] = obj;
+    obj->id = availableSlotIx;
+#ifdef RC_DEBUG2
+    Serial.print((int)obj->id);
+    Serial.println(": replaced tracked object");
+#endif
+  } else {
+#ifdef RC_DEBUG2
+    Serial.print((int)obj->id);
+    Serial.println(": delete tracked object");
+#endif
+    // No available slot, delete trackedobject
+    delete obj;
+  }
+}
+
+void Perception::AddTrackedObject(Sensor *sensor, Spherical position) {
+  InterestingThing *obj = new InterestingThing(sensor, position);
+
+  unsigned char farthestObjIx = 0;
+  unsigned char availableSlotIx = 0;
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    if (this->trackedObjects[objIx] == nullptr) {
+      availableSlotIx = objIx;
+    }
+    // Do we see the same object?
+    else {
+      if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
+#ifdef RC_DEBUG2
+        Serial.print((int)this->trackedObjects[objIx]->id);
+        Serial.println(": update tracked object");
+#endif
+
+        this->trackedObjects[objIx]->Refresh(obj->position);
+        delete obj;
+        return;
+      }
+      // Is this the fartest object we see?
+      else if (this->trackedObjects[farthestObjIx] == nullptr ||
+               (this->trackedObjects[objIx]->position.magnitude >
+                this->trackedObjects[farthestObjIx]->position.magnitude)) {
+        farthestObjIx = objIx;
+      }
+    }
+  }
+
+  // Check if an perception slot is available (we currently see less than the
+  // max number of objects)
+  if (availableSlotIx < maxObjectCount) {
+    // a slot is available
+    this->trackedObjects[availableSlotIx] = obj;
+    obj->id = availableSlotIx;
+#ifdef RC_DEBUG2
+    Serial.print((int)obj->id);
+    Serial.println(": new tracked object");
+#endif
+
+  }
+  // If this object is closer than the farthest object, then replace it
+  else if (obj->position.magnitude <
+           this->trackedObjects[farthestObjIx]->position.magnitude) {
     delete this->trackedObjects[farthestObjIx];
     this->trackedObjects[farthestObjIx] = obj;
     obj->id = availableSlotIx;
@@ -194,10 +256,29 @@ unsigned char Perception::TrackedObjectCount() {
   return objectCount;
 }
 
-TrackedObject **Perception::GetTrackedObjects() { return this->trackedObjects; }
+InterestingThing **Perception::GetTrackedObjects() {
+  return this->trackedObjects;
+}
+
+InterestingThing *Perception::GetMostInterestingThing() {
+  InterestingThing *closestObject = nullptr;
+  float closestDistance = INFINITY;
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *obj = this->trackedObjects[objIx];
+    if (obj != nullptr) {
+      if (obj->position.magnitude < closestDistance) {
+        closestObject = obj;
+        closestDistance = obj->position.magnitude;
+      }
+    }
+  }
+  return closestObject;
+}
+
+#include <Arduino.h>
 
 void Perception::Update(float currentTimeMs) {
-  float deltaTime = currentTimeMs - lastUpdateTimeMs;
+  float deltaTime = (currentTimeMs - lastUpdateTimeMs) / 1000;
   if (deltaTime <= 0)
     return;
 
@@ -225,11 +306,13 @@ void Perception::Update(float currentTimeMs) {
         Polar position = Polar(sensor->position.angle, nearbyDistance);
         AddTrackedObject(switchSensor, position);
       }
+    } else {
+      sensor->Update();
     }
   }
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    TrackedObject *obj = trackedObjects[objIx];
+    InterestingThing *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
 
@@ -245,11 +328,14 @@ void Perception::Update(float currentTimeMs) {
 
 void Perception::UpdatePose(Polar translation) {
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    TrackedObject *obj = trackedObjects[objIx];
+    InterestingThing *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
 
-    Polar newPosition = obj->position - translation;
+    // We only support translations on polars at this moment...
+    // This needs Spherical operator- to be implemented to work in 3d space
+    Polar horizontalPosition = obj->position.ProjectOnHorizontalPlane();
+    Spherical newPosition = Spherical(horizontalPosition - translation);
     obj->position = newPosition;
   }
 }
@@ -264,11 +350,12 @@ void Perception::UpdatePose(Quaternion rotation) {
     rotationAngle = -rotationAngle;
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    TrackedObject *obj = trackedObjects[objIx];
+    InterestingThing *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
 
-    float updatedAngle = Angle::Normalize(obj->position.angle - rotationAngle);
+    float updatedAngle =
-    obj->position.angle = updatedAngle;
+        Angle::Normalize(obj->position.horizontalAngle - rotationAngle);
+    obj->position.horizontalAngle = updatedAngle;
   }
 }

Perception.h

@@ -4,6 +4,7 @@
 #include "TrackedObject.h"
 #include "VectorAlgebra/Polar.h"
 #include "VectorAlgebra/Quaternion.h"
+#include "VectorAlgebra/Spherical.h"
 
 // #include <vector.h>
 
@@ -85,6 +86,8 @@ public:
   /// @param position The position of the sensor in polar coordinates local to
   /// the roboid
   void AddTrackedObject(Sensor *sensor, Polar position);
+  void AddTrackedObject(Sensor *sensor, Spherical position);
+
   /// @brief Retrieve the number of objects currently being tracked by the
   /// roboid
   /// @return The object of objects, which is always lower than maxObjectCount
@@ -94,7 +97,9 @@ public:
   /// @details The returned array this should never be a nullptr, but
   /// each array entry may be a nullptr when less than maxObjectCount objects is
   /// currently being tracked.
-  TrackedObject **GetTrackedObjects();
+  InterestingThing **GetTrackedObjects();
+
+  InterestingThing *GetMostInterestingThing();
 
   // mainly used for confidence update
 
@@ -135,7 +140,7 @@ public:
   static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
                                        // number of object which can
                                        // be tracked by a human
-  TrackedObject **trackedObjects;
+  InterestingThing **trackedObjects;
 };
 
 } // namespace RoboidControl

Roboid.cpp

@@ -19,7 +19,15 @@ Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() {
   propulsion->roboid = this;
 }
 
-Roboid::Roboid(ServoMotor **actuation) : actuation(actuation) {}
+Roboid::Roboid(Perception *perception, ServoMotor *actuationRoot) : Roboid() {
+  this->perception = perception;
+  perception->roboid = this;
+  this->propulsion = nullptr;
+
+  this->actuationRoot = actuationRoot;
+}
+
+Roboid::Roboid(ServoMotor *actuationRoot) : actuationRoot(actuationRoot) {}
 
 void Roboid::Update(float currentTimeMs) {
   if (perception != nullptr)
@@ -28,6 +36,9 @@ void Roboid::Update(float currentTimeMs) {
     propulsion->Update(currentTimeMs);
   if (networkSync != nullptr)
     networkSync->NetworkUpdate(this);
+
+  if (actuationRoot != nullptr)
+    actuationRoot->Update(currentTimeMs);
 }
 
 Vector3 Roboid::GetPosition() { return this->worldPosition; }

Roboid.h

@@ -19,9 +19,9 @@ public:
   /// @param propulsion The Propulsion implementation to use for this Roboid
   Roboid(Perception *perception, Propulsion *propulsion = nullptr);
 
-  Roboid(Perception *perception, ServoMotor **actuation);
+  Roboid(Perception *perception, ServoMotor *actuationRoot);
 
-  Roboid(ServoMotor **actuation);
+  Roboid(ServoMotor *actuationRoot);
 
   /// @brief Update the state of the Roboid
   /// @param currentTimeMs The time in milliseconds when calling this
@@ -32,7 +32,7 @@ public:
   Perception *perception = nullptr;
   /// @brief The Propulsion module of this Roboid
   Propulsion *propulsion = nullptr;
-  ServoMotor **actuation = nullptr;
+  ServoMotor *actuationRoot = nullptr;
 
   /// @brief The reference to the module to synchronize states across a network
   NetworkSync *networkSync = nullptr;

Sensor.h

@@ -7,9 +7,11 @@ namespace RoboidControl {
 
 /// @brief A sensor is a thing which can perform measurements in the environment
 class Sensor : public Thing {
- public:
+public:
   /// @brief Default Constructor for a Sensor
   Sensor();
+
+  virtual void Update(){};
 };
 
 } // namespace RoboidControl

ServoMotor.cpp

@@ -3,6 +3,7 @@
 #include "VectorAlgebra/FloatSingle.h"
 
 ServoMotor::ServoMotor() {
+  this->type = Thing::ServoType;
   this->controlMode = ControlMode::Position;
   this->targetAngle = 0;
   this->hasTargetAngle = false;
@@ -41,6 +42,8 @@ void ServoMotor::SetTargetVelocity(float targetVelocity) {
 
 float ServoMotor::GetTargetVelocity() { return this->targetVelocity; }
 
+#include <Arduino.h>
+
 void ServoMotor::Update(float currentTimeMs) {
   if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
     this->lastUpdateTimeMs = currentTimeMs;
@@ -83,4 +86,13 @@ void ServoMotor::Update(float currentTimeMs) {
 
     this->lastUpdateTimeMs = currentTimeMs;
   }
+
+  Serial.println(this->childCount);
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    Thing *child = this->GetChild(childIx);
+    if (child->type == Thing::ServoType) {
+      ServoMotor *servo = (ServoMotor *)child;
+      servo->Update(currentTimeMs);
+    }
+  }
 }

Thing.cpp

@@ -4,6 +4,9 @@ using namespace Passer::RoboidControl;
 
 Thing::Thing() : position(Polar::zero) {
   this->type = (unsigned int)Type::Undetermined;
+  this->childCount = 0;
+  this->parent = nullptr;
+  this->children = nullptr;
 }
 
 const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch;
@@ -13,8 +16,35 @@ const unsigned int Thing::ControlledMotorType =
     MotorType | (unsigned int)Type::ControlledMotor;
 const unsigned int Thing::UncontrolledMotorType =
     MotorType | (unsigned int)Type::UncontrolledMotor;
+const unsigned int Thing::ServoType = (unsigned int)Type::Servo;
 const unsigned int Thing::ExternalType = (unsigned int)Type::ExternalSensor;
 
 bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; }
 
 bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; }
+
+void Thing::SetParent(Thing *parent) { this->parent = parent; }
+
+Thing *Thing::GetParent() { return this->parent; }
+
+void Thing::AddChild(Thing *child) {
+  Thing **newChildren = new Thing *[this->childCount + 1];
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++)
+    newChildren[childIx] = this->children[childIx];
+
+  newChildren[this->childCount] = child;
+  child->SetParent(this);
+
+  if (this->children != nullptr)
+    delete[] this->children;
+
+  this->children = newChildren;
+  this->childCount++;
+}
+
+Thing *Thing::GetChild(unsigned char childIx) {
+  if (childIx < this->childCount) {
+    return this->children[childIx];
+  } else
+    return nullptr;
+}

Thing.h

@@ -23,6 +23,7 @@ public:
   /// @brief The type of an uncontrolled motor
   static const unsigned int UncontrolledMotorType;
   /// @brief The type of an object received from the network
+  static const unsigned int ServoType;
   static const unsigned int ExternalType;
 
   /// @brief Check if the Thing is a Motor
@@ -34,6 +35,12 @@ public:
 
   Polar position;
 
+  void SetParent(Thing *parent);
+  Thing *GetParent();
+
+  void AddChild(Thing *child);
+  Thing *GetChild(unsigned char childIx);
+
 protected:
   /// @brief Bitmask for Motor type
   static const unsigned int MotorType = 0x8000;
@@ -49,9 +56,14 @@ protected:
     // Motor,
     ControlledMotor,
     UncontrolledMotor,
+    Servo,
     // Other
     ExternalSensor,
   };
+
+  Thing *parent = nullptr;
+  unsigned char childCount = 0;
+  Thing **children = nullptr;
 };
 
 } // namespace RoboidControl

TrackedObject.cpp

@@ -2,24 +2,38 @@
 
 #include <math.h>
 
-TrackedObject::TrackedObject(Sensor *sensor, Polar position) {
+InterestingThing::InterestingThing(Sensor *sensor, Polar position) {
+  this->id = 0;
+  this->confidence = maxConfidence;
+  this->sensor = sensor;
+  this->position = Spherical(position);
+}
+
+InterestingThing::InterestingThing(Sensor *sensor, Spherical position) {
   this->id = 0;
   this->confidence = maxConfidence;
   this->sensor = sensor;
   this->position = position;
 }
 
-bool TrackedObject::IsTheSameAs(TrackedObject *otherObj) {
+bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) {
   if (id != 0 && id == otherObj->id)
     return true;
-  if (fabsf(position.distance - otherObj->position.distance) > equalityDistance)
+  if (fabsf(position.magnitude - otherObj->position.magnitude) >
+      equalityDistance)
     return false;
-  if (fabsf(position.angle - otherObj->position.angle) > equalityAngle)
+  if (fabsf(position.horizontalAngle - otherObj->position.horizontalAngle) >
+      equalityAngle)
+    return false;
+  if (fabsf(position.verticalAngle - otherObj->position.verticalAngle) >
+      equalityAngle)
     return false;
   return true;
 }
 
-bool TrackedObject::DegradeConfidence(float deltaTime) {
+#include <Arduino.h>
+
+bool InterestingThing::DegradeConfidence(float deltaTime) {
   unsigned char confidenceDrop =
       (unsigned char)((float)confidenceDropSpeed * deltaTime);
   // Make sure the confidence always drops
@@ -36,7 +50,12 @@ bool TrackedObject::DegradeConfidence(float deltaTime) {
   }
 }
 
-void TrackedObject::Refresh(Polar position) {
+void InterestingThing::Refresh(Polar position) {
+  this->position = Spherical(position);
+  this->confidence = maxConfidence;
+}
+
+void InterestingThing::Refresh(Spherical position) {
   this->position = position;
   this->confidence = maxConfidence;
 }

TrackedObject.h

@@ -2,23 +2,26 @@
 
 #include "Sensor.h"
 #include "VectorAlgebra/Polar.h"
+#include "VectorAlgebra/Spherical.h"
 
 namespace Passer {
 namespace RoboidControl {
 
 /// @brief An object tracked by the roboid
-class TrackedObject {
+class InterestingThing {
 public:
   /// @brief An object tracked by the roboid
   /// @param sensor The Sensor which detected this object
   /// @param position The position in polar coordinates local to the roboid
-  TrackedObject(Sensor *sensor, Polar position);
+  InterestingThing(Sensor *sensor, Polar position);
+  InterestingThing(Sensor *sensor, Spherical position);
 
   /// @brief Update the position of the object
   /// @param position The latest known position of the object
   /// @details This will also update the confidence of the object to the
   /// maxConfidence value
   void Refresh(Polar position);
+  void Refresh(Spherical position);
 
   /// @brief Decrease the confidence based on the elapsed time
   /// @param deltaTime The time since the last DegradeConfidence call
@@ -33,7 +36,7 @@ public:
   /// @return Returns true when both objects are considered the same
   /// The result of this check depends on the equalityDistance and equalityAngle
   /// value.
-  bool IsTheSameAs(TrackedObject *otherObj);
+  bool IsTheSameAs(InterestingThing *otherObj);
   /// @brief The maximum difference in distance from the roboid in which two
   /// objects may be considered the same
   /// @details When the difference in distance is exactly this
@@ -56,14 +59,16 @@ public:
   char id;
 
   /// @brief The current position of the object
-  Polar position = Polar::zero;
+  // Polar position = Polar::zero;
+  Spherical position = Spherical::zero;
   /// @brief The sensor which provided that lastet pose this object
   Sensor *sensor = nullptr;
 
+  unsigned char confidence;
+
 protected:
   static constexpr unsigned char maxConfidence = 255;
-  static constexpr unsigned char confidenceDropSpeed = 1; // 2;
+  static constexpr unsigned char confidenceDropSpeed = 150; // 2;
-  unsigned char confidence;
 };
 
 } // namespace RoboidControl

test/BB2B_Test.cc

@@ -103,8 +103,8 @@ TEST(BB2B, NoObstacle) {
   trackedObjectCount = roboid->perception->TrackedObjectCount();
   EXPECT_EQ(trackedObjectCount, 0);
 
-  TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
+  InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects();
-  TrackedObject *trackedObject = nullptr;
+  InterestingThing *trackedObject = nullptr;
   for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
     if (trackedObjects[i] != nullptr)
       trackedObject = trackedObjects[0];
@@ -168,8 +168,8 @@ TEST(BB2B, ObstacleLeft) {
   EXPECT_EQ(trackedObjectCount, 1);
 
   // Find the single tracked object
-  TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
+  InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects();
-  TrackedObject *trackedObject = nullptr;
+  InterestingThing *trackedObject = nullptr;
   for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
     if (trackedObjects[i] != nullptr)
       trackedObject = trackedObjects[i];
@@ -269,8 +269,8 @@ TEST(BB2B, ObstacleRight) {
   EXPECT_EQ(trackedObjectCount, 1);
 
   // Find the single tracked object
-  TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
+  InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects();
-  TrackedObject *trackedObject = nullptr;
+  InterestingThing *trackedObject = nullptr;
   for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
     if (trackedObjects[i] != nullptr)
       trackedObject = trackedObjects[i];
@@ -368,7 +368,7 @@ TEST(BB2B, ObstacleBoth) {
   EXPECT_EQ(trackedObjectCount, 2);
 
   // Find the single tracked object
-  TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects();
+  InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects();
   for (int i = 0; i < roboid->perception->maxObjectCount; i++) {
     if (trackedObjects[i] != nullptr) {
       EXPECT_FLOAT_EQ(trackedObjects[i]->position.distance, 0.1F);