Add GetThingsOfType

Perception.cpp

@@ -277,6 +277,25 @@ InterestingThing **Perception::GetTrackedObjects() {
   return this->trackedObjects;
 }
 
+unsigned char Perception::GetThingsOfType(unsigned char objectType,
+                                          InterestingThing *buffer[],
+                                          unsigned char bufferSize) {
+  unsigned char thingCount = 0;
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+
+    if (thing->type == objectType) {
+      buffer[thingCount] = thing;
+      thingCount++;
+      if (thingCount >= bufferSize)
+        return bufferSize;
+    }
+  }
+  return thingCount;
+}
+
 InterestingThing *Perception::GetMostInterestingThing() {
   InterestingThing *closestObject = nullptr;
   float closestDistance = INFINITY;

Perception.h

@@ -103,6 +103,10 @@ public:
   /// currently being tracked.
   InterestingThing **GetTrackedObjects();
 
+  unsigned char GetThingsOfType(unsigned char objectType,
+                                InterestingThing *buffer[],
+                                unsigned char bufferSize);
+
   InterestingThing *GetMostInterestingThing();
 
   // mainly used for confidence update