From 5b417ad9cef44aa47627f738e391da5cde72e880 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 31 Jan 2024 09:50:48 +0100 Subject: [PATCH] Renamed TrackedObject to InterestingThing --- NetworkSync.cpp | 15 ++--- NetworkSync.h | 8 +-- Perception.cpp | 143 +++++++++++++++++++++++++++++++++++++--------- Perception.h | 9 ++- Roboid.cpp | 13 ++++- Roboid.h | 6 +- Sensor.h | 8 ++- ServoMotor.cpp | 12 ++++ Thing.cpp | 32 ++++++++++- Thing.h | 12 ++++ TrackedObject.cpp | 31 ++++++++-- TrackedObject.h | 17 ++++-- test/BB2B_Test.cc | 14 ++--- 13 files changed, 252 insertions(+), 68 deletions(-) diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 742feb5..59e4488 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -27,9 +27,9 @@ void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) { } void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer, - TrackedObject **objects) { + InterestingThing **objects) { for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) { - TrackedObject *obj = objects[objIx]; + InterestingThing *obj = objects[objIx]; if (obj == nullptr) continue; // if (obj->sensor->type == Thing::ExternalType) @@ -42,9 +42,10 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer, } void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer, - TrackedObject *object) { - Vector2 worldPosition2 = Vector2::Rotate( - Vector2::forward * object->position.distance, -object->position.angle); + InterestingThing *object) { + Vector2 worldPosition2 = + Vector2::Rotate(Vector2::forward * object->position.magnitude, + -object->position.horizontalAngle); Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y); #ifdef RC_DEBUG @@ -57,7 +58,7 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer, Serial.print(", "); Serial.println(worldPosition3.z); #else - UInt16 bufferSize = 3 + 12; + const UInt16 bufferSize = 3 + 12; UInt8 buffer[bufferSize] = { PoseMsg, object->id, @@ -104,7 +105,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) { #endif } -void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj) { +void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, InterestingThing *obj) { #ifdef RC_DEBUG Serial.print("Send Destroy "); Serial.println((int)obj->id); diff --git a/NetworkSync.h b/NetworkSync.h index 9f207d2..0ac983f 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -16,7 +16,7 @@ public: virtual void NetworkUpdate(Roboid *roboid) = 0; /// @brief Inform that the given object is no longer being tracked /// @param obj - virtual void DestroyObject(TrackedObject *obj) = 0; + virtual void DestroyObject(InterestingThing *obj) = 0; /// @brief The id of a Pose message static const char PoseMsg = 0x10; @@ -38,13 +38,13 @@ public: typedef void (*SendBuffer)(UInt8 *buffer, UInt16 bufferSize); void SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid); - void SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj); + void SendDestroyObject(SendBuffer sendBuffer, InterestingThing *obj); - void PublishTrackedObjects(SendBuffer sendBuffer, TrackedObject **objects); + void PublishTrackedObjects(SendBuffer sendBuffer, InterestingThing **objects); protected: NetworkPerception *networkPerception; - void PublishTrackedObject(SendBuffer sendBuffer, TrackedObject *object); + void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object); void SendVector3(unsigned char *data, int startIndex, Vector3 v); void SendSingle100(unsigned char *data, int startIndex, float value); diff --git a/Perception.cpp b/Perception.cpp index 691b7a7..98442da 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -15,7 +15,7 @@ unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum // be tracked by a human Perception::Perception() { - this->trackedObjects = new TrackedObject *[maxObjectCount]; + this->trackedObjects = new InterestingThing *[maxObjectCount]; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) this->trackedObjects[objIx] = nullptr; } @@ -27,7 +27,7 @@ Perception::Perception(Sensor **sensors, unsigned int sensorCount) for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) this->sensors[sensorIx] = sensors[sensorIx]; - this->trackedObjects = new TrackedObject *[maxObjectCount]; + this->trackedObjects = new InterestingThing *[maxObjectCount]; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) this->trackedObjects[objIx] = nullptr; } @@ -68,19 +68,19 @@ Sensor *Perception::FindSensorOfType(unsigned int sensorType) { return nullptr; } -float Perception::GetDistance(float direction, float range) { +float Perception::GetDistance(float horizontalDirection, float range) { float minDistance = INFINITY; if (range < 0) range = -range; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - TrackedObject *obj = trackedObjects[objIx]; + InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; - if (obj->position.angle > direction - range && - obj->position.angle < direction + range) { + if (obj->position.horizontalAngle > horizontalDirection - range && + obj->position.horizontalAngle < horizontalDirection + range) { - minDistance = fminf(minDistance, obj->position.distance); + minDistance = fminf(minDistance, obj->position.magnitude); } } return minDistance; @@ -93,13 +93,13 @@ float Perception::GetDistance(float horizontalDirection, range = -range; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - TrackedObject *obj = trackedObjects[objIx]; + InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; - if (obj->position.angle > horizontalDirection - range && - obj->position.angle < horizontalDirection + range) { + if (obj->position.horizontalAngle > horizontalDirection - range && + obj->position.horizontalAngle < horizontalDirection + range) { - minDistance = fminf(minDistance, obj->position.distance); + minDistance = fminf(minDistance, obj->position.magnitude); } } @@ -110,13 +110,13 @@ bool Perception::ObjectNearby(float direction, float range) { range = -range; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - TrackedObject *obj = trackedObjects[objIx]; + InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; - if (obj->position.angle > direction - range && - obj->position.angle < direction + range) { - if (obj->position.distance <= nearbyDistance) + if (obj->position.horizontalAngle > direction - range && + obj->position.horizontalAngle < direction + range) { + if (obj->position.magnitude <= nearbyDistance) return true; } } @@ -124,7 +124,7 @@ bool Perception::ObjectNearby(float direction, float range) { } void Perception::AddTrackedObject(Sensor *sensor, Polar position) { - TrackedObject *obj = new TrackedObject(sensor, position); + InterestingThing *obj = new InterestingThing(sensor, position); unsigned char farthestObjIx = 0; unsigned char availableSlotIx = 0; @@ -146,8 +146,8 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) { } // Is this the fartest object we see? else if (this->trackedObjects[farthestObjIx] == nullptr || - (this->trackedObjects[objIx]->position.distance > - this->trackedObjects[farthestObjIx]->position.distance)) { + (this->trackedObjects[objIx]->position.magnitude > + this->trackedObjects[farthestObjIx]->position.magnitude)) { farthestObjIx = objIx; } } @@ -166,8 +166,70 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) { } // If this object is closer than the farthest object, then replace it - else if (obj->position.distance < - this->trackedObjects[farthestObjIx]->position.distance) { + else if (obj->position.magnitude < + this->trackedObjects[farthestObjIx]->position.magnitude) { + delete this->trackedObjects[farthestObjIx]; + this->trackedObjects[farthestObjIx] = obj; + obj->id = availableSlotIx; +#ifdef RC_DEBUG2 + Serial.print((int)obj->id); + Serial.println(": replaced tracked object"); +#endif + } else { +#ifdef RC_DEBUG2 + Serial.print((int)obj->id); + Serial.println(": delete tracked object"); +#endif + // No available slot, delete trackedobject + delete obj; + } +} + +void Perception::AddTrackedObject(Sensor *sensor, Spherical position) { + InterestingThing *obj = new InterestingThing(sensor, position); + + unsigned char farthestObjIx = 0; + unsigned char availableSlotIx = 0; + for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { + if (this->trackedObjects[objIx] == nullptr) { + availableSlotIx = objIx; + } + // Do we see the same object? + else { + if (obj->IsTheSameAs(this->trackedObjects[objIx])) { +#ifdef RC_DEBUG2 + Serial.print((int)this->trackedObjects[objIx]->id); + Serial.println(": update tracked object"); +#endif + + this->trackedObjects[objIx]->Refresh(obj->position); + delete obj; + return; + } + // Is this the fartest object we see? + else if (this->trackedObjects[farthestObjIx] == nullptr || + (this->trackedObjects[objIx]->position.magnitude > + this->trackedObjects[farthestObjIx]->position.magnitude)) { + 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 + this->trackedObjects[availableSlotIx] = obj; + obj->id = availableSlotIx; +#ifdef RC_DEBUG2 + Serial.print((int)obj->id); + Serial.println(": new tracked object"); +#endif + + } + // If this object is closer than the farthest object, then replace it + else if (obj->position.magnitude < + this->trackedObjects[farthestObjIx]->position.magnitude) { delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = obj; obj->id = availableSlotIx; @@ -194,10 +256,29 @@ unsigned char Perception::TrackedObjectCount() { return objectCount; } -TrackedObject **Perception::GetTrackedObjects() { return this->trackedObjects; } +InterestingThing **Perception::GetTrackedObjects() { + return this->trackedObjects; +} + +InterestingThing *Perception::GetMostInterestingThing() { + InterestingThing *closestObject = nullptr; + float closestDistance = INFINITY; + for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { + InterestingThing *obj = this->trackedObjects[objIx]; + if (obj != nullptr) { + if (obj->position.magnitude < closestDistance) { + closestObject = obj; + closestDistance = obj->position.magnitude; + } + } + } + return closestObject; +} + +#include void Perception::Update(float currentTimeMs) { - float deltaTime = currentTimeMs - lastUpdateTimeMs; + float deltaTime = (currentTimeMs - lastUpdateTimeMs) / 1000; if (deltaTime <= 0) return; @@ -225,11 +306,13 @@ void Perception::Update(float currentTimeMs) { Polar position = Polar(sensor->position.angle, nearbyDistance); AddTrackedObject(switchSensor, position); } + } else { + sensor->Update(); } } for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - TrackedObject *obj = trackedObjects[objIx]; + InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; @@ -245,11 +328,14 @@ void Perception::Update(float currentTimeMs) { void Perception::UpdatePose(Polar translation) { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - TrackedObject *obj = trackedObjects[objIx]; + InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; - Polar newPosition = obj->position - translation; + // We only support translations on polars at this moment... + // This needs Spherical operator- to be implemented to work in 3d space + Polar horizontalPosition = obj->position.ProjectOnHorizontalPlane(); + Spherical newPosition = Spherical(horizontalPosition - translation); obj->position = newPosition; } } @@ -264,11 +350,12 @@ void Perception::UpdatePose(Quaternion rotation) { rotationAngle = -rotationAngle; for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - TrackedObject *obj = trackedObjects[objIx]; + InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; - float updatedAngle = Angle::Normalize(obj->position.angle - rotationAngle); - obj->position.angle = updatedAngle; + float updatedAngle = + Angle::Normalize(obj->position.horizontalAngle - rotationAngle); + obj->position.horizontalAngle = updatedAngle; } } \ No newline at end of file diff --git a/Perception.h b/Perception.h index ac3f3f1..119801e 100644 --- a/Perception.h +++ b/Perception.h @@ -4,6 +4,7 @@ #include "TrackedObject.h" #include "VectorAlgebra/Polar.h" #include "VectorAlgebra/Quaternion.h" +#include "VectorAlgebra/Spherical.h" // #include @@ -85,6 +86,8 @@ public: /// @param position The position of the sensor in polar coordinates local to /// the roboid void AddTrackedObject(Sensor *sensor, Polar position); + void AddTrackedObject(Sensor *sensor, Spherical position); + /// @brief Retrieve the number of objects currently being tracked by the /// roboid /// @return The object of objects, which is always lower than maxObjectCount @@ -94,7 +97,9 @@ public: /// @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(); + InterestingThing **GetTrackedObjects(); + + InterestingThing *GetMostInterestingThing(); // mainly used for confidence update @@ -135,7 +140,7 @@ public: static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum // number of object which can // be tracked by a human - TrackedObject **trackedObjects; + InterestingThing **trackedObjects; }; } // namespace RoboidControl diff --git a/Roboid.cpp b/Roboid.cpp index 2ea020d..9a81227 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -19,7 +19,15 @@ Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() { propulsion->roboid = this; } -Roboid::Roboid(ServoMotor **actuation) : actuation(actuation) {} +Roboid::Roboid(Perception *perception, ServoMotor *actuationRoot) : Roboid() { + this->perception = perception; + perception->roboid = this; + this->propulsion = nullptr; + + this->actuationRoot = actuationRoot; +} + +Roboid::Roboid(ServoMotor *actuationRoot) : actuationRoot(actuationRoot) {} void Roboid::Update(float currentTimeMs) { if (perception != nullptr) @@ -28,6 +36,9 @@ void Roboid::Update(float currentTimeMs) { propulsion->Update(currentTimeMs); if (networkSync != nullptr) networkSync->NetworkUpdate(this); + + if (actuationRoot != nullptr) + actuationRoot->Update(currentTimeMs); } Vector3 Roboid::GetPosition() { return this->worldPosition; } diff --git a/Roboid.h b/Roboid.h index 47d5375..57e1f83 100644 --- a/Roboid.h +++ b/Roboid.h @@ -19,9 +19,9 @@ public: /// @param propulsion The Propulsion implementation to use for this Roboid Roboid(Perception *perception, Propulsion *propulsion = nullptr); - Roboid(Perception *perception, ServoMotor **actuation); + Roboid(Perception *perception, ServoMotor *actuationRoot); - Roboid(ServoMotor **actuation); + Roboid(ServoMotor *actuationRoot); /// @brief Update the state of the Roboid /// @param currentTimeMs The time in milliseconds when calling this @@ -32,7 +32,7 @@ public: Perception *perception = nullptr; /// @brief The Propulsion module of this Roboid Propulsion *propulsion = nullptr; - ServoMotor **actuation = nullptr; + ServoMotor *actuationRoot = nullptr; /// @brief The reference to the module to synchronize states across a network NetworkSync *networkSync = nullptr; diff --git a/Sensor.h b/Sensor.h index f88906b..b7cfd82 100644 --- a/Sensor.h +++ b/Sensor.h @@ -7,11 +7,13 @@ namespace RoboidControl { /// @brief A sensor is a thing which can perform measurements in the environment class Sensor : public Thing { - public: +public: /// @brief Default Constructor for a Sensor Sensor(); + + virtual void Update(){}; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/ServoMotor.cpp b/ServoMotor.cpp index 633844c..edf05d5 100644 --- a/ServoMotor.cpp +++ b/ServoMotor.cpp @@ -3,6 +3,7 @@ #include "VectorAlgebra/FloatSingle.h" ServoMotor::ServoMotor() { + this->type = Thing::ServoType; this->controlMode = ControlMode::Position; this->targetAngle = 0; this->hasTargetAngle = false; @@ -41,6 +42,8 @@ void ServoMotor::SetTargetVelocity(float targetVelocity) { float ServoMotor::GetTargetVelocity() { return this->targetVelocity; } +#include + void ServoMotor::Update(float currentTimeMs) { if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) { this->lastUpdateTimeMs = currentTimeMs; @@ -83,4 +86,13 @@ void ServoMotor::Update(float currentTimeMs) { this->lastUpdateTimeMs = currentTimeMs; } + + Serial.println(this->childCount); + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing *child = this->GetChild(childIx); + if (child->type == Thing::ServoType) { + ServoMotor *servo = (ServoMotor *)child; + servo->Update(currentTimeMs); + } + } } \ No newline at end of file diff --git a/Thing.cpp b/Thing.cpp index 0817bba..537a691 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -4,6 +4,9 @@ using namespace Passer::RoboidControl; Thing::Thing() : position(Polar::zero) { this->type = (unsigned int)Type::Undetermined; + this->childCount = 0; + this->parent = nullptr; + this->children = nullptr; } const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch; @@ -13,8 +16,35 @@ const unsigned int Thing::ControlledMotorType = MotorType | (unsigned int)Type::ControlledMotor; const unsigned int Thing::UncontrolledMotorType = MotorType | (unsigned int)Type::UncontrolledMotor; +const unsigned int Thing::ServoType = (unsigned int)Type::Servo; const unsigned int Thing::ExternalType = (unsigned int)Type::ExternalSensor; bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; } -bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; } \ No newline at end of file +bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; } + +void Thing::SetParent(Thing *parent) { this->parent = parent; } + +Thing *Thing::GetParent() { return this->parent; } + +void Thing::AddChild(Thing *child) { + Thing **newChildren = new Thing *[this->childCount + 1]; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + newChildren[childIx] = this->children[childIx]; + + newChildren[this->childCount] = child; + child->SetParent(this); + + if (this->children != nullptr) + delete[] this->children; + + this->children = newChildren; + this->childCount++; +} + +Thing *Thing::GetChild(unsigned char childIx) { + if (childIx < this->childCount) { + return this->children[childIx]; + } else + return nullptr; +} \ No newline at end of file diff --git a/Thing.h b/Thing.h index 3f2b6be..77284f9 100644 --- a/Thing.h +++ b/Thing.h @@ -23,6 +23,7 @@ public: /// @brief The type of an uncontrolled motor static const unsigned int UncontrolledMotorType; /// @brief The type of an object received from the network + static const unsigned int ServoType; static const unsigned int ExternalType; /// @brief Check if the Thing is a Motor @@ -34,6 +35,12 @@ public: Polar position; + void SetParent(Thing *parent); + Thing *GetParent(); + + void AddChild(Thing *child); + Thing *GetChild(unsigned char childIx); + protected: /// @brief Bitmask for Motor type static const unsigned int MotorType = 0x8000; @@ -49,9 +56,14 @@ protected: // Motor, ControlledMotor, UncontrolledMotor, + Servo, // Other ExternalSensor, }; + + Thing *parent = nullptr; + unsigned char childCount = 0; + Thing **children = nullptr; }; } // namespace RoboidControl diff --git a/TrackedObject.cpp b/TrackedObject.cpp index 4f0bbec..27aecbd 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -2,24 +2,38 @@ #include -TrackedObject::TrackedObject(Sensor *sensor, Polar position) { +InterestingThing::InterestingThing(Sensor *sensor, Polar position) { + this->id = 0; + this->confidence = maxConfidence; + this->sensor = sensor; + this->position = Spherical(position); +} + +InterestingThing::InterestingThing(Sensor *sensor, Spherical position) { this->id = 0; this->confidence = maxConfidence; this->sensor = sensor; this->position = position; } -bool TrackedObject::IsTheSameAs(TrackedObject *otherObj) { +bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) { if (id != 0 && id == otherObj->id) return true; - if (fabsf(position.distance - otherObj->position.distance) > equalityDistance) + if (fabsf(position.magnitude - otherObj->position.magnitude) > + equalityDistance) return false; - if (fabsf(position.angle - otherObj->position.angle) > equalityAngle) + if (fabsf(position.horizontalAngle - otherObj->position.horizontalAngle) > + equalityAngle) + return false; + if (fabsf(position.verticalAngle - otherObj->position.verticalAngle) > + equalityAngle) return false; return true; } -bool TrackedObject::DegradeConfidence(float deltaTime) { +#include + +bool InterestingThing::DegradeConfidence(float deltaTime) { unsigned char confidenceDrop = (unsigned char)((float)confidenceDropSpeed * deltaTime); // Make sure the confidence always drops @@ -36,7 +50,12 @@ bool TrackedObject::DegradeConfidence(float deltaTime) { } } -void TrackedObject::Refresh(Polar position) { +void InterestingThing::Refresh(Polar position) { + this->position = Spherical(position); + this->confidence = maxConfidence; +} + +void InterestingThing::Refresh(Spherical position) { this->position = position; this->confidence = maxConfidence; } diff --git a/TrackedObject.h b/TrackedObject.h index fe36a44..6be51f5 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -2,23 +2,26 @@ #include "Sensor.h" #include "VectorAlgebra/Polar.h" +#include "VectorAlgebra/Spherical.h" namespace Passer { namespace RoboidControl { /// @brief An object tracked by the roboid -class TrackedObject { +class InterestingThing { 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); + InterestingThing(Sensor *sensor, Polar position); + InterestingThing(Sensor *sensor, Spherical 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); + void Refresh(Spherical position); /// @brief Decrease the confidence based on the elapsed time /// @param deltaTime The time since the last DegradeConfidence call @@ -33,7 +36,7 @@ public: /// @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); + bool IsTheSameAs(InterestingThing *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 @@ -56,14 +59,16 @@ public: char id; /// @brief The current position of the object - Polar position = Polar::zero; + // Polar position = Polar::zero; + Spherical position = Spherical::zero; /// @brief The sensor which provided that lastet pose this object Sensor *sensor = nullptr; + unsigned char confidence; + protected: static constexpr unsigned char maxConfidence = 255; - static constexpr unsigned char confidenceDropSpeed = 1; // 2; - unsigned char confidence; + static constexpr unsigned char confidenceDropSpeed = 150; // 2; }; } // namespace RoboidControl diff --git a/test/BB2B_Test.cc b/test/BB2B_Test.cc index 53c338a..ec59bc6 100644 --- a/test/BB2B_Test.cc +++ b/test/BB2B_Test.cc @@ -103,8 +103,8 @@ TEST(BB2B, NoObstacle) { trackedObjectCount = roboid->perception->TrackedObjectCount(); EXPECT_EQ(trackedObjectCount, 0); - TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects(); - TrackedObject *trackedObject = nullptr; + InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects(); + InterestingThing *trackedObject = nullptr; for (int i = 0; i < roboid->perception->maxObjectCount; i++) { if (trackedObjects[i] != nullptr) trackedObject = trackedObjects[0]; @@ -168,8 +168,8 @@ TEST(BB2B, ObstacleLeft) { EXPECT_EQ(trackedObjectCount, 1); // Find the single tracked object - TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects(); - TrackedObject *trackedObject = nullptr; + InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects(); + InterestingThing *trackedObject = nullptr; for (int i = 0; i < roboid->perception->maxObjectCount; i++) { if (trackedObjects[i] != nullptr) trackedObject = trackedObjects[i]; @@ -269,8 +269,8 @@ TEST(BB2B, ObstacleRight) { EXPECT_EQ(trackedObjectCount, 1); // Find the single tracked object - TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects(); - TrackedObject *trackedObject = nullptr; + InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects(); + InterestingThing *trackedObject = nullptr; for (int i = 0; i < roboid->perception->maxObjectCount; i++) { if (trackedObjects[i] != nullptr) trackedObject = trackedObjects[i]; @@ -368,7 +368,7 @@ TEST(BB2B, ObstacleBoth) { EXPECT_EQ(trackedObjectCount, 2); // Find the single tracked object - TrackedObject **trackedObjects = roboid->perception->GetTrackedObjects(); + InterestingThing **trackedObjects = roboid->perception->GetTrackedObjects(); for (int i = 0; i < roboid->perception->maxObjectCount; i++) { if (trackedObjects[i] != nullptr) { EXPECT_FLOAT_EQ(trackedObjects[i]->position.distance, 0.1F);