diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 099ae5c..85c627a 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -126,8 +126,8 @@ void ParticipantUDP::Receive() { bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) - std::cout << "Sending to " << remoteParticipant->ipAddress << ":" - << remoteParticipant->port << "\n"; + // std::cout << "Sending to " << remoteParticipant->ipAddress << ":" + // << remoteParticipant->port << "\n"; int err = sendto(this->sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr)); diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index 9bafa6f..4bbb25a 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -11,12 +11,10 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { if (thing->positionUpdated || force) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; - thing->positionUpdated = false; } if (thing->orientationUpdated || force) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; - thing->orientationUpdated = false; } if (thing->linearVelocityUpdated) { this->linearVelocity = thing->GetLinearVelocity(); diff --git a/Participant.cpp b/Participant.cpp index 42a0d94..f79e707 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -27,7 +27,12 @@ Participant::~Participant() { delete[] this->ipAddress; } -void Participant::Update(unsigned long currentTimeMs) {} +void Participant::Update(unsigned long currentTimeMs) { + for (Thing* thing : this->things) { + if (thing != nullptr) + thing->Update(currentTimeMs, true); + } +} Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { for (Thing* thing : this->things) { diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 622574e..6cd6517 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -32,9 +32,7 @@ ParticipantUDP::ParticipantUDP(int port) { this->isIsolated = true; } -ParticipantUDP::ParticipantUDP(const char* ipAddress, - int port, - int localPort) +ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) : Participant("127.0.0.1", localPort) { if (this->port == 0) this->isIsolated = true; @@ -58,15 +56,14 @@ void ParticipantUDP::begin() { } void ParticipantUDP::SetupUDP(int localPort, - const char* remoteIpAddress, - int remotePort) { + const char* remoteIpAddress, + int remotePort) { #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); thisWindows->Setup(localPort, remoteIpAddress, remotePort); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); thisPosix->Setup(localPort, remoteIpAddress, remotePort); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -90,6 +87,7 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { ParticipantMsg* msg = new ParticipantMsg(this->networkId); + std::cout << "Send Participant\n"; if (this->remoteSite == nullptr) this->Publish(msg); else @@ -98,20 +96,22 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { this->nextPublishMe = currentTimeMs + this->publishInterval; } - + this->ReceiveUDP(); } for (Thing* thing : this->things) { - if (thing != nullptr) { - thing->Update(currentTimeMs); - if (this->isIsolated == false) { - PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - for (Participant* sender : this->senders) - this->Send(sender, poseMsg); - delete poseMsg; - } + if (thing == nullptr) + continue; + + if (this->isIsolated == false) { + PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); + this->Send(thing->owner, poseMsg); + BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + this->Send(thing->owner, binaryMsg); + delete poseMsg; } + thing->Update(currentTimeMs, true); } } @@ -121,8 +121,7 @@ void ParticipantUDP::ReceiveUDP() { static_cast(this); thisWindows->Receive(); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); thisPosix->Receive(); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -159,8 +158,8 @@ Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) { #pragma region Send void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, - Thing* thing) { - // std::cout << "Send thing info " << (int)thing->id << " \n"; + Thing* thing) { + std::cout << "Send thing info [" << (int)thing->id << "] \n"; ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); this->Send(remoteParticipant, thingMsg); delete thingMsg; @@ -188,8 +187,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { static_cast(this); return thisWindows->Send(remoteParticipant, bufferSize); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); return thisPosix->Send(remoteParticipant, bufferSize); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -231,8 +229,7 @@ bool ParticipantUDP::Publish(IMessage* msg) { static_cast(this); return thisWindows->Publish(msg); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); return thisPosix->Publish(msg); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -253,14 +250,12 @@ bool ParticipantUDP::Publish(IMessage* msg) { #pragma region Receive void ParticipantUDP::ReceiveData(unsigned char packetSize, - char* senderIpAddress, - unsigned int senderPort) { + char* senderIpAddress, + unsigned int senderPort) { Participant* remoteParticipant = this->GetParticipant(senderIpAddress, senderPort); if (remoteParticipant == nullptr) { remoteParticipant = this->AddParticipant(senderIpAddress, senderPort); - // std::cout << "New sender " << senderIpAddress << ":" << senderPort - // << "\n"; std::cout << "New remote participant " << remoteParticipant->ipAddress << ":" << remoteParticipant->port << " " << (int)remoteParticipant->networkId << "\n"; @@ -270,7 +265,7 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize, } void ParticipantUDP::ReceiveData(unsigned char bufferSize, - Participant* remoteParticipant) { + Participant* remoteParticipant) { unsigned char msgId = this->buffer[0]; // std::cout << "receive msg " << (int)msgId << "\n"; switch (msgId) { @@ -312,11 +307,14 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, }; } -void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {} +void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { + std::cout << this->name << ": Process Participant " << (int)msg->networkId + << "\n"; +} void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { - std::cout << this->name << ": process Site Id " << (int)this->networkId - << "->" << (int)msg->networkId << "\n"; + // std::cout << this->name << ": process Site Id " << (int)this->networkId + // << "->" << (int)msg->networkId << "\n"; if (this->networkId != msg->networkId) { this->networkId = msg->networkId; // std::cout << this->things.size() << " things\n"; diff --git a/Thing.cpp b/Thing.cpp index b57be7f..73fb6fd 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -2,6 +2,7 @@ #include "Participant.h" #include "Participants/IsolatedParticipant.h" +#include "Messages/PoseMsg.h" #include @@ -190,7 +191,11 @@ void Thing::Update(bool recursive) { } void Thing::Update(unsigned long currentTimeMs, bool recursive) { - (void)currentTimeMs; + // if (this->positionUpdated || this->orientationUpdated) + // OnPoseChanged callback + this->positionUpdated = false; + this->orientationUpdated = false; + if (recursive) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { Thing* child = this->children[childIx];