diff --git a/ControlCore/CoreThing.h b/ControlCore/CoreThing.h index 691110a..2222b1e 100644 --- a/ControlCore/CoreThing.h +++ b/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[]; diff --git a/ControlCore/Messages.cpp b/ControlCore/Messages.cpp index e5562b5..5278400 100644 --- a/ControlCore/Messages.cpp +++ b/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 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; diff --git a/ControlCore/Messages.h b/ControlCore/Messages.h index 8436455..09f5958 100644 --- a/ControlCore/Messages.h +++ b/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; diff --git a/DirectionalSensor.cpp b/DirectionalSensor.cpp index 6679169..b590705 100644 --- a/DirectionalSensor.cpp +++ b/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); } diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 36355a6..307d025 100644 --- a/NetworkSync.cpp +++ b/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 +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) { diff --git a/NetworkSync.h b/NetworkSync.h index 36d3bec..7481f42 100644 --- a/NetworkSync.h +++ b/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); diff --git a/Thing.h b/Thing.h index 5933a5e..94edb59 100644 --- a/Thing.h +++ b/Thing.h @@ -5,6 +5,7 @@ #include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/SwingTwist.h" +#include 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;