diff --git a/Perception.cpp b/Perception.cpp index c277f22..c5d225d 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -3,11 +3,16 @@ #include "DistanceSensor.h" #include "Switch.h" +#include #include -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) { + } +} \ No newline at end of file diff --git a/Perception.h b/Perception.h index 1dafa1a..115bf92 100644 --- a/Perception.h +++ b/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 diff --git a/Roboid.cpp b/Roboid.cpp index 79d1a6a..36314d3 100644 --- a/Roboid.cpp +++ b/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); +} \ No newline at end of file