diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index a787d5a..6d5c7f0 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -88,7 +88,7 @@ void ParticipantUDP::Receive() { #endif } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) { #if defined(ARDUINO) && defined(HAS_WIFI) // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" // << remoteParticipant->port << "\n"; diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index 5a9396f..5211300 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 22e8160..ca853a5 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -130,7 +130,7 @@ void ParticipantUDP::Receive() { #endif // IDF_VER } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) // std::cout << "Sending to " << remoteParticipant->ipAddress << ":" // << remoteParticipant->port << "\n"; diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index cf1c6bf..565c0fe 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -13,7 +13,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: diff --git a/Participant.cpp b/Participant.cpp index 3f1adc6..697872c 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -49,10 +49,10 @@ Participant::~Participant() { delete[] this->ipAddress; } -void Participant::Update() { +void Participant::Update(bool recurse) { for (Thing* thing : this->things) { if (thing != nullptr) - thing->Update(true); + thing->Update(); } } @@ -67,26 +67,26 @@ bool Participant::Send(IMessage* msg) { #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); - return thisWindows->Send(this, bufferSize); + return thisWindows->SendTo(this, bufferSize); #elif defined(__unix__) || defined(__APPLE__) Posix::ParticipantUDP* thisPosix = static_cast(this); - return thisPosix->Send(this, bufferSize); + return thisPosix->SendTo(this, bufferSize); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = static_cast(this); - return thisArduino->Send(this, bufferSize); + return thisArduino->SendTo(this, bufferSize); #elif defined(IDF_VER) EspIdf::ParticipantUDP* thisEspIdf = static_cast(this); - return thisEspIdf->Send(remoteParticipant, bufferSize); + return thisEspIdf->SendTo(this, bufferSize); #else return false; #endif } -Thing* Participant::Get(unsigned char thingId) { +Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { for (Thing* thing : this->things) { - if (thing->id == thingId) + if (thing->owner->networkId == networkId && thing->id == thingId) return thing; } std::cout << "Could not find thing " << this->ipAddress << ":" << this->port @@ -115,7 +115,7 @@ void Participant::Add(Thing* thing, bool checkId) { // std::cout << "Add thing with generated ID " << this->ipAddress << ":" // << this->port << "[" << (int)thing->id << "]\n"; } else { - Thing* foundThing = Get(thing->id); + Thing* foundThing = Get(thing->owner->networkId, thing->id); if (foundThing == nullptr) { #if defined(NO_STD) this->things[this->thingCount++] = thing; diff --git a/Participant.h b/Participant.h index 6e1217d..f8ecc9e 100644 --- a/Participant.h +++ b/Participant.h @@ -112,9 +112,10 @@ class Participant { std::list things; #endif /// @brief Find a thing managed by this participant + /// @param networkId The network ID of the thing /// @param thingId The ID of the thing /// @return The thing if found, nullptr when no thing has been found - Thing* Get(unsigned char thingId); + Thing* Get(unsigned char networkId, unsigned char thingId); /// @brief Add a new thing for this participant. /// @param thing The thing to add /// @param checkId If true, the thing.id is regenerated if it is zero @@ -129,7 +130,7 @@ class Participant { public: /// @brief Update all things for this participant - virtual void Update(); + virtual void Update(bool recurse = true); #pragma endregion Update diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index dc26caf..0830434 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -8,6 +8,10 @@ #include "Posix/PosixParticipant.h" #include "Windows/WindowsParticipant.h" +#include "Things/DistanceSensor.h" +#include "Things/TouchSensor.h" +#include "Things/DifferentialDrive.h" + #include namespace RoboidControl { @@ -83,7 +87,7 @@ void ParticipantUDP::SetupUDP(int localPort, // 1. receive external messages // 2. update the state // 3. send out the updated messages -void ParticipantUDP::Update() { +void ParticipantUDP::Update(bool recurse) { unsigned long currentTimeMs = Thing::GetTimeMs(); if (this->isIsolated == false) { @@ -182,7 +186,7 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, bool ParticipantUDP::Send(IMessage* msg) { if (this->remoteSite != nullptr) return this->remoteSite->Send(msg); - + return true; } @@ -379,6 +383,43 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) { << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " << (int)msg->parentId << "\n"; #endif + Participant* owner = Participant::registry.Get(msg->networkId); + if (owner == nullptr) { + owner = new Participant(); + owner->networkId = msg->networkId; + Participant::registry.Add(owner); + } + + Thing* thing = owner->Get(msg->networkId, msg->thingId); + if (thing == nullptr) { + bool isRemote = (sender->networkId != owner->networkId); + thing = ProcessNewThing(owner, msg, isRemote); + thing->id = msg->thingId; + thing->type = msg->thingType; + thing->isRemote = isRemote; + } + + if (msg->parentId != 0) { + thing->SetParent(owner->Get(msg->networkId, msg->parentId)); + if (thing->GetParent() == nullptr) + std::cout << "Could not find parent" << std::endl; + } else + thing->SetParent(nullptr); +} + +Thing* ParticipantUDP::ProcessNewThing(Participant* owner, + ThingMsg* msg, + bool isRemote) { + switch (msg->thingType) { + case Thing::Type::DistanceSensor: + return new DistanceSensor(owner->root); + case Thing::Type::TouchSensor: + return new TouchSensor(owner->root); + case Thing::Type::DifferentialDrive: + return new DifferentialDrive(owner->root); + default: + return new Thing(owner->root); + } } void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { @@ -387,7 +428,7 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { << (int)msg->thingId << "] "; #endif - Thing* thing = sender->Get(msg->thingId); + Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) { int nameLength = msg->nameLength; int stringLen = nameLength + 1; @@ -429,7 +470,7 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { if (owner == nullptr) return; - Thing* thing = owner->Get(msg->thingId); + Thing* thing = owner->Get(msg->networkId, msg->thingId); if (thing == nullptr) return; @@ -451,7 +492,7 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { Participant* owner = Participant::registry.Get(msg->networkId); if (owner != nullptr) { - Thing* thing = owner->Get(msg->thingId); + Thing* thing = owner->Get(msg->networkId, msg->thingId); if (thing != nullptr) thing->ProcessBinary(msg->data); #if !defined(NO_STD) @@ -478,7 +519,7 @@ void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) { << (int)msg->thingId << "]\n"; #endif - Thing* thing = sender->Get(msg->thingId); + Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) this->Remove(thing); } diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index a23bb6b..0eb205b 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -96,7 +96,7 @@ public: #pragma region Update public: - virtual void Update() override; + virtual void Update(bool recurse = true) override; protected: unsigned long nextPublishMe = 0; @@ -133,7 +133,10 @@ protected: virtual void Process(Participant* sender, ParticipantMsg* msg); virtual void Process(Participant* sender, NetworkIdMsg* msg); virtual void Process(Participant* sender, InvestigateMsg* msg); + virtual void Process(Participant* sender, ThingMsg* msg); + virtual Thing* ProcessNewThing(Participant* sender, ThingMsg* msg, bool isRemote); + virtual void Process(Participant* sender, NameMsg* msg); virtual void Process(Participant* sender, ModelUrlMsg* msg); virtual void Process(Participant* sender, PoseMsg* msg); diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index cc61639..085cd6f 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -70,16 +70,16 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {} void SiteServer::Process(Participant* sender, ThingMsg* msg) { - Thing* thing = sender->Get(msg->thingId); - if (thing == nullptr) + Thing* thing = sender->Get(msg->networkId, msg->thingId); + if (thing == nullptr) { // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); // Thing::Reconstruct(sender, msg->thingType, msg->thingId); //thing = new Thing(msg->thingType, sender->root); - ; + } thing->id = msg->thingId; if (msg->parentId != 0) { - thing->SetParent(Get(msg->parentId)); + thing->SetParent(Get(msg->networkId, msg->parentId)); if (thing->IsRoot()) // if (thing->GetParent() != nullptr) #if defined(NO_STD) diff --git a/Posix/PosixParticipant.cpp b/Posix/PosixParticipant.cpp index a01a059..9d44299 100644 --- a/Posix/PosixParticipant.cpp +++ b/Posix/PosixParticipant.cpp @@ -90,7 +90,7 @@ void ParticipantUDP::Receive() { #endif } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) { #if defined(__unix__) || defined(__APPLE__) // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port) // << "\n"; diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h index 98302f3..fc20888 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: diff --git a/Thing.h b/Thing.h index f538c33..f556712 100644 --- a/Thing.h +++ b/Thing.h @@ -88,6 +88,11 @@ class Thing { /// This can be either a Thing::Type of a byte value for custom types unsigned char type = Type::Undetermined; + /// @brief Is this a remote thing? + /// A remote thing is owned by other participant + /// and is not simulated by the local participant + bool isRemote = false; + /// @brief The participant owning this thing Participant* owner = nullptr; diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index c6e99db..5268787 100644 --- a/Windows/WindowsParticipant.cpp +++ b/Windows/WindowsParticipant.cpp @@ -102,7 +102,7 @@ void ParticipantUDP::Receive() { #endif // _WIN32 || _WIN64 } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) { #if defined(_WIN32) || defined(_WIN64) char ip_str[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN); diff --git a/Windows/WindowsParticipant.h b/Windows/WindowsParticipant.h index 9a747d9..46cb629 100644 --- a/Windows/WindowsParticipant.h +++ b/Windows/WindowsParticipant.h @@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: