From 2d94f61c833a39fb59dce67ca82ae5f2677c0ac5 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 2 Jan 2024 11:52:02 +0100 Subject: [PATCH] Renamed perceived object to tracked object --- NetworkSync.h | 16 +++++- PerceivedObject.h | 27 --------- Perception.cpp | 72 ++++++++---------------- Perception.h | 69 +++++++++++++++++------ PerceivedObject.cpp => TrackedObject.cpp | 22 ++------ TrackedObject.h | 63 +++++++++++++++++++++ 6 files changed, 159 insertions(+), 110 deletions(-) delete mode 100644 PerceivedObject.h rename PerceivedObject.cpp => TrackedObject.cpp (61%) create mode 100644 TrackedObject.h diff --git a/NetworkSync.h b/NetworkSync.h index 6f6e3a3..b3c4012 100644 --- a/NetworkSync.h +++ b/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; }; diff --git a/PerceivedObject.h b/PerceivedObject.h deleted file mode 100644 index 5b97ba3..0000000 --- a/PerceivedObject.h +++ /dev/null @@ -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); -}; diff --git a/Perception.cpp b/Perception.cpp index 095c00a..15bbed9 100644 --- a/Perception.cpp +++ b/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) { - // int objCount = PerceivedObjectCount(); - // printf("perc obj count %d\n"); +void Perception::AddTrackedObject(Sensor *sensor, Polar position) { + TrackedObject *obj = new TrackedObject(sensor, position); - 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]); - // Is this slot available? - if (this->perceivedObjects[objIx] == nullptr) { + if (this->trackedObjects[objIx] == nullptr) { availableSlotIx = objIx; } // Do we see the same object? else { - // printf("(%d) my %f %f =^= received %f %f\n", objIx, - // this->perceivedObjects[objIx]->position.distance, - // 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); + if (obj->IsTheSameAs(this->trackedObjects[objIx])) { + this->trackedObjects[objIx]->Refresh(obj->position); return; } // Is this the fartest object we see? - else if (this->perceivedObjects[farthestObjIx] == nullptr || - (this->perceivedObjects[objIx]->position.distance > - this->perceivedObjects[farthestObjIx]->position.distance)) { + else if (this->trackedObjects[farthestObjIx] == nullptr || + (this->trackedObjects[objIx]->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->perceivedObjects[availableSlotIx] = obj; + this->trackedObjects[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->perceivedObjects[farthestObjIx] = obj; + this->trackedObjects[farthestObjIx]->position.distance) { + 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() { - return this->perceivedObjects; -} +TrackedObject **Perception::GetTrackedObjects() { return this->trackedObjects; } 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; - // } else { - // Serial.printf("[%d] confidence: %d\n", objIx, - // this->perceivedObjects[objIx]->confidence); + this->trackedObjects[objIx] = nullptr; } } - 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; diff --git a/Perception.h b/Perception.h index 1519e32..d652872 100644 --- a/Perception.h +++ b/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 - /// azimuthal angle (horizontal) in the spherical coordinate system. + /// @details The directions can be thought of as the polar angle (vertical) + /// 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 - /// like a Switch or a DistanceSensor. The latter uses the + /// @details Wether an object is closeby depends on the sensor. This can be a + /// 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 - /// like a Switch or a DistanceSensor. The latter uses the + /// @details Wether an object is closeby depends on the sensor. This can be a + /// 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 - - void AddPerceivedObject(Sensor *sensor, Polar position); - // void AddPerceivedObject(PerceivedObject *obj); - unsigned char PerceivedObjectCount(); - PerceivedObject **GetPerceivedObjects(); + /// @brief Add or update an object detected by the given sensor + /// @param sensor The sensor which has detected the object + /// @param position The position of the sensor in polar coordinates local to + /// the roboid + void AddTrackedObject(Sensor *sensor, Polar position); + /// @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 - *perceivedObjects[maxObjectCount]; // 7 is typically the maximum number of - // object which can be tracked by a - // human + TrackedObject *trackedObjects[maxObjectCount]; // 7 is typically the maximum + // number of object which can + // be tracked by a human }; } // namespace RoboidControl diff --git a/PerceivedObject.cpp b/TrackedObject.cpp similarity index 61% rename from PerceivedObject.cpp rename to TrackedObject.cpp index 991f05a..4f0bbec 100644 --- a/PerceivedObject.cpp +++ b/TrackedObject.cpp @@ -1,26 +1,15 @@ -#include "PerceivedObject.h" +#include "TrackedObject.h" #include -/*** - * Oject perception - ***/ - -PerceivedObject::PerceivedObject() { +TrackedObject::TrackedObject(Sensor *sensor, Polar position) { 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; } diff --git a/TrackedObject.h b/TrackedObject.h new file mode 100644 index 0000000..811fba5 --- /dev/null +++ b/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; +};