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

Improved network perception

Browse Source
This commit is contained in:
Pascal Serrarens 2024-04-25 15:22:45 +02:00
parent 0ba9d55a4f
commit 7614064788
7 changed files with 96 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit fe12c99159706688925b57ed24b61f651825e117
Subproject commit 4b07328790ffd565f9f3303b625dbde68abaecc7

28
NetworkPerception.cpp
View File

@ -40,38 +40,46 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
unsigned char networkId = data[1];
unsigned char objectId = data[2];
unsigned char poseType = data[3];
printf("Received pose message [%d/%d]\n", networkId, objectId);
// printf("Received pose message [%d/%d]\n", networkId, objectId);
// printf("My network id == %d\n", roboid->networkSync->networkId);
if (networkId == roboid->networkSync->networkId)
networkId = 0x00;
if ((poseType & NetworkSync::Pose_Position) != 0) {
Vector3 worldPosition = ReceiveVector3(data, 4);
if (objectId == 0) {
roboid->SetPosition(worldPosition);
} else {
Vector3 myPosition = roboid->GetPosition(); // Vector3::zero;
// Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition);
Vector3 myPosition = roboid->GetPosition();
Quaternion myOrientation = roboid->GetOrientation();
// Vector3 myDirection = myOrientation * Vector3::forward;
// Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection);
Vector3 localPosition =
Quaternion::Inverse(myOrientation) * (worldPosition - myPosition);
// float distance = Vector3::Distance(myPosition, worldPosition);
// float angle = Vector3::SignedAngle(myDirection, localPosition,
// Vector3::up);
float distance = Vector3::Magnitude(localPosition);
float angle =
Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
Polar position = Polar(angle, distance);
/*
Vector2 worldPosition2D = Vector2(worldPosition);
// int objCount = roboid->perception->TrackedObjectCount();
Vector2 myPosition = roboid->GetPosition2D();
float myOrientation = roboid->GetOrientation2D();
Vector2 localPosition =
Vector2::Rotate(worldPosition2D - myPosition, -myOrientation);
float distance = Vector2::Magnitude(localPosition);
float angle = Vector2::SignedAngle(Vector3::forward, localPosition);
Polar position = Polar(angle, distance);
*/
// printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
// worldPosition.x, worldPosition.z, localPosition.x,
// localPosition.z, distance, angle);
// roboid->perception->AddTrackedObject(this, position);
InterestingThing *thing = roboid->perception->AddTrackedObject(
this, networkId, objectId, position);
if (thing->type == 0xFF) {

2
NetworkSync.cpp
View File

@ -254,7 +254,7 @@ void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) {
}
void NetworkSync::SendInvestigateThing(InterestingThing *thing) {
// printf("Investigate [%d/%d]\n", thing->networkId, thing->id);
printf("Investigate [%d/%d]\n", thing->networkId, thing->id);
unsigned char ix = 0;
buffer[ix++] = InvestigateMsg;
buffer[ix++] = thing->networkId;

95
Perception.cpp
View File

@ -6,7 +6,7 @@
#include <math.h>
// #define RC_DEBUG2
#define RC_DEBUG2
#ifdef RC_DEBUG2
#include <Arduino.h>
#endif
@ -126,12 +126,12 @@ bool Perception::ObjectNearby(float direction, float range) {
#include <WifiSync.h>
void Perception::AddTrackedObject(Sensor *sensor, Polar position,
unsigned char objectType) {
unsigned char thingType) {
InterestingThing *obj = new InterestingThing(sensor, position);
obj->type = objectType;
obj->type = thingType;
unsigned char farthestObjIx = 0;
unsigned char availableSlotIx = 0;
int availableSlotIx = -1;
for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
if (this->trackedObjects[objIx] == nullptr) {
availableSlotIx = objIx;
@ -140,12 +140,13 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
else {
if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
#ifdef RC_DEBUG2
Serial.print((int)this->trackedObjects[objIx]->id);
Serial.println(": update tracked object");
// 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 = objectType;
this->trackedObjects[objIx]->type = thingType;
delete obj;
return;
}
@ -160,14 +161,13 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
// Check if an perception slot is available (we currently see less than the
// max number of objects)
if (availableSlotIx < maxObjectCount) {
if (availableSlotIx >= 0) { //< maxObjectCount) {
// a slot is available
this->trackedObjects[availableSlotIx] = obj;
obj->networkId = 0x00;
obj->id = availableSlotIx + 1;
obj->id = lastObjectId++; // availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": new tracked object");
printf("%d: new tracked object [/%d]\n", availableSlotIx, obj->id);
#endif
roboid->networkSync->NewObject(obj);
((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
@ -178,15 +178,20 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
delete this->trackedObjects[farthestObjIx];
this->trackedObjects[farthestObjIx] = obj;
obj->networkId = 0x00;
obj->id = availableSlotIx + 1;
obj->id = lastObjectId++; // availableSlotIx + 1;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": replaced tracked object");
// Serial.print((int)obj->id);
// Serial.println(": replaced tracked object");
printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id);
roboid->networkSync->NewObject(obj);
((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
#endif
} else {
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": delete tracked object");
// 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;
@ -195,7 +200,9 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
Spherical position,
Quaternion orientation) {
Quaternion orientation,
unsigned char objectId,
unsigned networkId) {
InterestingThing *obj = new InterestingThing(sensor, position, orientation);
unsigned char farthestObjIx = 0;
@ -230,11 +237,14 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
if (availableSlotIx < maxObjectCount) {
// a slot is available
this->trackedObjects[availableSlotIx] = obj;
obj->networkId = 0x00;
obj->id = availableSlotIx + 1;
obj->networkId = networkId;
if (objectId == 0x00)
obj->id = lastObjectId++; // availableSlotIx + 1;
else
obj->id = objectId;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": new tracked object");
printf("%d: new tracked object {%d/%d}\n", availableSlotIx, obj->networkId,
obj->id);
#endif
return obj;
}
@ -243,17 +253,23 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
this->trackedObjects[farthestObjIx]->position.distance) {
delete this->trackedObjects[farthestObjIx];
this->trackedObjects[farthestObjIx] = obj;
obj->networkId = 0x00;
obj->id = availableSlotIx + 1;
obj->networkId = networkId;
if (objectId == 0x00)
obj->id = lastObjectId++; // availableSlotIx + 1;
else
obj->id = objectId;
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": replaced tracked object");
// Serial.print((int)obj->id);
// Serial.println(": replaced tracked object");
printf("%d: replaced tracked object {%d/%d}\n", farthestObjIx,
obj->networkId, obj->id);
#endif
return obj;
} else {
#ifdef RC_DEBUG2
Serial.print((int)obj->id);
Serial.println(": delete tracked object");
// 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;
@ -268,7 +284,8 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
Quaternion orientation) {
InterestingThing *thing = FindTrackedObject(networkId, objectId);
if (thing == nullptr) {
thing = AddTrackedObject(sensor, position, orientation);
thing =
AddTrackedObject(sensor, position, orientation, objectId, networkId);
thing->networkId = networkId;
thing->id = objectId;
thing->type = 0xFF; // unknown
@ -315,7 +332,7 @@ InterestingThing **Perception::GetTrackedObjects() {
return this->trackedObjects;
}
unsigned char Perception::ThingsOfType(unsigned char objectType,
unsigned char Perception::ThingsOfType(unsigned char thingType,
InterestingThing *buffer[],
unsigned char bufferSize) {
unsigned char thingCount = 0;
@ -324,16 +341,32 @@ unsigned char Perception::ThingsOfType(unsigned char objectType,
if (thing == nullptr)
continue;
if (thing->type == objectType) {
printf("[%d/%d]%d ", thing->networkId, thing->id, thing->type);
if (thing->type == thingType) {
buffer[thingCount] = thing;
thingCount++;
if (thingCount >= bufferSize)
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() {
InterestingThing *closestObject = nullptr;
float closestDistance = INFINITY;

5
Perception.h
View File

@ -89,7 +89,8 @@ public:
unsigned char objectType = 0x00);
InterestingThing *
AddTrackedObject(Sensor *sensor, Spherical position,
Quaternion orientation = Quaternion::identity);
Quaternion orientation = Quaternion::identity,
unsigned char objectId = 0x00, unsigned networkId = 0x00);
InterestingThing *
AddTrackedObject(Sensor *sensor, unsigned char networkId,
@ -115,6 +116,7 @@ public:
unsigned char ThingsOfType(unsigned char objectType,
InterestingThing *buffer[],
unsigned char bufferSize);
InterestingThing *ThingOfType(unsigned char objectType);
InterestingThing *GetMostInterestingThing();
@ -153,6 +155,7 @@ public:
unsigned int sensorCount = 0;
float lastUpdateTimeMs = 0;
unsigned char lastObjectId = 1;
static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
// number of object which can

6
Roboid.cpp
View File

@ -67,8 +67,14 @@ void Roboid::Update(float currentTimeMs) {
}
Vector3 Roboid::GetPosition() { return this->worldPosition; }
Vector2 Roboid::GetPosition2D() {
return Vector2(this->worldPosition.x, this->worldPosition.z);
}
Quaternion Roboid::GetOrientation() { return this->worldOrientation; }
float Roboid::GetOrientation2D() {
return Quaternion::GetAngleAround(Vector3::up, this->worldOrientation);
}
void Roboid::SetPosition(Vector3 newWorldPosition) {
Vector3 translation = newWorldPosition - this->worldPosition;

2
Roboid.h
View File

@ -43,12 +43,14 @@ public:
/// tracking system used. This value will be Vector3::zero unless a position
/// is received through network synchronisation
virtual Vector3 GetPosition();
Vector2 GetPosition2D();
/// @brief Retrieve the current orientation of the roboid
/// @return The orientation quaternion in world space
/// @details The origin orientation depends on the position tracking system
/// used. This value will be Quaternion::identity unless an orientation is
/// received though network synchronization
virtual Quaternion GetOrientation();
float GetOrientation2D();
/// @brief Update the current position of the roboid
/// @param worldPosition The position of the roboid in carthesian coordinates