Working object perception

Perception.cpp

@@ -3,11 +3,16 @@
 #include "DistanceSensor.h"
 #include "Switch.h"
 
+#include <Arduino.h>
 #include <math.h>
 
-Perception::Perception() {}
+Perception::Perception() {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
+    this->perceivedObjects[objIx] = nullptr;
+}
 
-Perception::Perception(Placement *sensors, unsigned int sensorCount) {
+Perception::Perception(Placement *sensors, unsigned int sensorCount)
+    : Perception() {
   this->sensorCount = sensorCount;
   this->sensorPlacements = (Placement *)sensors;
 }
@@ -100,3 +105,122 @@ bool Perception::ObjectNearby(float direction, float range) {
   }
   return false;
 }
+
+/***
+ * Oject perception
+ ***/
+
+PerceivedObject::PerceivedObject() {
+  this->id = 0;
+  this->confidence = maxConfidence;
+  this->position = Polar(0, INFINITY);
+  this->radius = INFINITY;
+}
+
+PerceivedObject::PerceivedObject(Polar position, float radius)
+    : PerceivedObject() {
+  this->position = position;
+  this->radius = radius;
+}
+
+bool PerceivedObject::IsTheSameAs(PerceivedObject *otherObj) {
+  if (id != 0 && id == otherObj->id)
+    return true;
+  if (abs(position.distance - otherObj->position.distance) > equalityDistance)
+    return false;
+  if (abs(position.angle - otherObj->position.angle) > equalityAngle)
+    return false;
+  return true;
+}
+
+bool PerceivedObject::DegradeConfidence(float deltaTime) {
+  unsigned char confidenceDrop =
+      (unsigned char)((float)confidenceDropSpeed * deltaTime);
+  // Make sure the confidence always drops
+  if (confidenceDrop == 0)
+    confidenceDrop = 1;
+
+  if (confidence <= confidenceDrop) {
+    // object is dead
+    confidence = 0;
+    return false;
+  } else {
+    confidence -= confidenceDrop;
+    return true;
+  }
+}
+
+void PerceivedObject::Refresh(Polar position, float radius) {
+  this->position = position;
+  this->radius = radius;
+  this->confidence = maxConfidence;
+}
+
+void Perception::AddPerceivedObject(PerceivedObject *obj) {
+  unsigned char farthestObjIx = 0;
+  unsigned char availableSlotIx = 0;
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    // Is this slot available?
+    if (perceivedObjects[objIx] == nullptr) {
+      availableSlotIx = objIx;
+    }
+    // Do we see the same object?
+    else if (obj->IsTheSameAs(perceivedObjects[objIx])) {
+      perceivedObjects[objIx]->Refresh(obj->position, obj->radius);
+      return;
+    }
+    // Is this the fartest object we see?
+    else if (perceivedObjects[farthestObjIx] == nullptr ||
+             (perceivedObjects[objIx]->position.distance >
+              perceivedObjects[farthestObjIx]->position.distance)) {
+      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
+    perceivedObjects[availableSlotIx] = obj;
+  }
+  // If this object is closer than the farthest object, then replace it
+  else if (obj->position.distance <
+           perceivedObjects[farthestObjIx]->position.distance) {
+    perceivedObjects[farthestObjIx] = obj;
+    // we may want to destroy the fartest object, but if it is created
+    // externally, other links may still exist...
+  }
+}
+
+unsigned char Perception::PerceivedObjectCount() {
+  unsigned char objectCount = 0;
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    if (perceivedObjects[objIx] != nullptr)
+      objectCount++;
+  }
+  return objectCount;
+}
+
+void Perception::Update(float currentTimeMs) {
+  float deltaTime = currentTimeMs - lastUpdateTimeMs;
+  if (deltaTime <= 0)
+    return;
+
+  lastUpdateTimeMs = currentTimeMs;
+
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    PerceivedObject *obj = perceivedObjects[objIx];
+    if (obj == nullptr)
+      continue;
+
+    if (obj->DegradeConfidence(deltaTime) == false) {
+      // delete obj
+      perceivedObjects[objIx] = nullptr;
+    } else {
+      Serial.printf("[%d] confidence: %d\n", objIx,
+                    perceivedObjects[objIx]->confidence);
+    }
+  }
+  if (perceivedObjects[0] != nullptr) {
+  }
+}

Perception.h

@@ -1,11 +1,33 @@
 #pragma once
 
 #include "Placement.h"
+#include "Polar.h"
 #include "Sensor.h"
 
 namespace Passer {
 namespace RoboidControl {
 
+class PerceivedObject {
+public:
+  PerceivedObject();
+  PerceivedObject(Polar position, float radius);
+
+  static constexpr float equalityDistance = 0.3F;
+  static constexpr float equalityAngle = 5.0F;
+  bool IsTheSameAs(PerceivedObject *otherObj);
+
+  int id;
+
+  Polar position;
+  float radius;
+
+  static constexpr char maxConfidence = 255;
+  static constexpr char confidenceDropSpeed = 2;
+  char confidence;
+  bool DegradeConfidence(float deltaTime);
+  void Refresh(Polar position, float radius);
+};
+
 /// @brief Module to which keeps track of objects around the roboid
 class Perception {
 public:
@@ -70,11 +92,27 @@ public:
   bool ObjectNearby(float horizontalDirection, float verticalDirection,
                     float range = 10.0F);
 
+  // Object Perception
+
+  void AddPerceivedObject(PerceivedObject *obj);
+  unsigned char PerceivedObjectCount();
+
+  // mainly used for confidence update
+  void Update(float currentTimeMs);
+
 protected:
   /// @brief The Placement of the Sensors used for Perception
   Placement *sensorPlacements = nullptr;
   /// @brief The number of Sensors used for Perception
   unsigned int sensorCount = 0;
+
+  float lastUpdateTimeMs = 0;
+
+  static const unsigned char maxObjectCount = 7;
+  PerceivedObject
+      *perceivedObjects[maxObjectCount]; // 7 is typically the maximum number of
+                                         // object which can be tracked by a
+                                         // human
 };
 
 } // namespace RoboidControl

Roboid.cpp

@@ -2,7 +2,12 @@
 
 Roboid::Roboid() {}
 
-Roboid::Roboid(Perception* perception, Propulsion* propulsion) {
+Roboid::Roboid(Perception *perception, Propulsion *propulsion) {
   this->perception = perception;
   this->propulsion = propulsion;
 }
+
+void Roboid::Update(float currentTimeMs) {
+  if (perception != nullptr)
+    perception->Update(currentTimeMs);
+}