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

Renamed perceived object to tracked object

Browse Source
This commit is contained in:
Pascal Serrarens 2024-01-02 11:52:02 +01:00
parent 6cce388a7d
commit 2d94f61c83
6 changed files with 159 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
NetworkSync.h
View File

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

27
PerceivedObject.h
View File

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

72
Perception.cpp
View File

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

69
Perception.h
View File

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

22
PerceivedObject.cpp → TrackedObject.cpp
View File

@ -1,26 +1,15 @@
#include "PerceivedObject.h"
#include "TrackedObject.h"
#include <math.h>
/***
* 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;
}

63
TrackedObject.h Normal file
View File

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