diff --git a/ControlCore/Client.cpp b/ControlCore/Client.cpp new file mode 100644 index 0000000..57cc7d0 --- /dev/null +++ b/ControlCore/Client.cpp @@ -0,0 +1,6 @@ +#include "Client.h" + +bool Passer::Control::ControlClient::SendMsg(unsigned char *buffer, + unsigned char bufferSize) { + return false; +} \ No newline at end of file diff --git a/ControlCore/Client.h b/ControlCore/Client.h new file mode 100644 index 0000000..e0edf13 --- /dev/null +++ b/ControlCore/Client.h @@ -0,0 +1,11 @@ +#pragma once + +namespace Passer::Control { + +class ControlClient { +public: + unsigned char *buffer; + bool SendMsg(unsigned char *buffer, unsigned char bufferSize); +}; + +} // namespace Passer::Control \ No newline at end of file diff --git a/ControlCore/LowLevelMessages.cpp b/ControlCore/LowLevelMessages.cpp new file mode 100644 index 0000000..a72ada4 --- /dev/null +++ b/ControlCore/LowLevelMessages.cpp @@ -0,0 +1,12 @@ +#include "LowLevelMessages.h" + +#include "../float16/float16.h" + +void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix, + float value) { + float16 value16 = float16(value); + short binary = value16.getBinary(); + + buffer[(*ix)++] = (binary >> 8) & 0xFF; + buffer[(*ix)++] = binary & 0xFF; +} \ No newline at end of file diff --git a/ControlCore/LowLevelMessages.h b/ControlCore/LowLevelMessages.h new file mode 100644 index 0000000..67a6095 --- /dev/null +++ b/ControlCore/LowLevelMessages.h @@ -0,0 +1,12 @@ +#include "../LinearAlgebra/Spherical.h" + +namespace Passer::Control { + +class LowLevelMessages { +public: + static void SendFloat16(unsigned char *buffer, unsigned char *ix, + float value); +}; + +} // namespace Passer::Control +using namespace Passer::Control; \ No newline at end of file diff --git a/ControlCore/Messages.cpp b/ControlCore/Messages.cpp index 261c01c..7705a91 100644 --- a/ControlCore/Messages.cpp +++ b/ControlCore/Messages.cpp @@ -1,39 +1,128 @@ #include "Messages.h" +#include "LowLevelMessages.h" + #pragma region IMessage -unsigned char *IMessage::Serialize() { return nullptr; } +unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; } -bool IMessage::SendMsg(IMessage msg, unsigned char bufferSize) { - return SendMsg(msg.Serialize(), bufferSize); +bool IMessage::SendMsg(ControlClient client, IMessage msg, + unsigned char bufferSize) { + return SendMsg(client, client.buffer, msg.Serialize(client.buffer)); } -bool IMessage::SendMsg(unsigned char *buffer, unsigned char bufferSize) { - // SendBuffer(buffer, bufferSize); - return false; +bool IMessage::SendMsg(ControlClient client, unsigned char *buffer, + unsigned char bufferSize) { + return false; // client.SendMsg(buffer, bufferSize); } #pragma endregion #pragma region Investigate -InvestigateMsg::InvestigateMsg(unsigned char thingId) { - this->networkId = 0; +InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) { + this->networkId = networkId; this->thingId = thingId; } -unsigned char *InvestigateMsg::Serialize() { - unsigned char *buffer = IMessage::buffer; - buffer[0] = InvestigateMsg::id; - buffer[1] = this->networkId; - buffer[2] = this->thingId; - return buffer; +unsigned char InvestigateMsg::Serialize(unsigned char *buffer) { + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + buffer[ix++] = this->thingId; + return ix; } -bool InvestigateMsg::Send(unsigned char thingId) { - InvestigateMsg msg = InvestigateMsg(thingId); - return SendMsg(msg, InvestigateMsg::length); +bool InvestigateMsg::Send(ControlClient client, unsigned char networkId, + unsigned char thingId) { + InvestigateMsg msg = InvestigateMsg(networkId, thingId); + return SendMsg(client, msg, InvestigateMsg::length); } -// Investiaget +// Investigate +#pragma endregion + +#pragma region Thing + +Passer::Control::ThingMsg::ThingMsg(unsigned char networkId, + unsigned char thingId, + unsigned char thingType, + unsigned char parentId) { + this->networkId = networkId; + this->thingId = thingId; + this->thingType = thingType; + this->parentId = parentId; +} + +unsigned char ThingMsg::Serialize(unsigned char *buffer) { + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + buffer[ix++] = this->thingId; + buffer[ix++] = this->thingType; + buffer[ix++] = this->parentId; + return ix; +} + +bool Passer::Control::ThingMsg::Send(ControlClient client, + unsigned char networkId, + unsigned char thingId, + unsigned char thingType, + unsigned char parentId) { + ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId); + return SendMsg(client, msg, ThingMsg::length); +} + +// Thing +#pragma endregion + +#pragma region Name + +NameMsg::NameMsg(unsigned char networkId, unsigned char thingId, + unsigned char nameLength, const char *name) { + this->networkId = networkId; + this->thingId = thingId; + this->nameLength = nameLength; + this->name = name; +} + +unsigned char Passer::Control::NameMsg::Serialize(unsigned char *buffer) { + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + buffer[ix++] = this->thingId; + buffer[ix++] = this->nameLength; + for (int nameIx = 0; nameIx < this->nameLength; nameIx++) + buffer[ix++] = name[nameIx]; + return ix; +} + +// Name +#pragma endregion + +#pragma region ModelUrl + +ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId, + unsigned char urlLength, const char *url, + float scale) { + this->networkId = networkId; + this->thingId = thingId; + this->urlLength = urlLength; + this->url = url; + this->scale = scale; +} + +unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) { + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + buffer[ix++] = this->thingId; + LowLevelMessages::SendFloat16(buffer, &ix, this->scale); + buffer[ix++] = this->urlLength; + for (int urlIx = 0; urlIx < this->urlLength; urlIx++) + buffer[ix++] = url[urlIx]; + return ix; +} + +// Model Url #pragma endregion \ No newline at end of file diff --git a/ControlCore/Messages.h b/ControlCore/Messages.h index ad298d9..e5f5164 100644 --- a/ControlCore/Messages.h +++ b/ControlCore/Messages.h @@ -1,13 +1,19 @@ +#pragma once +#include "../float16/float16.h" +#include "Client.h" + namespace Passer::Control { class IMessage { public: - static unsigned char buffer[256]; + // static unsigned char buffer[256]; - virtual unsigned char *Serialize(); + virtual unsigned char Serialize(unsigned char *buffer); - static bool SendMsg(IMessage msg, unsigned char bufferSize); - static bool SendMsg(unsigned char *buffer, unsigned char bufferSize); + static bool SendMsg(ControlClient client, IMessage msg, + unsigned char bufferSize); + static bool SendMsg(ControlClient client, unsigned char *buffer, + unsigned char bufferSize); }; class InvestigateMsg : public IMessage { @@ -17,11 +23,61 @@ public: unsigned char networkId; unsigned char thingId; - InvestigateMsg(unsigned char thingId); + InvestigateMsg(unsigned char networkId, unsigned char thingId); - virtual unsigned char *Serialize() override; + virtual unsigned char Serialize(unsigned char *buffer) override; - static bool Send(unsigned char thingId); + static bool Send(ControlClient client, unsigned char networkId, + unsigned char thingId); +}; + +class ThingMsg : public IMessage { +public: + static const unsigned char id = 0x80; + static const unsigned char length = 5; + unsigned char networkId; + unsigned char thingId; + unsigned char thingType; + unsigned char parentId; + + ThingMsg(unsigned char networkId, unsigned char thingId, + unsigned char thingType, unsigned char parentId); + + virtual unsigned char Serialize(unsigned char *buffer) override; + + static bool Send(ControlClient client, unsigned char networkId, + unsigned char thingId, unsigned char thingType, + unsigned char parentId); +}; + +class NameMsg : public IMessage { +public: + static const unsigned char id = 0x91; + static const unsigned char length = 4; + unsigned char networkId; + unsigned char thingId; + unsigned char nameLength; + const char *name; + + NameMsg(unsigned char networkId, unsigned char thingId, + unsigned char nameLength, const char *name); + + virtual unsigned char Serialize(unsigned char *buffer) override; +}; + +class ModelUrlMsg : public IMessage { +public: + static const unsigned char id = 0x90; + unsigned char networkId; + unsigned char thingId; + float scale; + unsigned char urlLength; + const char *url; + + ModelUrlMsg(unsigned char networkId, unsigned char thingId, + unsigned char urlLegth, const char *url, float scale = 1); + + virtual unsigned char Serialize(unsigned char *buffer) override; }; } // namespace Passer::Control diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 439f663..ff3906c 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -1,5 +1,6 @@ #include "NetworkPerception.h" +#include "ControlCore/Messages.h" #include "NetworkSync.h" #include "float16/float16.h" @@ -174,118 +175,120 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) { if (networkId == roboid->networkSync->networkId) networkId = 0x00; - if (objectId == 0x80) - return ReceivePlane(data, roboid); - else if (objectId == 0x81) - return ReceiveSphere(data, roboid); + // 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 - InterestingThing *thing = - roboid->perception->FindTrackedObject(networkId, objectId); - if (thing == nullptr) { - thing = roboid->perception->AddTrackedObject(this, position, orientation, - 0xFF, objectId, networkId); - if (thing->networkId != 0x00) { - // Unknown thing - roboid->networkSync->SendInvestigate(thing); - } - } + // InterestingThing *thing = + // roboid->perception->FindTrackedObject(networkId, objectId); + // if (thing == nullptr) { + // thing = roboid->perception->AddTrackedObject(this, position, orientation, + // 0xFF, objectId, networkId); + // if (thing->networkId != 0x00) { + // // Unknown thing + // InvestigateMsg::Send(thing); + // // roboid->networkSync->SendInvestigate(thing); + // } + // } // SwingTwist16 roboidOrientation = roboid->GetOrientation(); - Spherical16 position = Spherical16::zero; - SwingTwist16 orientation = SwingTwist16::identity; + // Spherical16 position = Spherical16::zero; + // SwingTwist16 orientation = SwingTwist16::identity; - Vector3 worldAngles = ReceiveVector3(data, 16); - SwingTwist16 worldOrientation = SwingTwist16( - Angle16::Degrees(worldAngles.Up()), Angle16::Degrees(worldAngles.Right()), - Angle16::Degrees( - worldAngles.Forward())); // Quaternion::Euler(worldAngles); + // Vector3 worldAngles = ReceiveVector3(data, 16); + // SwingTwist16 worldOrientation = SwingTwist16( + // Angle16::Degrees(worldAngles.Up()), + // Angle16::Degrees(worldAngles.Right()), Angle16::Degrees( + // worldAngles.Forward())); // Quaternion::Euler(worldAngles); - if ((poseType & NetworkSync::Pose_Orientation) != 0) { - if (objectId == 0) { - // roboid->SetOrientation(worldOrientation); - if (roboid->worldOrigin == nullptr) { - printf("creating new origin\n"); - roboid->worldOrigin = new Thing(0); - printf("created\n"); - } + // if ((poseType & NetworkSync::Pose_Orientation) != 0) { + // if (objectId == 0) { + // // roboid->SetOrientation(worldOrientation); + // if (roboid->worldOrigin == nullptr) { + // printf("creating new origin\n"); + // roboid->worldOrigin = new Thing(0); + // printf("created\n"); + // } - roboid->worldOrigin->orientation = - SwingTwist16::Inverse(worldOrientation); - } else { - // orientation = SwingTwist16::Inverse(roboidOrientation) * - // worldOrientation; if (thing != nullptr) { - // thing->orientation = orientation; - // } - SwingTwist16 originOrientation; - Spherical16 originPosition; - if (roboid->worldOrigin == nullptr) { - originOrientation = SwingTwist16::identity; - originPosition = Spherical16::zero; - } else { - originOrientation = roboid->worldOrigin->orientation; - originPosition = roboid->worldOrigin->position; - } + // roboid->worldOrigin->orientation = + // SwingTwist16::Inverse(worldOrientation); + // } else { + // // orientation = SwingTwist16::Inverse(roboidOrientation) * + // // worldOrientation; if (thing != nullptr) { + // // thing->orientation = orientation; + // // } + // SwingTwist16 originOrientation; + // Spherical16 originPosition; + // if (roboid->worldOrigin == nullptr) { + // originOrientation = SwingTwist16::identity; + // originPosition = Spherical16::zero; + // } else { + // originOrientation = roboid->worldOrigin->orientation; + // originPosition = roboid->worldOrigin->position; + // } - thing->orientation = originOrientation * worldOrientation; - } - } + // thing->orientation = originOrientation * worldOrientation; + // } + // } - if ((poseType & NetworkSync::Pose_Position) != 0) { - Vector3 worldVector3 = ReceiveVector3(data, 4); - Spherical16 worldPosition = Spherical16::FromVector3(worldVector3); - if (objectId == 0) { - // roboid->SetPosition(worldPosition); - if (roboid->worldOrigin == nullptr) { - printf("creating new origin again\n"); - roboid->worldOrigin = new Thing(0); - } + // if ((poseType & NetworkSync::Pose_Position) != 0) { + // Vector3 worldVector3 = ReceiveVector3(data, 4); + // Spherical16 worldPosition = Spherical16::FromVector3(worldVector3); + // if (objectId == 0) { + // // roboid->SetPosition(worldPosition); + // if (roboid->worldOrigin == nullptr) { + // printf("creating new origin again\n"); + // roboid->worldOrigin = new Thing(0); + // } - roboid->worldOrigin->position = - roboid->worldOrigin->orientation * -worldPosition; - } else { - SwingTwist16 originOrientation; - Spherical16 originPosition; - if (roboid->worldOrigin == nullptr) { - originOrientation = SwingTwist16::identity; - originPosition = Spherical16::zero; - } else { - originOrientation = roboid->worldOrigin->orientation; - originPosition = roboid->worldOrigin->position; - } + // roboid->worldOrigin->position = + // roboid->worldOrigin->orientation * -worldPosition; + // } else { + // SwingTwist16 originOrientation; + // Spherical16 originPosition; + // if (roboid->worldOrigin == nullptr) { + // originOrientation = SwingTwist16::identity; + // originPosition = Spherical16::zero; + // } else { + // originOrientation = roboid->worldOrigin->orientation; + // originPosition = roboid->worldOrigin->position; + // } - // SwingTwist16 roboidLocalOrientation = - // originOrientation * worldOrientation; + // // SwingTwist16 roboidLocalOrientation = + // // originOrientation * worldOrientation; - thing->position = originPosition + originOrientation * worldPosition; + // thing->position = originPosition + originOrientation * worldPosition; - /* - Spherical16 roboidPosition = roboid->GetPosition(); + // /* + // Spherical16 roboidPosition = roboid->GetPosition(); - // float distance = Vector3::Distance(roboidPosition, worldPosition); - // if (roboid->perception->IsInteresting(distance) == false) { - // // printf(" not interesting\n"); - // return; - // } + // // float distance = Vector3::Distance(roboidPosition, worldPosition); + // // if (roboid->perception->IsInteresting(distance) == false) { + // // // printf(" not interesting\n"); + // // return; + // // } - Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) * - (worldPosition - roboidPosition); - Vector3 roboidVector3 = roboidPosition.ToVector3(); - printf(" roboid position (%f %f %f)\n", roboidVector3.Right(), - roboidVector3.Up(), roboidVector3.Forward()); - printf(" [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n ", - networkId, objectId, worldVector3.Right(), worldVector3.Up(), - worldVector3.Forward(), localPosition.distance, - localPosition.direction.horizontal.InDegrees(), - localPosition.direction.vertical.InDegrees()); + // Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) * + // (worldPosition - roboidPosition); + // Vector3 roboidVector3 = roboidPosition.ToVector3(); + // printf(" roboid position (%f %f %f)\n", roboidVector3.Right(), + // roboidVector3.Up(), roboidVector3.Forward()); + // printf(" [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n + // ", + // networkId, objectId, worldVector3.Right(), worldVector3.Up(), + // worldVector3.Forward(), localPosition.distance, + // localPosition.direction.horizontal.InDegrees(), + // localPosition.direction.vertical.InDegrees()); - thing->position = localPosition; - */ - } - } + // thing->position = localPosition; + // */ + // } + // } } void NetworkPerception::ReceiveTypedObject(unsigned char *data, diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 8997353..3c3d2ef 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -12,6 +12,7 @@ #endif #endif +#include "ControlCore/Messages.h" #include "LinearAlgebra/Angle.h" #include "LinearAlgebra/AngleUsing.h" #include "LinearAlgebra/Spherical.h" @@ -61,16 +62,16 @@ void NetworkSync::ReceiveNetworkId() { printf("completed\n"); } -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::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::ReceiveCustom(unsigned char packetSize) { unsigned char ix = 1; // first byte is the msgId @@ -101,6 +102,9 @@ void NetworkSync::PublishState(Roboid *roboid) { } void NetworkSync::SendThingInfo(Thing *thing, bool recurse) { + if (thing == nullptr) + return; + SendThing(thing); SendName(thing); SendModel(thing); @@ -120,17 +124,23 @@ void NetworkSync::SendThing(Thing *thing) { if (thing == nullptr) return; - unsigned char ix = 0; - buffer[ix++] = ThingMsg; - buffer[ix++] = this->networkId; - buffer[ix++] = thing->id; - buffer[ix++] = (unsigned char)thing->type; - Thing *parentThing = thing->GetParent(); - if (parentThing != nullptr) - buffer[ix++] = parentThing->id; - else - buffer[ix++] = 0x00; - SendBuffer(ix); + // unsigned char ix = 0; + // buffer[ix++] = ThingMsg; + // buffer[ix++] = this->networkId; + // buffer[ix++] = thing->id; + // buffer[ix++] = (unsigned char)thing->type; + // Thing *parentThing = thing->GetParent(); + // if (parentThing != nullptr) + // buffer[ix++] = parentThing->id; + // else + // buffer[ix++] = 0x00; + // SendBuffer(ix); + + Thing *parent = thing->GetParent(); + Passer::Control::ThingMsg msg = + Passer::Control::ThingMsg(this->networkId, thing->id, thing->type, + parent == nullptr ? 0 : parent->id); + SendBuffer(msg.Serialize(this->buffer)); #ifdef RC_DEBUG printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type); @@ -145,16 +155,19 @@ void NetworkSync::SendName(Thing *thing) { if (len > 255) return; - unsigned char ix = 0; - buffer[ix++] = NameMsg; - buffer[ix++] = this->networkId; - buffer[ix++] = thing->id; + // unsigned char ix = 0; + // buffer[ix++] = NameMsg; + // buffer[ix++] = this->networkId; + // buffer[ix++] = thing->id; - buffer[ix++] = len; - for (unsigned char nameIx = 0; nameIx < len; nameIx++) - buffer[ix++] = thing->name[nameIx]; + // buffer[ix++] = len; + // for (unsigned char nameIx = 0; nameIx < len; nameIx++) + // buffer[ix++] = thing->name[nameIx]; - SendBuffer(ix); + // SendBuffer(ix); + Passer::Control::NameMsg msg = + Passer::Control::NameMsg(this->networkId, thing->id, len, thing->name); + SendBuffer(msg.Serialize(this->buffer)); #ifdef RC_DEBUG SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1], @@ -170,19 +183,22 @@ void NetworkSync::SendModel(Thing *thing) { if (len > 255) return; - unsigned char ix = 0; - buffer[ix++] = ModelMsg; - buffer[ix++] = this->networkId; - buffer[ix++] = thing->id; // objectId - // Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition); - // Messages::SendSpherical16(buffer, &ix, s); - Messages::SendFloat16(buffer, &ix, thing->modelScale); + // unsigned char ix = 0; + // buffer[ix++] = ModelMsg; + // buffer[ix++] = this->networkId; + // buffer[ix++] = thing->id; // objectId + // // Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition); + // // Messages::SendSpherical16(buffer, &ix, s); + // Messages::SendFloat16(buffer, &ix, thing->modelScale); - buffer[ix++] = len; - for (int urlIx = 0; urlIx < len; urlIx++) - buffer[ix++] = thing->modelUrl[urlIx]; + // buffer[ix++] = len; + // for (int urlIx = 0; urlIx < len; urlIx++) + // buffer[ix++] = thing->modelUrl[urlIx]; - SendBuffer(ix); + // SendBuffer(ix); + Passer::Control::ModelUrlMsg msg = Passer::Control::ModelUrlMsg( + this->networkId, thing->id, len, thing->modelUrl, thing->modelScale); + SendBuffer(msg.Serialize(this->buffer)); } void NetworkSync::SendCustom(Thing *thing) { @@ -392,4 +408,7 @@ void NetworkSync::SendInt(const int x) { void NetworkSync::SendBuffer(unsigned char bufferSize) {} -void NetworkSync::PublishBuffer(unsigned char bufferSize) {} \ No newline at end of file +void NetworkSync::PublishBuffer(unsigned char bufferSize) {} + +void NetworkSync::SendCBuffer(unsigned char bufferSize, unsigned char *buffer) { +} \ No newline at end of file diff --git a/NetworkSync.h b/NetworkSync.h index 04e6fad..1a4b30e 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -1,5 +1,6 @@ #pragma once +#include "ControlCore/Messages.h" #include "NetworkPerception.h" #include "Perception.h" #include "Roboid.h" @@ -66,7 +67,7 @@ public: void PublishState(Roboid *roboid); - void SendInvestigate(InterestingThing *thing); + // void SendInvestigate(InterestingThing *thing); void PublishPerception(Roboid *roboid); void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects); @@ -100,6 +101,8 @@ protected: virtual void SendBuffer(unsigned char bufferSize); virtual void PublishBuffer(unsigned char bufferSize); + virtual void SendCBuffer(unsigned char bufferSize, unsigned char *buffer); + void PublishClient(); };