From 25cb1e1822a8c899ed175f4834ed4bf00a6a8916 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 30 Aug 2024 12:01:38 +0200 Subject: [PATCH] Send only when connected --- NetworkPerception.cpp | 13 ++++++++----- NetworkSync.cpp | 45 +++++++++++++++++++++++++++---------------- NetworkSync.h | 8 ++++---- Perception.cpp | 2 +- 4 files changed, 41 insertions(+), 27 deletions(-) diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 3fbf571..6d28c13 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -131,6 +131,9 @@ void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) { } void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { + if (roboid->networkSync->networkId == 0) // We're not connected to a site yet + return; + unsigned char networkId = data[1]; unsigned char objectId = data[2]; unsigned char poseType = data[3]; @@ -138,15 +141,15 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { if (networkId == roboid->networkSync->networkId) networkId = 0x00; -#if RC_DEBUG - printf("Received PoseMsg [%d/%d]\n", networkId, objectId); -#endif - if (objectId == 0x80) return ReceivePlane(data, roboid); else if (objectId == 0x81) return ReceiveSphere(data, roboid); +#if RC_DEBUG + // printf("Received PoseMsg [%d/%d]\n", networkId, objectId); +#endif + Quaternion roboidOrientation = roboid->GetOrientation(); Spherical16 position = Spherical16::zero; Quaternion orientation = Quaternion::identity; @@ -184,7 +187,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { this, position, orientation, 0x81, 0x81, networkId); if (thing->networkId != 0x00 && thing->type == 0xFF) { // Unknown thing - roboid->networkSync->SendInvestigateThing(thing); + roboid->networkSync->SendInvestigate(thing); } } diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 34f71f2..c2defa4 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -57,7 +57,8 @@ void NetworkSync::NewObject(InterestingThing* thing) { printf("Sent CreateMsg [%d/%d] %d\n", networkId, buffer[1], buffer[2]); #endif - // PublishTrackedObject(roboid, obj); + thing->updated = true; + PublishTrackedObject(roboid, thing); } void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) { @@ -128,18 +129,24 @@ void NetworkSync::PublishModel(Thing* thing) { SendBuffer(ix); } -void NetworkSync::DestroyObject(InterestingThing* thing) { +void NetworkSync::SendDestroyThing(InterestingThing* thing) { + if (networkId == 0) // We're not connected to a site yet + return; + 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]); + printf("Sent DestroyMsg [%d/%d]\n", thing->networkId, thing->id); #endif } void NetworkSync::SendPose(Roboid* roboid, bool recurse) { + if (networkId == 0) // We're not connected to a site yet + return; + unsigned char ix = 0; buffer[ix++] = PoseMsg; buffer[ix++] = 0x00; @@ -149,7 +156,7 @@ void NetworkSync::SendPose(Roboid* roboid, bool recurse) { SendBuffer(ix); #if RC_DEBUG - printf("Sent PoseMsg [%d/%d]\n", networkId, buffer[1]); + // printf("Sent PoseMsg [%d/%d]\n", networkId, buffer[1]); #endif if (recurse) { @@ -196,6 +203,9 @@ void NetworkSync::PublishClient() { void NetworkSync::PublishTrackedObjects(Roboid* roboid, InterestingThing** objects) { + if (networkId == 0) // We're not connected to a site yet + return; + int n = 0; for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) { InterestingThing* obj = objects[objIx]; @@ -288,25 +298,26 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) { #endif } -void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing* obj) { -#ifdef RC_DEBUG - Serial.print("Send Destroy "); - Serial.println((int)obj->id); -#else - unsigned char buffer[2] = {DestroyMsg, (unsigned char)obj->id}; - sendBuffer(buffer, 2); -#endif -} +// void NetworkSync::SendDestroyThing(Buffer sendBuffer, InterestingThing* +// thing) { +// unsigned char ix = 0; +// buffer[ix++] = DestroyMsg; +// buffer[ix++] = thing->id; +// SendBuffer(ix); +// #ifdef RC_DEBUG +// printf("Sent DestroyThing [%d/%d]", thing->networkId, thing->id); +// #endif +// } -void NetworkSync::SendInvestigateThing(InterestingThing* thing) { -#ifdef RC_DEBUG - printf("Investigate [%d/%d]\n", thing->networkId, thing->id); -#endif +void NetworkSync::SendInvestigate(InterestingThing* thing) { unsigned char ix = 0; buffer[ix++] = InvestigateMsg; buffer[ix++] = thing->networkId; buffer[ix++] = thing->id; SendBuffer(ix); +#ifdef RC_DEBUG + printf("Sent Investigate [%d/%d]\n", thing->networkId, thing->id); +#endif } void NetworkSync::SendText(const char* s) { diff --git a/NetworkSync.h b/NetworkSync.h index d539a9b..13f9d59 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -20,7 +20,7 @@ class NetworkSync { virtual void NetworkUpdate(Roboid* roboid) = 0; /// @brief Inform that the given object is no longer being tracked /// @param obj - virtual void DestroyObject(InterestingThing* obj); + virtual void SendDestroyThing(InterestingThing* obj); virtual void NewObject(InterestingThing* obj); virtual void PublishModel(Roboid* obj); void PublishModel(Thing* thing); @@ -58,11 +58,11 @@ class NetworkSync { void ReceiveNetworkId(); - void SendInvestigateThing(InterestingThing* thing); + void SendInvestigate(InterestingThing* thing); void SendPoseMsg(Buffer sendBuffer, Roboid* roboid); - void SendDestroyObject(Buffer sendBuffer, InterestingThing* obj); - void PublishNewObject(); + // void SendDestroyThing(Buffer sendBuffer, InterestingThing* obj); + // void PublishNewObject(); void PublishRelativeThing(Thing* thing, bool recurse = false); void PublishTrackedObjects(Roboid* roboid, InterestingThing** objects); diff --git a/Perception.cpp b/Perception.cpp index fb3da07..180ad3f 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -510,7 +510,7 @@ void Perception::Update(unsigned long currentTimeMs) { if (thing->DegradeConfidence(deltaTime) == false) { // delete obj if (roboid != nullptr && roboid->networkSync != nullptr) { - roboid->networkSync->DestroyObject(thing); + roboid->networkSync->SendDestroyThing(thing); } this->trackedObjects[objIx] = nullptr; delete thing;