diff --git a/Perception.cpp b/Perception.cpp index 5f1a162..4778241 100644 --- a/Perception.cpp +++ b/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; } diff --git a/Perception.h b/Perception.h index 5524d75..2e55765 100644 --- a/Perception.h +++ b/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;