Inverstigation response

NetworkPerception.cpp

@@ -2,6 +2,11 @@
 
 #include "RoboidControl/NetworkSync.h"
 
+// #define DEBUG true
+#if DEBUG
+#include <Arduino.h>
+#endif
+
 void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
                                       int packetsize) {
   // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
@@ -11,6 +16,9 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
   case NetworkSync::CreateMsg:
     ReceiveCreateMsg(buffer, roboid);
     break;
+  case NetworkSync::InvestigateMsg:
+    ReceiveInvestigateMsg(buffer, roboid);
+    break;
   case NetworkSync::PoseMsg:
     ReceivePoseMsg(buffer, roboid);
     break;
@@ -35,6 +43,27 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
   }
 }
 
+void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
+                                              Roboid *roboid) {
+  unsigned char networkId = data[1];
+  unsigned char objectId = data[2];
+
+#if DEBUG
+  printf("Received InvestigateMsg [%d/%d]\n", networkId, objectId);
+#endif
+
+  if (networkId != roboid->networkSync->networkId)
+    // We only response to investigation requests for our own objects
+    return;
+
+  InterestingThing *thing =
+      roboid->perception->FindTrackedObject(0x00, objectId);
+  if (thing == nullptr)
+    return;
+
+  roboid->networkSync->NewObject(thing);
+}
+
 void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char poseType = data[3];

NetworkPerception.h

@@ -13,6 +13,7 @@ public:
 
 protected:
   void ReceiveCreateMsg(unsigned char *data, Roboid *roboid);
+  void ReceiveInvestigateMsg(unsigned char *data, Roboid *roboid);
   void ReceivePoseMsg(unsigned char *data, Roboid *roboid);
   void ReceiveTypedObject(unsigned char *data, Roboid *roboid);