From 1dd0c3a4a930560300de60fead9d5951f3c58ba1 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 26 Feb 2025 11:13:18 +0100 Subject: [PATCH] Collisions work! --- Arduino/Participant.cpp | 2 +- Messages/PoseMsg.cpp | 36 +++++++++++----------- Messages/PoseMsg.h | 16 +++++----- Participant.cpp | 66 ++++++++++++++++------------------------- Participant.h | 5 ++-- 5 files changed, 56 insertions(+), 69 deletions(-) diff --git a/Arduino/Participant.cpp b/Arduino/Participant.cpp index 550e2d3..db22528 100644 --- a/Arduino/Participant.cpp +++ b/Arduino/Participant.cpp @@ -23,7 +23,7 @@ void Participant::Setup(int localPort, const char* remoteIpAddress, int remotePo std::cout << "No network available!\n"; return; } - udp.begin(this->localPort); + udp.begin(localPort); std::cout << "Wifi sync started to port " << this->remotePort << "\n"; #endif diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index d164c9e..62fc235 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -3,33 +3,33 @@ namespace RoboidControl { -PoseMsg::PoseMsg(unsigned char networkId, - unsigned char thingId, - unsigned char poseType, - Spherical16 position, - SwingTwist16 orientation, - Spherical16 linearVelocity, - Spherical16 angularVelocity) { - this->networkId = networkId; - this->thingId = thingId; +// PoseMsg::PoseMsg(unsigned char networkId, +// unsigned char thingId, +// unsigned char poseType, +// Spherical16 position, +// SwingTwist16 orientation, +// Spherical16 linearVelocity, +// Spherical16 angularVelocity) { +// this->networkId = networkId; +// this->thingId = thingId; - this->poseType = poseType; - this->position = position; - this->orientation = orientation; - this->linearVelocity = linearVelocity; - this->angularVelocity = angularVelocity; -} -PoseMsg::PoseMsg(unsigned char networkId, Thing* thing) { +// this->poseType = poseType; +// this->position = position; +// this->orientation = orientation; +// this->linearVelocity = linearVelocity; +// this->angularVelocity = angularVelocity; +// } +PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { this->networkId = networkId; this->thingId = thing->id; this->poseType = 0; - if (thing->positionUpdated) { + if (thing->positionUpdated || force) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; thing->positionUpdated = false; } - if (thing->orientationUpdated) { + if (thing->orientationUpdated || force ) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; thing->orientationUpdated = false; diff --git a/Messages/PoseMsg.h b/Messages/PoseMsg.h index ca0add2..6e4dacc 100644 --- a/Messages/PoseMsg.h +++ b/Messages/PoseMsg.h @@ -45,18 +45,18 @@ class PoseMsg : public IMessage { /// @param orientation The orientation of the thing in local space /// @param linearVelocity The linear velocity of the thing in local space in meters per second /// @param angularVelocity The angular velocity of the thing in local space - PoseMsg(unsigned char networkId, - unsigned char thingId, - unsigned char poseType, - Spherical16 position, - SwingTwist16 orientation, - Spherical16 linearVelocity = Spherical16(), - Spherical16 angularVelocity = Spherical16()); +// PoseMsg(unsigned char networkId, +// unsigned char thingId, +// unsigned char poseType, +// Spherical16 position, +// SwingTwist16 orientation, +// Spherical16 linearVelocity = Spherical16(), +// Spherical16 angularVelocity = Spherical16()); /// @brief Create a new message for sending /// @param networkId he network ID of the thing /// @param thing The thing for which the pose shouldbe sent - PoseMsg(unsigned char networkId, Thing* thing); + PoseMsg(unsigned char networkId, Thing* thing, bool force = false); /// @copydoc RoboidControl::IMessage::IMessage(char*) PoseMsg(const char* buffer); diff --git a/Participant.cpp b/Participant.cpp index 0323fd0..c764bd2 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -26,27 +26,16 @@ namespace RoboidControl { Participant::Participant(int port) { this->ipAddress = "0.0.0.0"; this->port = port; - - // this->senders.push_back(this); - - // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152; - this->localPort = port; - // SetupUDP(randomPort, ipAddress, port); } Participant::Participant(const char* ipAddress, int port) { - this->ipAddress = ipAddress; + this->ipAddress = ipAddress; // maybe this is not needed anymore, keeping it to "0.0.0.0" this->port = port; - - // this->senders.push_back(this); - - // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152; - this->localPort = port; // randomPort; - // SetupUDP(randomPort, ipAddress, port); + this->site = new RemoteParticipant(ipAddress, port); } void Participant::begin() { - SetupUDP(this->localPort, this->ipAddress, this->port); + SetupUDP(this->port, this->ipAddress, this->port); } void Participant::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) { @@ -79,14 +68,11 @@ void Participant::Update(unsigned long currentTimeMs) { if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { ParticipantMsg* msg = new ParticipantMsg(this->networkId); - this->Publish(msg); + if (this->site == nullptr) + this->Publish(msg); + else + this->Send(this->site, msg); delete msg; - // std::cout << this->name << " published ParticipantMsg\n"; - - // for (RemoteParticipant* sender : this->senders) { - // for (Thing* thing : this->things) - // SendThingInfo(sender, thing); - // } this->nextPublishMe = currentTimeMs + this->publishInterval; } @@ -135,7 +121,7 @@ RemoteParticipant* Participant::AddParticipant(const char* ipAddress, int port) #pragma region Send void Participant::SendThingInfo(RemoteParticipant* owner, Thing* thing) { - std::cout << "Send thing info " << thing->id << " \n"; + std::cout << "Send thing info " << (int)thing->id << " \n"; ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); this->Send(owner, thingMsg); delete thingMsg; @@ -145,11 +131,28 @@ void Participant::SendThingInfo(RemoteParticipant* owner, Thing* thing) { ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); this->Send(owner, modelMsg); delete modelMsg; - PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); + PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true); this->Send(owner, poseMsg); delete poseMsg; } +bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) { + int bufferSize = msg->Serialize(this->buffer); + if (bufferSize <= 0) + return true; + +#if defined(_WIN32) || defined(_WIN64) + Windows::Participant* thisWindows = static_cast(this); + return thisWindows->Send(remoteParticipant, bufferSize); +#elif defined(__unix__) || defined(__APPLE__) + Posix::Participant* thisPosix = static_cast(this); + return thisPosix->Send(remoteParticipant, bufferSize); +#elif defined(ARDUINO) + Arduino::Participant* thisArduino = static_cast(this); + return thisArduino->Send(remoteParticipant, bufferSize); +#endif +} + void Participant::PublishThingInfo(Thing* thing) { // std::cout << "Publish thing info" << thing->networkId << "\n"; // Strange, when publishing, the network id is irrelevant, because it is @@ -168,23 +171,6 @@ void Participant::PublishThingInfo(Thing* thing) { delete customMsg; } -bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) { - int bufferSize = msg->Serialize(this->buffer); - if (bufferSize <= 0) - return true; - -#if defined(_WIN32) || defined(_WIN64) - Windows::Participant* thisWindows = static_cast(this); - return thisWindows->Send(remoteParticipant, bufferSize); -#elif defined(__unix__) || defined(__APPLE__) - Posix::Participant* thisPosix = static_cast(this); - return thisPosix->Send(remoteParticipant, bufferSize); -#elif defined(ARDUINO) - Arduino::Participant* thisArduino = static_cast(this); - return thisArduino->Send(remoteParticipant, bufferSize); -#endif -} - bool Participant::Publish(IMessage* msg) { #if defined(_WIN32) || defined(_WIN64) Windows::Participant* thisWindows = static_cast(this); diff --git a/Participant.h b/Participant.h index c863576..31685fd 100644 --- a/Participant.h +++ b/Participant.h @@ -33,7 +33,8 @@ class Participant : public RemoteParticipant { const char* name = "Participant"; - int localPort = 0; + //int localPort = 0; + RemoteParticipant* site = nullptr; #if defined(ARDUINO) const char* remoteIpAddress = nullptr; @@ -55,7 +56,7 @@ class Participant : public RemoteParticipant { #endif Participant(int port = 7681); - Participant(const char* ipAddress, int port); + Participant(const char* ipAddress, int port = 7681); void begin(); bool connected = false;