Object perception

Perception.cpp

@@ -103,6 +103,18 @@ bool Perception::ObjectNearby(float direction, float range) {
       }
     }
   }
+
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    PerceivedObject *obj = perceivedObjects[objIx];
+    if (obj == nullptr)
+      continue;
+
+    if (obj->position.angle > direction - range &&
+        obj->position.angle < direction + range) {
+      if (obj->position.distance <= nearbyDistance)
+        return true;
+    }
+  }
   return false;
 }
 

Perception.h

@@ -89,8 +89,8 @@ public:
   ///
   /// The directions can be thought of as the polar angle (vertical) and
   /// azimuthal angle (horizontal) in the spherical coordinate system.
-  bool ObjectNearby(float horizontalDirection, float verticalDirection,
-                    float range = 10.0F);
+  // bool ObjectNearby(float horizontalDirection, float verticalDirection,
+  //                   float range = 10.0F);
 
   // Object Perception
 
@@ -102,6 +102,8 @@ public:
   // mainly used for confidence update
   void Update(float currentTimeMs);
 
+  float nearbyDistance = 0.3F;
+
 protected:
   /// @brief The Placement of the Sensors used for Perception
   Placement *sensorPlacements = nullptr;