From 52a75d6b7b818489b9008930329d090eee270c2a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 17 May 2024 11:58:06 +0200 Subject: [PATCH] Refactoring --- NetworkSync.cpp | 50 +++++++++++++++++++++++++++++++++------ NetworkSync.h | 11 +++++---- Perception.cpp | 63 +++++++++++++++++++++++-------------------------- Perception.h | 1 + 4 files changed, 81 insertions(+), 44 deletions(-) diff --git a/NetworkSync.cpp b/NetworkSync.cpp index bcdaf67..ecdf3c3 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -1,6 +1,6 @@ #include "NetworkSync.h" -#define RC_DEBUG 1 +// #define RC_DEBUG 1 #ifdef RC_DEBUG #include @@ -9,7 +9,7 @@ #include "LinearAlgebra/Angle8.h" #include "LinearAlgebra/Spherical.h" -NetworkSync::NetworkSync() { buffer = new unsigned char[32]; } +NetworkSync::NetworkSync(Roboid *roboid) { this->roboid = roboid; } void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) { networkPerception->ProcessPacket(roboid, buffer, bytecount); @@ -24,7 +24,37 @@ void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) { void NetworkSync::ReceiveNetworkId() { this->networkId = buffer[1]; +#ifdef RC_DEBUG printf("Received network Id %d\n", this->networkId); +#endif +} + +void NetworkSync::NewObject(InterestingThing *thing) { + if (thing == nullptr || thing->networkId != 0x00) + return; + + unsigned char ix = 0; + buffer[ix++] = CreateMsg; + buffer[ix++] = thing->id; + buffer[ix++] = thing->type; + SendBuffer(ix); + +#ifdef RC_DEBUG + printf("Sent CreateMsg %d %d\n", buffer[1], buffer[2]); +#endif + + // PublishTrackedObject(roboid, obj); +} + +void NetworkSync::DestroyObject(InterestingThing *thing) { + unsigned char ix = 0; + buffer[ix++] = DestroyMsg; + buffer[ix++] = thing->id; + SendBuffer(ix); + +#if RC_DEBUG + printf("Sent DestroyMsg %d %d\n", buffer[1], buffer[2]); +#endif } void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, @@ -145,6 +175,10 @@ void NetworkSync::PublishClient() { buffer[ix++] = ClientMsg; buffer[ix++] = 0; // No network ID SendBuffer(ix); + +#ifdef RC_DEBUG + printf("Sent new Client\n"); +#endif } void NetworkSync::PublishTrackedObjects(Buffer sendBuffer, @@ -187,8 +221,8 @@ void NetworkSync::PublishTrackedObject(Buffer sendBuffer, (UInt8)object->id, Pose_Position, }; - unsigned int ix = 3; - SendVector3(buffer, ix, worldPosition3); + unsigned char ix = 3; + SendVector3(buffer, &ix, worldPosition3); sendBuffer(buffer, bufferSize); #endif } @@ -237,9 +271,9 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { 0, // objectId; Pose_LinearVelocity | Pose_AngularVelocity, }; - unsigned int ix = 3; - SendVector3(buffer, ix, worldVelocity3); - SendVector3(buffer, ix, worldAngularVelocity); + unsigned char ix = 3; + SendVector3(buffer, &ix, worldVelocity3); + SendVector3(buffer, &ix, worldAngularVelocity); sendBuffer(buffer, bufferSize); #endif @@ -256,7 +290,9 @@ void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) { } void NetworkSync::SendInvestigateThing(InterestingThing *thing) { +#ifdef RC_DEBUG printf("Investigate [%d/%d]\n", thing->networkId, thing->id); +#endif unsigned char ix = 0; buffer[ix++] = InvestigateMsg; buffer[ix++] = thing->networkId; diff --git a/NetworkSync.h b/NetworkSync.h index eb8491f..4d3f350 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -11,7 +11,7 @@ namespace RoboidControl { /// @brief Interface for synchronizaing state between clients across a network class NetworkSync { public: - NetworkSync(); + NetworkSync(Roboid *roboid); unsigned char networkId; @@ -20,8 +20,9 @@ public: virtual void NetworkUpdate(Roboid *roboid) = 0; /// @brief Inform that the given object is no longer being tracked /// @param obj - virtual void DestroyObject(InterestingThing *obj) = 0; - virtual void NewObject(InterestingThing *obj) {}; + virtual void DestroyObject(InterestingThing *obj); + virtual void NewObject(InterestingThing *obj); + /// @brief The id of a Pose message static const unsigned char PoseMsg = 0x10; static const unsigned char PoseTypeMsg = 0x11; @@ -67,7 +68,9 @@ public: virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {}; protected: + Roboid *roboid; NetworkPerception *networkPerception; + void PublishTrackedObject(Buffer sendBuffer, InterestingThing *object); void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId, InterestingThing *object); @@ -92,7 +95,7 @@ protected: void SendQuat32(unsigned char *data, unsigned char *startIndex, const Quaternion q); - unsigned char *buffer; // =[32]; + unsigned char buffer[32]; virtual void SendBuffer(unsigned char bufferSize); void PublishClient(); diff --git a/Perception.cpp b/Perception.cpp index 0f4780b..d813b10 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -268,36 +268,37 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position, } } -InterestingThing *Perception::AddTrackedObject(Sensor *sensor, - Spherical position, - Quaternion orientation, - unsigned char objectId, - unsigned networkId) { - InterestingThing *obj = new InterestingThing(sensor, position, orientation); +InterestingThing * +Perception::AddTrackedObject(Sensor *sensor, Spherical position, + Quaternion orientation, unsigned char thingType, + unsigned char thingId, unsigned networkId) { + InterestingThing *thing = new InterestingThing(sensor, position, orientation); + thing->type = thingType; unsigned char farthestObjIx = 0; unsigned char availableSlotIx = 0; - for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { - if (this->trackedObjects[objIx] == nullptr) { - availableSlotIx = objIx; + for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { + if (this->trackedObjects[thingIx] == nullptr) { + availableSlotIx = thingIx; } // Do we see the same object? else { - if (obj->IsTheSameAs(this->trackedObjects[objIx])) { + if (thing->IsTheSameAs(this->trackedObjects[thingIx])) { #ifdef RC_DEBUG2 // Serial.print((int)this->trackedObjects[objIx]->id); // Serial.println(": update tracked object"); #endif - this->trackedObjects[objIx]->Refresh(obj->position, obj->orientation); - delete obj; - return this->trackedObjects[objIx]; + this->trackedObjects[thingIx]->Refresh(thing->position, + thing->orientation); + delete thing; + return this->trackedObjects[thingIx]; } // Is this the fartest object we see? else if (this->trackedObjects[farthestObjIx] == nullptr || - (this->trackedObjects[objIx]->position.distance > + (this->trackedObjects[thingIx]->position.distance > this->trackedObjects[farthestObjIx]->position.distance)) { - farthestObjIx = objIx; + farthestObjIx = thingIx; } } } @@ -306,43 +307,39 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor, // max number of objects) if (availableSlotIx < maxObjectCount) { // a slot is available - this->trackedObjects[availableSlotIx] = obj; - obj->networkId = networkId; - if (objectId == 0x00) - obj->id = lastObjectId++; // availableSlotIx + 1; + this->trackedObjects[availableSlotIx] = thing; + thing->networkId = networkId; + if (thingId == 0x00) + thing->id = lastObjectId++; // availableSlotIx + 1; else - obj->id = objectId; + thing->id = thingId; #ifdef RC_DEBUG2 printf("%d: new tracked object {%d/%d}\n", availableSlotIx, obj->networkId, obj->id); #endif - return obj; + return thing; } // If this object is closer than the farthest object, then replace it - else if (obj->position.distance < + else if (thing->position.distance < this->trackedObjects[farthestObjIx]->position.distance) { delete this->trackedObjects[farthestObjIx]; - this->trackedObjects[farthestObjIx] = obj; - obj->networkId = networkId; - if (objectId == 0x00) - obj->id = lastObjectId++; // availableSlotIx + 1; + this->trackedObjects[farthestObjIx] = thing; + thing->networkId = networkId; + if (thingId == 0x00) + thing->id = lastObjectId++; // availableSlotIx + 1; else - obj->id = objectId; + thing->id = thingId; #ifdef RC_DEBUG2 - // 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; + return thing; } else { #ifdef RC_DEBUG2 - // Serial.print((int)obj->id); - // Serial.println(": delete tracked object"); printf("%d: delete tracked object {%d/%d}\n", -1, obj->networkId, obj->id); #endif // No available slot, delete trackedobject - delete obj; + delete thing; return nullptr; } } diff --git a/Perception.h b/Perception.h index 6abc9d1..26483f8 100644 --- a/Perception.h +++ b/Perception.h @@ -93,6 +93,7 @@ public: InterestingThing * AddTrackedObject(Sensor *sensor, Spherical position, Quaternion orientation = Quaternion::identity, + unsigned char objectType = 0x00, unsigned char objectId = 0x00, unsigned networkId = 0x00); InterestingThing *