Send only when connected

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);
   }
 }
 

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) {

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);

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;