Renamed perceived object to tracked object

NetworkSync.h

@@ -6,15 +6,29 @@
 namespace Passer {
 namespace RoboidControl {
 
+/// @brief Interface for synchronizaing state between clients across a network
 class NetworkSync {
 public:
+  /// @brief Retreive and send the roboid state
+  /// @param roboid The roboid for which the state is updated
   virtual void NetworkUpdate(Roboid *roboid) = 0;
-  virtual void DestroyObject(PerceivedObject *obj) = 0;
+  /// @brief Inform that the given object is no longer being tracked
+  /// @param obj
+  virtual void DestroyObject(TrackedObject *obj) = 0;
 
+  /// @brief The id of a Pose message
   static const char PoseMsg = 0x10;
+  /// @brief A bit pattern for the pose, stating that this message contains a
+  /// position in world coordinates
   static const char Pose_Position = 0x01;
+  /// @brief A bit pattern for the pose, stating that this message contains an
+  /// orientation in world coordinates
   static const char Pose_Orientation = 0x02;
+  /// @brief A bit pattern for the pose, stating that this messsage contains a
+  /// linear velocity in world coordinates
   static const char Pose_LinearVelocity = 0x04;
+  /// @brief A bit pattern for the pose, stating that this message contains an
+  /// angular velocity in world coordinates
   static const char Pose_AngularVelocity = 0x08;
 };
 

PerceivedObject.h

@@ -1,27 +0,0 @@
-#pragma once
-
-#include "Polar.h"
-#include "Sensor.h"
-
-class PerceivedObject {
-public:
-  PerceivedObject();
-  PerceivedObject(Sensor *sensor, Polar position, float radius = 0.1F);
-
-  static constexpr float equalityDistance = 0.3F;
-  static constexpr float equalityAngle = 5.0F;
-  bool IsTheSameAs(PerceivedObject *otherObj);
-
-  char id;
-
-  Polar position = Polar::zero;
-  float radius;
-  Sensor *sensor = nullptr;
-
-  static constexpr unsigned char maxConfidence = 255;
-  static constexpr unsigned char confidenceDropSpeed = 2;
-  unsigned char confidence;
-
-  bool DegradeConfidence(float deltaTime);
-  void Refresh(Polar position, float radius);
-};

Perception.cpp

@@ -8,7 +8,7 @@
 
 Perception::Perception() {
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
-    this->perceivedObjects[objIx] = nullptr;
+    this->trackedObjects[objIx] = nullptr;
 }
 
 Perception::Perception(Placement *sensors, unsigned int sensorCount)
@@ -46,7 +46,7 @@ float Perception::GetDistance(float direction, float range) {
     range = -range;
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    PerceivedObject *obj = perceivedObjects[objIx];
+    TrackedObject *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
     if (obj->position.angle > direction - range &&
@@ -65,7 +65,7 @@ float Perception::GetDistance(float horizontalDirection,
     range = -range;
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    PerceivedObject *obj = perceivedObjects[objIx];
+    TrackedObject *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
     if (obj->position.angle > horizontalDirection - range &&
@@ -82,7 +82,7 @@ bool Perception::ObjectNearby(float direction, float range) {
     range = -range;
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    PerceivedObject *obj = perceivedObjects[objIx];
+    TrackedObject *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
 
@@ -95,40 +95,25 @@ bool Perception::ObjectNearby(float direction, float range) {
   return false;
 }
 
-void Perception::AddPerceivedObject(Sensor *sensor, Polar position) {
+void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
-  // int objCount = PerceivedObjectCount();
+  TrackedObject *obj = new TrackedObject(sensor, position);
-  // printf("perc obj count %d\n");
 
-  PerceivedObject *obj = new PerceivedObject(sensor, position);
-  // objCount = PerceivedObjectCount();
-  // printf("perc obj count %d\n");
-  //   AddPerceivedObject(obj);
-  // }
-
-  // void Perception::AddPerceivedObject(PerceivedObject *obj) {
   unsigned char farthestObjIx = 0;
   unsigned char availableSlotIx = 0;
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    // printf("[%d] %d\n", objIx, this->perceivedObjects[objIx]);
+    if (this->trackedObjects[objIx] == nullptr) {
-    // Is this slot available?
-    if (this->perceivedObjects[objIx] == nullptr) {
       availableSlotIx = objIx;
     }
     // Do we see the same object?
     else {
-      // printf("(%d) my %f %f =^= received %f %f\n", objIx,
+      if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
-      //        this->perceivedObjects[objIx]->position.distance,
+        this->trackedObjects[objIx]->Refresh(obj->position);
-      //        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 ||
+      else if (this->trackedObjects[farthestObjIx] == nullptr ||
-               (this->perceivedObjects[objIx]->position.distance >
+               (this->trackedObjects[objIx]->position.distance >
-                this->perceivedObjects[farthestObjIx]->position.distance)) {
+                this->trackedObjects[farthestObjIx]->position.distance)) {
         farthestObjIx = objIx;
       }
     }
@@ -138,30 +123,27 @@ void Perception::AddPerceivedObject(Sensor *sensor, Polar position) {
   // max number of objects)
   if (availableSlotIx < maxObjectCount) {
     // a slot is available
-    // printf("[%d] new object \n", availableSlotIx);
+    this->trackedObjects[availableSlotIx] = obj;
-    this->perceivedObjects[availableSlotIx] = obj;
   }
   // If this object is closer than the farthest object, then replace it
   else if (obj->position.distance <
-           this->perceivedObjects[farthestObjIx]->position.distance) {
+           this->trackedObjects[farthestObjIx]->position.distance) {
-    this->perceivedObjects[farthestObjIx] = obj;
+    this->trackedObjects[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 Perception::TrackedObjectCount() {
   unsigned char objectCount = 0;
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    if (this->perceivedObjects[objIx] != nullptr)
+    if (this->trackedObjects[objIx] != nullptr)
       objectCount++;
   }
   return objectCount;
 }
 
-PerceivedObject **Perception::GetPerceivedObjects() {
+TrackedObject **Perception::GetTrackedObjects() { return this->trackedObjects; }
-  return this->perceivedObjects;
-}
 
 void Perception::Update(float currentTimeMs) {
   float deltaTime = currentTimeMs - lastUpdateTimeMs;
@@ -183,40 +165,36 @@ void Perception::Update(float currentTimeMs) {
       float distance = distanceSensor->GetDistance();
       float angle = thingPlacement.horizontalDirection;
       Polar position = Polar(angle, distance);
-      AddPerceivedObject(distanceSensor, position);
+      AddTrackedObject(distanceSensor, position);
     } else if (thing->type == Thing::SwitchType) {
       Switch *switchSensor = (Switch *)thing;
       if (switchSensor != nullptr && switchSensor->IsOn()) {
         Polar position =
             Polar(thingPlacement.horizontalDirection, nearbyDistance);
-        AddPerceivedObject(switchSensor, position);
+        AddTrackedObject(switchSensor, position);
       }
     }
   }
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    PerceivedObject *obj = perceivedObjects[objIx];
+    TrackedObject *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
 
     if (obj->DegradeConfidence(deltaTime) == false) {
       // delete obj
-      // printf("[%d] delete object\n", objIx);
       if (roboid != nullptr && roboid->networkSync != nullptr)
         roboid->networkSync->DestroyObject(obj);
-      this->perceivedObjects[objIx] = nullptr;
+      this->trackedObjects[objIx] = nullptr;
-      // } else {
-      //   Serial.printf("[%d] confidence: %d\n", objIx,
-      //                 this->perceivedObjects[objIx]->confidence);
     }
   }
-  if (this->perceivedObjects[0] != nullptr) {
+  if (this->trackedObjects[0] != nullptr) {
   }
 }
 
 void Perception::UpdatePose(Polar translation) {
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    PerceivedObject *obj = perceivedObjects[objIx];
+    TrackedObject *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
 
@@ -235,7 +213,7 @@ void Perception::UpdatePose(Quaternion rotation) {
     rotationAngle = -rotationAngle;
 
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-    PerceivedObject *obj = perceivedObjects[objIx];
+    TrackedObject *obj = trackedObjects[objIx];
     if (obj == nullptr)
       continue;
 

Perception.h

@@ -1,10 +1,10 @@
 #pragma once
 
-#include "PerceivedObject.h"
 #include "Placement.h"
 #include "Polar.h"
 #include "Quaternion.h"
 #include "Sensor.h"
+#include "TrackedObject.h"
 
 namespace Passer {
 namespace RoboidControl {
@@ -22,13 +22,18 @@ public:
   /// @param sensorCount The number of sensors in the placement array
   Perception(Placement *sensors, unsigned int sensorCount);
 
+  /// @brief The roboid of this perception system
   Roboid *roboid = nullptr;
 
   /// @brief Get the number of Sensors
   /// @return The number of sensors, zero when no sensors are present
   unsigned int GetSensorCount();
   Sensor *GetSensor(unsigned int sensorId);
-
+  /// @brief Find the first sensor of the given type
+  /// @param sensorType The type of sensor as is defined in the Thing class (for
+  /// example Thing::SensorType)
+  /// @return The first sensor found or a nullptr which no sensor has been found
+  /// of the given type
   Sensor *FindSensorOfType(unsigned int sensorType);
 
   /// @brief Gets the distance to the closest object
@@ -43,8 +48,8 @@ public:
   /// objects
   /// @param range The range in which objects should be looked for
   /// @return The distance to the closest object in meters
-  /// The directions can be thought of as the polar angle (vertical) and
+  /// @details The directions can be thought of as the polar angle (vertical)
-  /// azimuthal angle (horizontal) in the spherical coordinate system.
+  /// and azimuthal angle (horizontal) in the spherical coordinate system.
   float GetDistance(float horizontalDirection, float verticalDirection,
                     float range = 10.0F);
 
@@ -52,8 +57,8 @@ public:
   /// @param direction The direction to look for objects
   /// @param range The range in which objects should be looked for
   /// @return True when an object is close, False otherwise
-  /// Wether an object is closeby depends on the sensor. This can be a sensor
+  /// @details Wether an object is closeby depends on the sensor. This can be a
-  /// like a Switch or a DistanceSensor. The latter uses the
+  /// sensor like a Switch or a DistanceSensor. The latter uses the
   /// DistanceSensor::triggerDistance to check if an object is nearby.
   bool ObjectNearby(float direction, float range = 10.0F);
   /// @brief Checks if an object is nearby
@@ -63,8 +68,8 @@ public:
   /// objects
   /// @param range The range in which objects should be looked for
   /// @return True when an object is close, False otherwise
-  /// Wether an object is closeby depends on the sensor. This can be a sensor
+  /// @details Wether an object is closeby depends on the sensor. This can be a
-  /// like a Switch or a DistanceSensor. The latter uses the
+  /// sensor like a Switch or a DistanceSensor. The latter uses the
   /// DistanceSensor::triggerDistance to check if an object is nearby.
   ///
   /// The directions can be thought of as the polar angle (vertical) and
@@ -72,19 +77,48 @@ public:
   // bool ObjectNearby(float horizontalDirection, float verticalDirection,
   //                   float range = 10.0F);
 
-  // Object Perception
+  /// @brief Add or update an object detected by the given sensor
-
+  /// @param sensor The sensor which has detected the object
-  void AddPerceivedObject(Sensor *sensor, Polar position);
+  /// @param position The position of the sensor in polar coordinates local to
-  // void AddPerceivedObject(PerceivedObject *obj);
+  /// the roboid
-  unsigned char PerceivedObjectCount();
+  void AddTrackedObject(Sensor *sensor, Polar position);
-  PerceivedObject **GetPerceivedObjects();
+  /// @brief Retrieve the number of objects currently being tracked by the
+  /// roboid
+  /// @return The object of objects, which is always lower than maxObjectCount
+  unsigned char TrackedObjectCount();
+  /// @brief Retreive the objects currently tracked by the roboid
+  /// @return An array of current objects
+  /// @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();
 
   // mainly used for confidence update
+
+  /// @brief Update the state of the perception.
+  /// @param currentTimeMs The current time in milliseconds
+  /// @details This will update the perceptoin of object. It will retrieve the
+  /// latest state for each sensor and update the confidence of the tracked
+  /// objects.
   void Update(float currentTimeMs);
 
+  /// @brief Update the position/orientation of the preceived objects from the
+  /// given roboid translation
+  /// @param translation The translation of the roboid in world space in polar
+  /// coordinates
+  /// @details This function will be called through Roboid::SetPosition. It
+  /// is advised to use that function to update the roboid position instead of
+  /// this function.
   void UpdatePose(Polar translation);
+  /// @brief Update the orientation of the perceived objecst from the given
+  /// roboid rotation
+  /// @param rotation The rotation of the roboid in world space
   void UpdatePose(Quaternion rotation);
 
+  /// @brief Objects with a distance closed that this value will be considered
+  /// nearby.
+  /// @details This value is used by the ObjectNearby function to select the
+  /// objects
   float nearbyDistance = 0.3F;
 
 public:
@@ -96,10 +130,9 @@ public:
   float lastUpdateTimeMs = 0;
 
   static const unsigned char maxObjectCount = 7;
-  PerceivedObject
+  TrackedObject *trackedObjects[maxObjectCount]; // 7 is typically the maximum
-      *perceivedObjects[maxObjectCount]; // 7 is typically the maximum number of
+                                                 // number of object which can
-                                         // object which can be tracked by a
+                                                 // be tracked by a human
-                                         // human
 };
 
 } // namespace RoboidControl

TrackedObject.cpp

@@ -1,26 +1,15 @@
-#include "PerceivedObject.h"
+#include "TrackedObject.h"
 
 #include <math.h>
 
-/***
+TrackedObject::TrackedObject(Sensor *sensor, Polar position) {
- * Oject perception
- ***/
-
-PerceivedObject::PerceivedObject() {
   this->id = 0;
   this->confidence = maxConfidence;
-  this->position = Polar(0, INFINITY);
-  this->radius = INFINITY;
-}
-
-PerceivedObject::PerceivedObject(Sensor *sensor, Polar position, float radius)
-    : PerceivedObject() {
   this->sensor = sensor;
   this->position = position;
-  this->radius = radius;
 }
 
-bool PerceivedObject::IsTheSameAs(PerceivedObject *otherObj) {
+bool TrackedObject::IsTheSameAs(TrackedObject *otherObj) {
   if (id != 0 && id == otherObj->id)
     return true;
   if (fabsf(position.distance - otherObj->position.distance) > equalityDistance)
@@ -30,7 +19,7 @@ bool PerceivedObject::IsTheSameAs(PerceivedObject *otherObj) {
   return true;
 }
 
-bool PerceivedObject::DegradeConfidence(float deltaTime) {
+bool TrackedObject::DegradeConfidence(float deltaTime) {
   unsigned char confidenceDrop =
       (unsigned char)((float)confidenceDropSpeed * deltaTime);
   // Make sure the confidence always drops
@@ -47,8 +36,7 @@ bool PerceivedObject::DegradeConfidence(float deltaTime) {
   }
 }
 
-void PerceivedObject::Refresh(Polar position, float radius) {
+void TrackedObject::Refresh(Polar position) {
   this->position = position;
-  this->radius = radius;
   this->confidence = maxConfidence;
 }

TrackedObject.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include "Polar.h"
+#include "Sensor.h"
+
+class TrackedObject {
+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);
+
+  /// @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);
+
+  /// @brief Decrease the confidence based on the elapsed time
+  /// @param deltaTime The time since the last DegradeConfidence call
+  /// @return Returns false when the object's confidence has reached zero, true
+  /// otherwise
+  /// @details When this function returns false, the object should no longer be
+  /// tracked.
+  bool DegradeConfidence(float deltaTime);
+
+  /// @brief Determine whether this object is the same as another object
+  /// @param otherObj The other object to compare to
+  /// @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);
+  /// @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
+  /// value, the objects can be the same.
+  /// @example with a value of 0.3, objects with coordinates position->distance
+  /// = 1.2F and position->distance = 1.6 will be considered different, but
+  /// objects with coordinates position->distance = 1.2 and position->distance
+  /// = 1.0 can be the same.
+  static constexpr float equalityDistance = 0.3F;
+  /// @brief The maximum difference in angle from the roboids orientation in
+  /// which two objects may be considered the same
+  /// @details When the difference in angle is exactly this value, the objects
+  /// can be the same.
+  /// @example With a value of 5.0, object with coordinates position->angle = 30
+  /// and position->angle = 36 will be considered different, but object with
+  /// coordinated position->angle = 30 and position->angle = 27 can be the same.
+  static constexpr float equalityAngle = 5.0F;
+
+  /// @brief The id of the tracked object
+  char id;
+
+  /// @brief The current position of the object
+  Polar position = Polar::zero;
+  /// @brief The sensor which provided that lastet pose this object
+  Sensor *sensor = nullptr;
+
+protected:
+  static constexpr unsigned char maxConfidence = 255;
+  static constexpr unsigned char confidenceDropSpeed = 2;
+  unsigned char confidence;
+};