From 4059e260273e81d4e911074ef2db3b34a59dbff3 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 7 Jun 2024 17:47:42 +0200 Subject: [PATCH] Added Sphere perception --- NetworkPerception.cpp | 26 +++++++ NetworkPerception.h | 1 + Perception.cpp | 165 ++++++++++++++++++++---------------------- 3 files changed, 105 insertions(+), 87 deletions(-) diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 846a53f..999ec36 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -107,6 +107,30 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) { roboid->perception->AddTrackedObject(this, position, 0x80, networkId); } +void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) { + unsigned char networkId = data[1]; + + float radius = ReceiveFloat100(data, 3); + + Vector3 worldPosition = ReceiveVector3(data, 7); + Vector3 roboidPosition = roboid->GetPosition(); + Vector3 deltaPosition = worldPosition - roboidPosition; + + Quaternion roboidOrientation = roboid->GetOrientation(); + Vector3 localPosition = + Quaternion::Inverse(roboidOrientation) * deltaPosition; + + Spherical position = Spherical(localPosition); + + roboid->perception->AddTrackedObject(this, position, Quaternion::identity, + 0x81, 0x81, networkId); + roboid->networkSync->SendText("Received Sphere\n\0"); + if ((float)position.horizontalAngle == 0) + roboid->networkSync->SendText("Zero hor angle\0"); + if ((float)position.verticalAngle == 0) + roboid->networkSync->SendText("Zero vertical angle\0"); +} + void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { unsigned char networkId = data[1]; unsigned char objectId = data[2]; @@ -118,6 +142,8 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { if (objectId == 0x80) return ReceivePlane(data, roboid); + else if (objectId = 0x81) + return ReceiveSphere(data, roboid); if ((poseType & NetworkSync::Pose_Position) != 0) { Vector3 worldPosition = ReceiveVector3(data, 4); diff --git a/NetworkPerception.h b/NetworkPerception.h index 77e60e1..769c3c2 100644 --- a/NetworkPerception.h +++ b/NetworkPerception.h @@ -18,6 +18,7 @@ protected: void ReceiveTypedObject(unsigned char *data, Roboid *roboid); void ReceivePlane(unsigned char *data, Roboid *roboid); + void ReceiveSphere(unsigned char *data, Roboid *roboid); Int32 ReceiveInt32(unsigned char *data, int startIndex); float ReceiveFloat100(unsigned char *data, int startIndex); diff --git a/Perception.cpp b/Perception.cpp index a110bcd..66783da 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -189,83 +189,87 @@ bool Perception::ObjectNearby(float direction, float range) { } // #include +// This function is deprecated void Perception::AddTrackedObject(Sensor *sensor, Polar position, unsigned char thingType, unsigned char networkId) { - InterestingThing *obj = new InterestingThing(sensor, position); - obj->type = thingType; + Spherical sPos = Spherical(position); + Quaternion orientation = Quaternion::identity; + AddTrackedObject(sensor, sPos, orientation, thingType, 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; + unsigned char farthestObjIx = 0; + int availableSlotIx = -1; + for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { + if (this->trackedObjects[objIx] == nullptr) { + availableSlotIx = objIx; } - // 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; + // 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); + // 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); + // 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; - } + #endif + // No available slot, delete trackedobject + delete obj; + } + */ } InterestingThing * @@ -273,6 +277,8 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position, Quaternion orientation, unsigned char thingType, unsigned char thingId, unsigned networkId) { InterestingThing *thing = new InterestingThing(sensor, position, orientation); + if (thingId != 0x00) + thing->id = thingId; thing->type = thingType; unsigned char farthestObjIx = 0; @@ -284,11 +290,11 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position, // Do we see the same object? else { if (thing->IsTheSameAs(this->trackedObjects[thingIx])) { -#ifdef RC_DEBUG2 - // Serial.print((int)this->trackedObjects[objIx]->id); - // Serial.println(": update tracked object"); -#endif - + if ((float)thing->position.horizontalAngle != + (float)this->trackedObjects[thingIx]->position.horizontalAngle) + this->roboid->networkSync->SendText("Update\0"); + else + this->roboid->networkSync->SendText("Refresh\0"); this->trackedObjects[thingIx]->Refresh(thing->position, thing->orientation); delete thing; @@ -311,12 +317,6 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position, thing->networkId = networkId; if (thingId == 0x00) thing->id = lastObjectId++; // availableSlotIx + 1; - else - thing->id = thingId; -#ifdef RC_DEBUG2 - printf("%d: new tracked object {%d/%d}\n", availableSlotIx, obj->networkId, - obj->id); -#endif return thing; } // If this object is closer than the farthest object, then replace it @@ -327,17 +327,8 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical position, thing->networkId = networkId; if (thingId == 0x00) thing->id = lastObjectId++; // availableSlotIx + 1; - else - thing->id = thingId; -#ifdef RC_DEBUG2 - printf("%d: replaced tracked object {%d/%d}\n", farthestObjIx, - obj->networkId, obj->id); -#endif return thing; } else { -#ifdef RC_DEBUG2 - printf("%d: delete tracked object {%d/%d}\n", -1, obj->networkId, obj->id); -#endif // No available slot, delete trackedobject delete thing; return nullptr;