RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Renamed TrackedObject to InterestingThing

Browse Source
This commit is contained in:
Pascal Serrarens 2024-01-31 09:50:48 +01:00
parent af3d2531b2
commit 5b417ad9ce
13 changed files with 252 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
NetworkSync.cpp
View File

@ -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);

8
NetworkSync.h
View File

@ -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);

143
Perception.cpp
View File

@ -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 <Arduino.h>
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;
}
}

9
Perception.h
View File

@ -4,6 +4,7 @@
#include "TrackedObject.h"
#include "VectorAlgebra/Polar.h"
#include "VectorAlgebra/Quaternion.h"
#include "VectorAlgebra/Spherical.h"
// #include <vector.h>
@ -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

13
Roboid.cpp
View File

@ -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; }

6
Roboid.h
View File

@ -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;

8
Sensor.h
View File

@ -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;

12
ServoMotor.cpp
View File

@ -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 <Arduino.h>
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);
}
}
}

32
Thing.cpp
View File

@ -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; }
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;
}

12
Thing.h
View File

@ -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

31
TrackedObject.cpp
View File

@ -2,24 +2,38 @@
#include <math.h>
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 <Arduino.h>
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;
}

17
TrackedObject.h
View File

@ -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

14
test/BB2B_Test.cc
View File

@ -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);