From 76140647886ef58c2d132e5faacf3a126937ee4b Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 25 Apr 2024 15:22:45 +0200 Subject: [PATCH] Improved network perception --- LinearAlgebra | 2 +- NetworkPerception.cpp | 28 ++++++++----- NetworkSync.cpp | 2 +- Perception.cpp | 95 +++++++++++++++++++++++++++++-------------- Perception.h | 5 ++- Roboid.cpp | 6 +++ Roboid.h | 2 + 7 files changed, 96 insertions(+), 44 deletions(-) diff --git a/LinearAlgebra b/LinearAlgebra index fe12c99..4b07328 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit fe12c99159706688925b57ed24b61f651825e117 +Subproject commit 4b07328790ffd565f9f3303b625dbde68abaecc7 diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 9d11ff1..9556821 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -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) { diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 7a36af5..c25011c 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -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; diff --git a/Perception.cpp b/Perception.cpp index 769c18c..74d852c 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -6,7 +6,7 @@ #include -// #define RC_DEBUG2 +#define RC_DEBUG2 #ifdef RC_DEBUG2 #include #endif @@ -126,12 +126,12 @@ bool Perception::ObjectNearby(float direction, float range) { #include 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; diff --git a/Perception.h b/Perception.h index c805315..70b160a 100644 --- a/Perception.h +++ b/Perception.h @@ -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 diff --git a/Roboid.cpp b/Roboid.cpp index 897ac2a..5bbd127 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -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; diff --git a/Roboid.h b/Roboid.h index 9a01655..f04eeeb 100644 --- a/Roboid.h +++ b/Roboid.h @@ -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