Send custom msg with control core

ControlCore/CoreThing.h

@@ -38,6 +38,8 @@ public:
 
   void SetName(const char *name);
 
+  virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
+
   // All things
 private:
   static CoreThing *allThings[];

ControlCore/Messages.cpp

@@ -237,52 +237,33 @@ unsigned char PoseMsg::Serialize(unsigned char *buffer) {
 #pragma region CustomMsg
 
 CustomMsg::CustomMsg(unsigned char *buffer) {
-  unsigned char ix;
+  unsigned char ix = 1;
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
-  this->dataSize = buffer[ix++];
   this->data =
       buffer + ix; // This is only valid because the code ensures the the msg
                    // lifetime is shorter than the buffer lifetime...
 }
 
-CustomMsg::CustomMsg(unsigned char networkId, unsigned char thingId,
-                     unsigned char *data, unsigned char dataSize) {
+CustomMsg::CustomMsg(unsigned char networkId, CoreThing *thing) {
   this->networkId = networkId;
-  this->thingId = thingId;
-  this->dataSize = dataSize;
-  this->data = data;
+  this->thingId = thing->id;
+  this->thing = thing;
 }
 
+#include <iostream>
 unsigned char CustomMsg::Serialize(unsigned char *buffer) {
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  buffer[ix++] = this->thingId;
-  for (int dataIx = 0; dataIx < this->dataSize; dataIx++)
-    buffer[ix++] = this->data[dataIx];
+  unsigned char ix = this->length;
+  this->thing->SendBytes(buffer, &ix);
+  if (ix <= this->length) // in this case, no data is actually sent
+    return 0;
+
+  buffer[0] = this->id;
+  buffer[1] = this->networkId;
+  buffer[2] = this->thingId;
   return ix;
 }
 
-// void CustomMsg::Deserialize(unsigned char *buffer) {
-//   unsigned char ix;
-//   this->networkId = buffer[ix++];
-//   this->thingId = buffer[ix++];
-//   this->dataSize = buffer[ix++];
-//   this->data = buffer + ix; // challenging: point directly into the buffer!
-
-//   // this->data = new unsigned char[this->dataSize]; // memory leak!
-//   // for (unsigned char dataIx = 0; dataIx < this->dataSize; dataIx++)
-//   //   this->data[dataIx] = buffer[ix++];
-// }
-
-// bool CustomMsg::Send(Participant *participant, unsigned char networkId,
-//                      unsigned char thingId, unsigned char *data,
-//                      unsigned char dataSize) {
-//   CustomMsg msg = CustomMsg(networkId, thingId, data, dataSize);
-//   return msg.Send(participant);
-// }
-
 CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
   CustomMsg msg = CustomMsg(buffer);
   return msg;

ControlCore/Messages.h

@@ -133,13 +133,13 @@ public:
 
   unsigned char networkId;
   unsigned char thingId;
+  CoreThing *thing;
 
   unsigned char dataSize;
   unsigned char *data;
 
   CustomMsg(unsigned char *buffer);
-  CustomMsg(unsigned char networkId, unsigned char thingId, unsigned char *data,
-            unsigned char dataSize);
+  CustomMsg(unsigned char networkId, CoreThing *thing);
 
   virtual unsigned char Serialize(unsigned char *buffer) override;
 

DirectionalSensor.cpp

@@ -9,7 +9,7 @@ DirectionalSensor::DirectionalSensor() : Sensor() {
 
 Spherical16 DirectionalSensor::GetVector() { return Spherical16::zero; }
 
-void DirectionalSensor::ProcessBytes(unsigned char *bytes) {
+void DirectionalSensor::ProcessBytes(unsigned char *data) {
   unsigned char ix = 0;
-  this->vector = LowLevelMessages::ReceiveSpherical16(bytes, &ix);
+  this->vector = LowLevelMessages::ReceiveSpherical16(data, &ix);
 }

NetworkSync.cpp

@@ -28,13 +28,7 @@ NetworkSync::NetworkSync(Roboid *roboid) {
 
 void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
   // printf("Received msgId %d, length %d\n", buffer[0], bytecount);
-
   ReceiveData(bytecount);
-  switch (buffer[0]) {
-  case CustomMsg::id:
-    ReceiveCustom(bytecount);
-    break;
-  }
 }
 
 void NetworkSync::PublishClient() {
@@ -109,29 +103,6 @@ void NetworkSync::ProcessThingMsg(ThingMsg msg) {
 
 void NetworkSync::ProcessPoseMsg(PoseMsg msg) {}
 
-void NetworkSync::ReceiveCustom(unsigned char packetSize) {
-  unsigned char ix = 1; // first byte is the msgId
-  unsigned char networkId = buffer[ix++];
-  unsigned char thingId = buffer[ix++];
-  // unsigned char len = buffer[ix++];
-  unsigned char len = packetSize - 3;
-  printf("Custom size = %d\n", len);
-  if (len > 0) {
-    unsigned char *bytes = new unsigned char[len];
-    // printf("Received %d bytes for [%d/%d]\n", len, networkId, thingId);
-    for (unsigned char bytesIx = 0; bytesIx < len; bytesIx++)
-      bytes[bytesIx] = buffer[ix++];
-
-    // networkId is not used right now, we assume networkId == 0
-    Thing *thing = roboid->FindChild(thingId);
-    printf("Found thing %s\n", thing->name);
-    if (thing != nullptr)
-      thing->ProcessBytes(bytes);
-
-    delete bytes;
-  }
-}
-
 void NetworkSync::PublishState(Roboid *roboid) {
   SendPose(roboid);
   PublishPerception(roboid);
@@ -201,14 +172,16 @@ void NetworkSync::SendModel(Thing *thing) {
 }
 
 void NetworkSync::SendCustom(Thing *thing) {
-  unsigned char ix = 0;
-  buffer[ix++] = CustomMsg::id;
-  buffer[ix++] = this->networkId;
-  buffer[ix++] = thing->id;
-  thing->SendBytes(buffer, &ix);
+  CustomMsg msg = CustomMsg(this->networkId, thing);
+  msg.Send(this);
+}
 
-  if (ix > 3) // When ix <= 3 then there is no custom data
-    SendBuffer(ix);
+#include <iostream>
+void NetworkSync::ProcessCustomMsg(CustomMsg msg) {
+  // we assume networkId == 0 as custom messages are intended for my things
+  Thing *thing = roboid->FindChild(msg.thingId);
+  if (thing != nullptr)
+    thing->ProcessBytes(msg.data);
 }
 
 void NetworkSync::SendDestroyThing(InterestingThing *thing) {

NetworkSync.h

@@ -57,6 +57,7 @@ protected:
   virtual void ProcessInvestigateMsg(InvestigateMsg msg) override;
   virtual void ProcessThingMsg(ThingMsg msg) override;
   virtual void ProcessPoseMsg(PoseMsg msg) override;
+  virtual void ProcessCustomMsg(CustomMsg msg) override;
 
   void ReceiveNetworkId();
   void ReceiveCustom(unsigned char packetSize);

Thing.h

@@ -5,6 +5,7 @@
 #include "LinearAlgebra/Quaternion.h"
 #include "LinearAlgebra/Spherical.h"
 #include "LinearAlgebra/SwingTwist.h"
+#include <iostream>
 
 namespace Passer {
 namespace RoboidControl {
@@ -100,8 +101,9 @@ public:
 
   float modelScale = 1;
 
-  virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
-  virtual void ProcessBytes(unsigned char *bytes) {};
+    virtual void ProcessBytes(unsigned char *bytes) {
+    std::cout << "no processing\n";
+  };
 
 protected:
   static int lastThingId;