585 lines
20 KiB
C++
585 lines
20 KiB
C++
#include "Perception.h"
|
|
#include "DistanceSensor.h"
|
|
#include "LinearAlgebra/Angle.h"
|
|
#include "NetworkSync.h"
|
|
#include "Switch.h"
|
|
|
|
#include <math.h>
|
|
|
|
// #define RC_DEBUG
|
|
#ifdef RC_DEBUG
|
|
#include <Arduino.h>
|
|
#endif
|
|
|
|
unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
|
|
// number of object which can
|
|
// be tracked by a human
|
|
|
|
Perception::Perception() {
|
|
this->trackedObjects = new InterestingThing*[maxObjectCount];
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
|
|
this->trackedObjects[objIx] = nullptr;
|
|
}
|
|
|
|
Perception::Perception(Sensor** sensors, unsigned int sensorCount)
|
|
: Perception() {
|
|
this->sensorCount = sensorCount;
|
|
this->sensors = new Sensor*[this->sensorCount];
|
|
for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
|
|
this->sensors[sensorIx] = sensors[sensorIx];
|
|
|
|
this->trackedObjects = new InterestingThing*[maxObjectCount];
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
|
|
this->trackedObjects[objIx] = nullptr;
|
|
}
|
|
|
|
unsigned int Perception::GetSensorCount() {
|
|
return this->sensorCount;
|
|
}
|
|
|
|
Sensor* Perception::GetSensor(unsigned int sensorId) {
|
|
if (sensorId >= this->sensorCount)
|
|
return nullptr;
|
|
|
|
Sensor* sensor = this->sensors[sensorId];
|
|
return sensor;
|
|
}
|
|
|
|
unsigned int Perception::AddSensor(Sensor* newSensor) {
|
|
unsigned int newSensorCount = this->sensorCount + 1;
|
|
Sensor** newSensors = new Sensor*[newSensorCount];
|
|
for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
|
|
newSensors[sensorIx] = sensors[sensorIx];
|
|
|
|
unsigned int sensorId = this->sensorCount;
|
|
newSensors[sensorId] = newSensor;
|
|
|
|
Sensor** oldSensors = this->sensors;
|
|
this->sensors = newSensors;
|
|
this->sensorCount = newSensorCount;
|
|
delete[] oldSensors;
|
|
return sensorId;
|
|
}
|
|
|
|
Sensor* Perception::FindSensorOfType(unsigned int sensorType) {
|
|
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
|
|
Sensor* sensor = this->sensors[sensorIx];
|
|
if (sensor->type == sensorType)
|
|
return sensor;
|
|
}
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
float GetPlaneDistance(InterestingThing* plane,
|
|
float horizontalAngle,
|
|
float range) {
|
|
float distance = plane->position.distance;
|
|
float deltaAngle =
|
|
Angle::Normalize((float)plane->position.horizontal.ToFloat() -
|
|
horizontalAngle)
|
|
.ToFloat();
|
|
if (fabsf(deltaAngle) < fabsf(range)) {
|
|
// distance = distance
|
|
// printf(" plane distance = %f (%f-%f)+%f=%f", distance,
|
|
// (float)plane->position.horizontalAngle, horizontalAngle, range,
|
|
// deltaAngle);
|
|
|
|
} else if (deltaAngle < -range) {
|
|
float angle = deltaAngle + range;
|
|
// printf(" plane distance < %f (%f-%f)+%f=%f %f", distance,
|
|
// (float)plane->position.horizontalAngle, horizontalAngle, range,
|
|
// angle, cosf(angle * Angle::Deg2Rad));
|
|
if (angle > -90)
|
|
distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
|
|
else
|
|
distance = 9999; // infinity?
|
|
|
|
} else if (deltaAngle > range) {
|
|
float angle = deltaAngle - range;
|
|
// printf(" plane distance > %f (%f-%f)-%f=%f %f", distance,
|
|
// (float)plane->position.horizontalAngle, horizontalAngle, range,
|
|
// angle, cosf(angle * Angle::Deg2Rad));
|
|
if (angle < 90)
|
|
distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
|
|
else
|
|
distance = 9999; // infinity?
|
|
}
|
|
// printf(" distance = %f\n", distance);
|
|
return distance;
|
|
}
|
|
|
|
float Perception::GetDistance(float horizontalDirection, float range) {
|
|
float minDistance = INFINITY;
|
|
if (range < 0)
|
|
range = -range;
|
|
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
InterestingThing* obj = trackedObjects[objIx];
|
|
if (obj == nullptr)
|
|
continue;
|
|
|
|
if (obj->type == 0x080) { // plane
|
|
float planeDistance = GetPlaneDistance(obj, horizontalDirection, range);
|
|
minDistance = fminf(minDistance, planeDistance);
|
|
} else if (obj->position.horizontal.ToFloat() >
|
|
horizontalDirection - range &&
|
|
obj->position.horizontal.ToFloat() <
|
|
horizontalDirection + range) {
|
|
minDistance = fminf(minDistance, obj->position.distance);
|
|
}
|
|
}
|
|
return minDistance;
|
|
}
|
|
|
|
float Perception::GetDistanceOfType(unsigned char thingType,
|
|
float horizontalAngle,
|
|
float range) {
|
|
float minDistance = INFINITY;
|
|
if (range < 0)
|
|
range = -range;
|
|
|
|
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
|
|
InterestingThing* thing = trackedObjects[thingIx];
|
|
if (thing == nullptr)
|
|
continue;
|
|
if (thing->type != thingType)
|
|
continue;
|
|
|
|
if (thing->type == 0x080) { // plane
|
|
float planeDistance = GetPlaneDistance(thing, horizontalAngle, range);
|
|
minDistance = fminf(minDistance, planeDistance);
|
|
} else if (thing->position.horizontal.ToFloat() > horizontalAngle - range &&
|
|
thing->position.horizontal.ToFloat() < horizontalAngle + range) {
|
|
minDistance = fminf(minDistance, thing->position.distance);
|
|
}
|
|
}
|
|
return minDistance;
|
|
}
|
|
|
|
float Perception::GetDistance(float horizontalDirection,
|
|
float verticalDirection,
|
|
float range) {
|
|
float minDistance = INFINITY;
|
|
if (range < 0)
|
|
range = -range;
|
|
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
InterestingThing* obj = trackedObjects[objIx];
|
|
if (obj == nullptr)
|
|
continue;
|
|
if (obj->position.horizontal.ToFloat() > horizontalDirection - range &&
|
|
obj->position.horizontal.ToFloat() < horizontalDirection + range) {
|
|
minDistance = fminf(minDistance, obj->position.distance);
|
|
}
|
|
}
|
|
|
|
return minDistance;
|
|
}
|
|
bool Perception::ObjectNearby(float direction, float range) {
|
|
if (range < 0)
|
|
range = -range;
|
|
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
InterestingThing* obj = trackedObjects[objIx];
|
|
if (obj == nullptr)
|
|
continue;
|
|
|
|
if (obj->position.horizontal.ToFloat() > direction - range &&
|
|
obj->position.horizontal.ToFloat() < direction + range) {
|
|
if (obj->position.distance <= nearbyDistance)
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// #include <WifiSync.h>
|
|
// This function is deprecated
|
|
/*
|
|
void Perception::AddTrackedObject(Sensor* sensor,
|
|
Spherical16 position,
|
|
unsigned char thingType,
|
|
unsigned char networkId) {
|
|
// Spherical16 sPos =
|
|
// Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
|
|
Quaternion orientation = Quaternion::identity;
|
|
AddTrackedObject(sensor, sPos, orientation, 0x00, thingType, networkId);
|
|
/*
|
|
InterestingThing *obj = new InterestingThing(sensor, position);
|
|
obj->type = thingType;
|
|
|
|
unsigned char farthestObjIx = 0;
|
|
int availableSlotIx = -1;
|
|
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");
|
|
printf("%d: update tracked object [/%d]\n", objIx, obj->id);
|
|
#endif
|
|
|
|
this->trackedObjects[objIx]->Refresh(obj->position);
|
|
this->trackedObjects[objIx]->type = thingType;
|
|
delete obj;
|
|
return;
|
|
}
|
|
// Is this the fartest object we see?
|
|
else if (this->trackedObjects[farthestObjIx] == nullptr ||
|
|
(this->trackedObjects[objIx]->position.distance >
|
|
this->trackedObjects[farthestObjIx]->position.distance)) {
|
|
farthestObjIx = objIx;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Check if an perception slot is available (we currently see less than the
|
|
// max number of objects)
|
|
if (availableSlotIx >= 0) { //< maxObjectCount) {
|
|
// a slot is available
|
|
this->trackedObjects[availableSlotIx] = obj;
|
|
obj->networkId = networkId;
|
|
obj->id = lastObjectId++; // availableSlotIx + 1;
|
|
#ifdef RC_DEBUG2
|
|
printf("%d: new tracked object [%d/%d]\n", availableSlotIx,
|
|
obj->networkId, obj->id); #endif if (roboid->networkSync != nullptr) {
|
|
roboid->networkSync->NewObject(obj);
|
|
// ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
|
|
}
|
|
}
|
|
// If this object is closer than the farthest object, then replace it
|
|
else if (obj->position.distance <
|
|
this->trackedObjects[farthestObjIx]->position.distance) {
|
|
delete this->trackedObjects[farthestObjIx];
|
|
this->trackedObjects[farthestObjIx] = obj;
|
|
obj->networkId = networkId;
|
|
obj->id = lastObjectId++; // availableSlotIx + 1;
|
|
#ifdef RC_DEBUG2
|
|
// Serial.print((int)obj->id);
|
|
// Serial.println(": replaced tracked object");
|
|
printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id);
|
|
#endif
|
|
if (roboid->networkSync != nullptr) {
|
|
roboid->networkSync->NewObject(obj);
|
|
// ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
|
|
}
|
|
} else {
|
|
#ifdef RC_DEBUG2
|
|
// Serial.print((int)obj->id);
|
|
// Serial.println(": delete tracked object");
|
|
printf("%d: delete tracked object [/%d]\n", -1, obj->id);
|
|
|
|
#endif
|
|
// No available slot, delete trackedobject
|
|
delete obj;
|
|
}
|
|
}
|
|
*/
|
|
|
|
InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
|
|
Spherical16 position,
|
|
Quaternion orientation,
|
|
unsigned char thingType,
|
|
unsigned char thingId,
|
|
unsigned char networkId) {
|
|
InterestingThing* thing = new InterestingThing(sensor, position, orientation);
|
|
if (thingId != 0x00)
|
|
thing->id = thingId;
|
|
thing->type = thingType;
|
|
|
|
unsigned char farthestObjIx = 0;
|
|
unsigned char availableSlotIx = 0;
|
|
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
|
|
if (this->trackedObjects[thingIx] == nullptr) {
|
|
availableSlotIx = thingIx;
|
|
}
|
|
// Do we see the same object?
|
|
else {
|
|
if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
|
|
this->trackedObjects[thingIx]->Refresh(
|
|
thing->position, thing->orientation.ToQuaternion());
|
|
delete thing;
|
|
|
|
return this->trackedObjects[thingIx];
|
|
}
|
|
// Is this the fartest object we see?
|
|
else if (this->trackedObjects[farthestObjIx] == nullptr ||
|
|
(this->trackedObjects[thingIx]->position.distance >
|
|
this->trackedObjects[farthestObjIx]->position.distance)) {
|
|
farthestObjIx = thingIx;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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] = thing;
|
|
thing->networkId = networkId;
|
|
if (thingId == 0x00)
|
|
thing->id = lastObjectId++; // availableSlotIx + 1;
|
|
return thing;
|
|
}
|
|
// If this object is closer than the farthest object, then replace it
|
|
else if (thing->position.distance <
|
|
this->trackedObjects[farthestObjIx]->position.distance) {
|
|
delete this->trackedObjects[farthestObjIx];
|
|
this->trackedObjects[farthestObjIx] = thing;
|
|
thing->networkId = networkId;
|
|
if (thingId == 0x00)
|
|
thing->id = lastObjectId++; // availableSlotIx + 1;
|
|
return thing;
|
|
} else {
|
|
// No available slot, delete trackedobject
|
|
delete thing;
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
|
|
unsigned char networkId,
|
|
unsigned char thingId,
|
|
Spherical16 position,
|
|
Quaternion orientation) {
|
|
InterestingThing* thing = FindTrackedObject(networkId, thingId);
|
|
if (thing == nullptr) {
|
|
thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId,
|
|
networkId);
|
|
// Don't we set this above already?
|
|
thing->networkId = networkId;
|
|
thing->id = thingId;
|
|
thing->type = 0xFF; // unknown
|
|
}
|
|
return thing;
|
|
}
|
|
|
|
bool Perception::IsInteresting(float distance) {
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
InterestingThing* thing = this->trackedObjects[objIx];
|
|
if (thing == nullptr)
|
|
return true;
|
|
if (thing->position.distance > distance)
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
InterestingThing* Perception::FindTrackedObject(char objectId) {
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
InterestingThing* thing = this->trackedObjects[objIx];
|
|
if (thing == nullptr)
|
|
continue;
|
|
if (thing->id == objectId) {
|
|
return thing;
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
InterestingThing* Perception::FindTrackedObject(unsigned char networkId,
|
|
unsigned char objectId) {
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
InterestingThing* thing = this->trackedObjects[objIx];
|
|
if (thing == nullptr)
|
|
continue;
|
|
|
|
if (thing->networkId == networkId && thing->id == objectId) {
|
|
return thing;
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
unsigned char Perception::TrackedObjectCount() {
|
|
unsigned char objectCount = 0;
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
if (this->trackedObjects[objIx] != nullptr)
|
|
objectCount++;
|
|
}
|
|
return objectCount;
|
|
}
|
|
|
|
InterestingThing** Perception::GetTrackedObjects() {
|
|
return this->trackedObjects;
|
|
}
|
|
|
|
unsigned char Perception::ThingsOfType(unsigned char thingType,
|
|
InterestingThing* buffer[],
|
|
unsigned char bufferSize) {
|
|
unsigned char thingCount = 0;
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
InterestingThing* thing = this->trackedObjects[objIx];
|
|
if (thing == nullptr)
|
|
continue;
|
|
|
|
// printf("[%d/%d]%d ", thing->networkId, thing->id, thing->type);
|
|
if (thing->type == thingType) {
|
|
buffer[thingCount] = thing;
|
|
thingCount++;
|
|
if (thingCount >= bufferSize) {
|
|
// printf("\n");
|
|
return bufferSize;
|
|
}
|
|
}
|
|
}
|
|
// printf("\n");
|
|
return thingCount;
|
|
}
|
|
|
|
InterestingThing* Perception::ThingOfType(unsigned char thingType) {
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
InterestingThing* thing = this->trackedObjects[objIx];
|
|
if (thing == nullptr)
|
|
continue;
|
|
|
|
if (thing->type == thingType)
|
|
return thing;
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
InterestingThing* Perception::GetMostInterestingThing() {
|
|
if (this->trackedObjects == nullptr)
|
|
return nullptr;
|
|
|
|
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.distance < closestDistance) {
|
|
closestObject = obj;
|
|
closestDistance = obj->position.distance;
|
|
}
|
|
}
|
|
}
|
|
return closestObject;
|
|
}
|
|
|
|
void Perception::Update(unsigned long currentTimeMs) {
|
|
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000.0f;
|
|
if (deltaTime <= 0)
|
|
return;
|
|
|
|
lastUpdateTimeMs = currentTimeMs;
|
|
|
|
// Update sensing
|
|
for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
|
|
Sensor* sensor = sensors[sensorIx];
|
|
if (sensor == nullptr)
|
|
continue;
|
|
|
|
if (sensor->type == Thing::DistanceSensorType) {
|
|
DistanceSensor* distanceSensor = (DistanceSensor*)sensor;
|
|
|
|
float distance = distanceSensor->GetDistance();
|
|
if (distance >= 0) {
|
|
// Angle16 angle = sensor->position.horizontal;
|
|
// Polar position = Polar(angle, distance);
|
|
// Polar position = Polar(distance, angle.ToFloat());
|
|
// AddTrackedObject(distanceSensor, position);
|
|
Spherical16 position = Spherical16(
|
|
distance, sensor->position.horizontal, sensor->position.vertical);
|
|
AddTrackedObject(distanceSensor, position);
|
|
}
|
|
|
|
} else if (sensor->type == Thing::SwitchType) {
|
|
Switch* switchSensor = (Switch*)sensor;
|
|
if (switchSensor->IsOn()) {
|
|
// Polar position = Polar(sensor->position.angle, nearbyDistance);
|
|
Polar position =
|
|
Polar(nearbyDistance, sensor->position.horizontal.ToFloat());
|
|
// AddTrackedObject(switchSensor, position);
|
|
}
|
|
} else {
|
|
sensor->Update(currentTimeMs);
|
|
}
|
|
}
|
|
|
|
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
|
|
InterestingThing* thing = trackedObjects[objIx];
|
|
if (thing == nullptr)
|
|
continue;
|
|
|
|
if (thing->DegradeConfidence(deltaTime) == false) {
|
|
// delete obj
|
|
if (roboid != nullptr && roboid->networkSync != nullptr) {
|
|
roboid->networkSync->SendDestroyThing(thing);
|
|
}
|
|
this->trackedObjects[objIx] = nullptr;
|
|
delete thing;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Perception::UpdatePose(Polar translation) {
|
|
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
|
|
InterestingThing* thing = trackedObjects[thingIx];
|
|
if (thing == nullptr)
|
|
continue;
|
|
|
|
// We only support translations in the horizontal plane at this moment...
|
|
// This needs Spherical operator- to be implemented to work in 3d space
|
|
if (thing->type == 0x80) { // plane
|
|
// printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
|
|
// (float)thing->position.horizontalAngle,
|
|
// (float)thing->position.verticalAngle);
|
|
// Update the closest point to the plane
|
|
float angle = (float)thing->position.horizontal.ToFloat() +
|
|
translation.angle.ToFloat();
|
|
angle = fabsf(angle);
|
|
|
|
float deltaDistance =
|
|
translation.distance * cosf(angle * Passer::LinearAlgebra::Deg2Rad);
|
|
// printf(" | translate %f %f %f | ", (float)translation.distance,
|
|
// (float)angle, deltaDistance);
|
|
thing->position.distance -= deltaDistance;
|
|
// printf("-> %f (%f %f)\n", thing->position.distance,
|
|
// (float)thing->position.horizontalAngle,
|
|
// (float)thing->position.verticalAngle);
|
|
} else {
|
|
// Polar horizontalPosition = Polar(thing->position);
|
|
// // obj->position.ProjectOnHorizontalPlane();
|
|
// Spherical16 newPosition = Spherical16(horizontalPosition -
|
|
// translation);
|
|
Spherical16 translationS = Spherical16(
|
|
translation.distance, Angle16(translation.angle.ToFloat()), 0);
|
|
Spherical16 newPosition = thing->position + translationS;
|
|
thing->position = newPosition;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Perception::UpdatePose(Quaternion rotation) {
|
|
// only rotation around vertical axis is supported for now
|
|
float rotationAngle;
|
|
Vector3 rotationAxis;
|
|
rotation.ToAngleAxis(&rotationAngle, &rotationAxis);
|
|
// Make sure rotation axis is positive
|
|
if (rotationAxis.Up() < 0)
|
|
rotationAngle = -rotationAngle;
|
|
|
|
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
|
|
InterestingThing* thing = trackedObjects[thingIx];
|
|
if (thing == nullptr)
|
|
continue;
|
|
|
|
// printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
|
|
// (float)thing->position.horizontalAngle,
|
|
// (float)thing->position.verticalAngle);
|
|
// printf("| rotate %f | ", rotationAngle);
|
|
|
|
thing->position.horizontal = Angle16(Angle16::Normalize(
|
|
thing->position.horizontal.ToFloat() - rotationAngle));
|
|
|
|
// printf("-> %f (%f %f) \n", thing->position.distance,
|
|
// (float)thing->position.horizontalAngle,
|
|
// (float)thing->position.verticalAngle);
|
|
}
|
|
} |