From db1265a135af236cccd451ffc08ed605082a98d3 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 30 Apr 2025 17:00:01 +0200 Subject: [PATCH] Aliged Participants --- Messages/NetworkIdMsg.cpp | 25 ++++++ Messages/{SiteMsg.h => NetworkIdMsg.h} | 8 +- Messages/SiteMsg.cpp | 25 ------ Participants/ParticipantUDP.cpp | 17 +++-- Participants/ParticipantUDP.h | 102 +++++++++---------------- Participants/SiteServer.cpp | 21 +++-- Participants/SiteServer.h | 18 ++++- 7 files changed, 104 insertions(+), 112 deletions(-) create mode 100644 Messages/NetworkIdMsg.cpp rename Messages/{SiteMsg.h => NetworkIdMsg.h} (82%) delete mode 100644 Messages/SiteMsg.cpp diff --git a/Messages/NetworkIdMsg.cpp b/Messages/NetworkIdMsg.cpp new file mode 100644 index 0000000..40592fc --- /dev/null +++ b/Messages/NetworkIdMsg.cpp @@ -0,0 +1,25 @@ +#include "NetworkIdMsg.h" + +namespace RoboidControl { + +NetworkIdMsg::NetworkIdMsg(const char* buffer) { + this->networkId = buffer[1]; +} + +NetworkIdMsg::NetworkIdMsg(unsigned char networkId) { + this->networkId = networkId; +} + +NetworkIdMsg::~NetworkIdMsg() {} + +unsigned char NetworkIdMsg::Serialize(char* buffer) { +#if defined(DEBUG) + std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl; +#endif + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + return NetworkIdMsg::length; +} + +} // namespace RoboidControl diff --git a/Messages/SiteMsg.h b/Messages/NetworkIdMsg.h similarity index 82% rename from Messages/SiteMsg.h rename to Messages/NetworkIdMsg.h index b6e433f..083439b 100644 --- a/Messages/SiteMsg.h +++ b/Messages/NetworkIdMsg.h @@ -3,7 +3,7 @@ namespace RoboidControl { /// @brief A message communicating the network ID for that participant -class SiteMsg : public IMessage { +class NetworkIdMsg : public IMessage { public: /// @brief The message ID static const unsigned char id = 0xA1; @@ -14,11 +14,11 @@ public: /// @brief Create a new message for sending /// @param networkId The network ID for the participant - SiteMsg(unsigned char networkId); + NetworkIdMsg(unsigned char networkId); /// @copydoc RoboidControl::IMessage::IMessage(char*) - SiteMsg(const char *buffer); + NetworkIdMsg(const char *buffer); /// @brief Destructor for the message - virtual ~SiteMsg(); + virtual ~NetworkIdMsg(); /// @copydoc RoboidControl::IMessage::Serialize virtual unsigned char Serialize(char *buffer) override; diff --git a/Messages/SiteMsg.cpp b/Messages/SiteMsg.cpp deleted file mode 100644 index 598a841..0000000 --- a/Messages/SiteMsg.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "SiteMsg.h" - -namespace RoboidControl { - -SiteMsg::SiteMsg(const char* buffer) { - this->networkId = buffer[1]; -} - -SiteMsg::SiteMsg(unsigned char networkId) { - this->networkId = networkId; -} - -SiteMsg::~SiteMsg() {} - -unsigned char SiteMsg::Serialize(char* buffer) { -#if defined(DEBUG) - std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl; -#endif - unsigned char ix = 0; - buffer[ix++] = this->id; - buffer[ix++] = this->networkId; - return SiteMsg::length; -} - -} // namespace RoboidControl diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index f946a00..cab013d 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -325,8 +325,8 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, Process(sender, msg); delete msg; } break; - case SiteMsg::id: { - SiteMsg* msg = new SiteMsg(this->buffer); + case NetworkIdMsg::id: { + NetworkIdMsg* msg = new NetworkIdMsg(this->buffer); bufferSize -= msg->length; Process(sender, msg); delete msg; @@ -382,7 +382,7 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { #endif } -void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { +void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process SiteMsg " << (int)this->networkId << " -> " << (int)msg->networkId << "\n"; @@ -452,9 +452,9 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { } void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { -#if defined(DEBUG) +#if !defined(DEBUG) std::cout << this->name << ": process PoseMsg [" << (int)this->networkId - << "/" << (int)msg->networkId << "]\n"; + << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; #endif Participant* owner = Participant::GetParticipant(msg->networkId); if (owner == nullptr) @@ -466,7 +466,12 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { if ((msg->poseType & PoseMsg::Pose_Position) != 0) thing->SetPosition(msg->position); - std::cout << "update position for" << (int)thing->id << std::endl; + if ((msg->poseType & PoseMsg::Pose_Orientation) != 0) + thing->SetOrientation(msg->orientation); + if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0) + thing->SetLinearVelocity(msg->linearVelocity); + if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0) + thing->SetAngularVelocity(msg->angularVelocity); } void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 375a3b0..8c2efdf 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -1,13 +1,13 @@ #pragma once #include "Messages/BinaryMsg.h" -#include "Messages/InvestigateMsg.h" #include "Messages/DestroyMsg.h" +#include "Messages/InvestigateMsg.h" #include "Messages/ModelUrlMsg.h" #include "Messages/NameMsg.h" #include "Messages/ParticipantMsg.h" #include "Messages/PoseMsg.h" -#include "Messages/SiteMsg.h" +#include "Messages/NetworkIdMsg.h" #include "Messages/ThingMsg.h" #include "Participant.h" @@ -30,7 +30,8 @@ namespace RoboidControl { constexpr int MAX_SENDER_COUNT = 256; -/// @brief A local participant is the local device which can communicate with +/// @brief A participant using UDP communication +/// A local participant is the local device which can communicate with /// other participants It manages all local things and communcation with other /// participants. Each application has a local participant which is usually /// explicit in the code. An participant can be isolated. In that case it is @@ -42,6 +43,8 @@ constexpr int MAX_SENDER_COUNT = 256; /// RoboidControl::IsolatedParticipant::Isolated(). /// @sa RoboidControl::Thing::Thing() class ParticipantUDP : public Participant { +#pragma region Init + public: /// @brief Create a participant without connecting to a site /// @param port The port on which the participant communicates @@ -52,11 +55,8 @@ class ParticipantUDP : public Participant { /// @brief Create a participant which will try to connect to a site. /// @param ipAddress The IP address of the site /// @param port The port used by the site - ParticipantUDP(const char* ipAddress, - int port = 7681, - int localPort = 7681); - // Note to self: one cannot specify the port used by the local participant - // now!! + /// @param localPort The port used by the local participant + ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); /// @brief Isolated participant is used when the application is run without /// networking @@ -65,7 +65,14 @@ class ParticipantUDP : public Participant { /// @brief True if the participant is running isolated. /// Isolated participants do not communicate with other participants + +#pragma endregion Init + + /// @brief True if the participant is running isolated. + /// Isolated participants do not communicate with other participants bool isIsolated = false; + /// @brief The remote site when this participant is connected to a site + Participant* remoteSite = nullptr; /// The interval in milliseconds for publishing (broadcasting) data on the /// local network @@ -74,19 +81,10 @@ class ParticipantUDP : public Participant { /// @brief The name of the participant const char* name = "ParticipantUDP"; - // int localPort = 0; - - /// @brief The remote site when this participant is connected to a site - Participant* remoteSite = nullptr; - -#if defined(ARDUINO) - // const char* remoteIpAddress = nullptr; - // unsigned short remotePort = 0; - // char* broadcastIpAddress = nullptr; - - // WiFiUDP udp; -#else + protected: + char buffer[1024]; +#if !defined(ARDUINO) #if defined(__unix__) || defined(__APPLE__) int sock; #elif defined(_WIN32) || defined(_WIN64) @@ -94,48 +92,48 @@ class ParticipantUDP : public Participant { sockaddr_in server_addr; sockaddr_in broadcast_addr; #endif - #endif - + public: void begin(); bool connected = false; +#pragma region Update + + public: virtual void Update(unsigned long currentTimeMs = 0) override; + + protected: + unsigned long nextPublishMe = 0; + virtual void UpdateMyThings(unsigned long currentTimeMs); virtual void UpdateOtherThings(unsigned long currentTimeMs); +#pragma endregion Update + +#pragma region Send + void SendThingInfo(Participant* remoteParticipant, Thing* thing); void PublishThingInfo(Thing* thing); bool Send(Participant* remoteParticipant, IMessage* msg); bool Publish(IMessage* msg); +#pragma endregion Send + +#pragma region Receive + +protected: void ReceiveData(unsigned char bufferSize, char* senderIpAddress, unsigned int senderPort); void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant); -// #if defined(NO_STD) -// unsigned char senderCount = 0; -// Participant* senders[MAX_SENDER_COUNT]; -// #else -// std::list senders; -// #endif - - protected: - unsigned long nextPublishMe = 0; - - char buffer[1024]; - void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort); - //Participant* GetParticipant(const char* ipAddress, int port); - // Participant* AddParticipant(const char* ipAddress, int port); - void ReceiveUDP(); virtual void Process(Participant* sender, ParticipantMsg* msg); - virtual void Process(Participant* sender, SiteMsg* msg); + virtual void Process(Participant* sender, NetworkIdMsg* msg); virtual void Process(Participant* sender, InvestigateMsg* msg); virtual void Process(Participant* sender, ThingMsg* msg); virtual void Process(Participant* sender, NameMsg* msg); @@ -143,34 +141,8 @@ class ParticipantUDP : public Participant { virtual void Process(Participant* sender, PoseMsg* msg); virtual void Process(Participant* sender, BinaryMsg* msg); -#if !defined(NO_STD) -// public: -// using ThingConstructor = std::function; +#pragma endregion Receive -// template -// void Register(unsigned char thingType) { -// thingMsgProcessors[thingType] = [](Participant* participant, -// unsigned char networkId, -// unsigned char thingId) { -// return new ThingClass(participant, networkId, thingId); -// }; -// }; - -// template -// void Register2(unsigned char thingType, ThingConstructor f) { -// thingMsgProcessors[thingType] = [f](Participant* participant, -// unsigned char networkId, -// unsigned char thingId) { -// return f(participant, networkId, thingId); -// }; -// }; - -// protected: -// std::unordered_map thingMsgProcessors; - -#endif }; } // namespace RoboidControl diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 39a859b..18b5d6b 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -58,27 +58,26 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { // std::cout << this->name << " received New Client -> " << // sender->ipAddress // << ":" << (int)sender->port << "\n"; - SiteMsg* msg = new SiteMsg(sender->networkId); + NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId); this->Send(sender, msg); delete msg; } } -void SiteServer::Process(Participant* sender, SiteMsg* msg) {} +void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {} void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing* thing = sender->Get(msg->thingId); - if (thing == nullptr) { + if (thing == nullptr) new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); - if (msg->parentId != 0) { - thing->SetParent(Get(msg->parentId)); - if (thing->GetParent() != nullptr) - std::cout << "Could not find parent [" << (int)msg->networkId << "/" - << (int)msg->parentId << "]\n"; - } else - thing->SetParent(nullptr); - } + if (msg->parentId != 0) { + thing->SetParent(Get(msg->parentId)); + if (thing->GetParent() != nullptr) + std::cout << "Could not find parent [" << (int)msg->networkId << "/" + << (int)msg->parentId << "]\n"; + } else + thing->SetParent(nullptr); } #pragma endregion Receive diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index 819b97b..a4e6767 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -12,17 +12,33 @@ namespace RoboidControl { /// @brief A participant is device which can communicate with other participants class SiteServer : public ParticipantUDP { + +#pragma region Init + public: + /// @brief Create a new site server + /// @param port The port of which to receive the messages SiteServer(int port = 7681); +#pragma endregion Init + +#pragma region Update + virtual void UpdateMyThings(unsigned long currentTimeMs) override; +#pragma endregion Update + +#pragma region Receive + protected: unsigned long nextPublishMe = 0; virtual void Process(Participant* sender, ParticipantMsg* msg) override; - virtual void Process(Participant* sender, SiteMsg* msg) override; + virtual void Process(Participant* sender, NetworkIdMsg* msg) override; virtual void Process(Participant* sender, ThingMsg* msg) override; + +#pragma endregion Receive + }; } // namespace RoboidControl