diff --git a/ArduinoUtils.cpp b/Arduino/ArduinoUtils.cpp similarity index 69% rename from ArduinoUtils.cpp rename to Arduino/ArduinoUtils.cpp index ccc85d6..06e8ff8 100644 --- a/ArduinoUtils.cpp +++ b/Arduino/ArduinoUtils.cpp @@ -1,11 +1,42 @@ #include "ArduinoUtils.h" + #if defined(ARDUINO) + #include +#include + +#if defined(ARDUINO_ARCH_ESP8266) #include #include +#elif defined(ESP32) +#include +#include +#endif -bool StartWifi(const char *wifiSsid, const char *wifiPassword, - bool hotspotFallback) { +const char* hotspotSSID = "Roboid"; +const char* hotspotPassword = "alchemy7000"; + +#if ESP32 +// Flash storage +#include "Preferences.h" + +#define PREFERENCES_NAMESPACE "roboidControl" +Preferences wifiPreferences; + +#define STORAGE_KEY_WIFI "rc/wifi" +struct WifiCredentials { + char ssid[32] = "\0"; + char password[32] = "\0"; +} credentials; + +#define STORAGE_KEY_NSS "rc/nss" +struct NssServer { + char ipAddress[16] = "127.0.0.1\0"; + unsigned short port = 7681; +} nssServer; +#endif + +bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallback) { #if UNO_R4 || ARDUINO_ARCH_RP2040 if (WiFi.status() == WL_NO_MODULE) { Serial.println("WiFi not present, WiFiSync is disabled"); @@ -83,10 +114,8 @@ bool StartWifi(const char *wifiSsid, const char *wifiPassword, #if ESP32 printf("Checking credentials in flash\n"); wifiPreferences.begin(PREFERENCES_NAMESPACE); - wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials, - sizeof(credentials)); - if (strcmp(wifiSsid, credentials.ssid) != 0 || - strcmp(wifiPassword, credentials.password) != 0) { + wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials, sizeof(credentials)); + if (strcmp(wifiSsid, credentials.ssid) != 0 || strcmp(wifiPassword, credentials.password) != 0) { printf("Updating credentials in flash..."); const int ssidLen = strlen(wifiSsid); if (ssidLen < 32) { @@ -99,8 +128,7 @@ bool StartWifi(const char *wifiSsid, const char *wifiPassword, memcpy(credentials.password, wifiPassword, pwdLen); credentials.password[pwdLen] = '\0'; } - wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials, - sizeof(credentials)); + wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials, sizeof(credentials)); printf(" completed.\n"); } wifiPreferences.end(); @@ -132,18 +160,26 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) { Serial.println("Preparing to update firmware."); String firmwareURL = url + FIRMWARE_NAME + ".bin"; +#if defined(ESP32) + t_httpUpdate_return ret = httpUpdate.update(client, firmwareURL); +#else t_httpUpdate_return ret = ESPhttpUpdate.update(client, firmwareURL); +#endif switch (ret) { - case HTTP_UPDATE_FAILED: - Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s", - ESPhttpUpdate.getLastError(), - ESPhttpUpdate.getLastErrorString().c_str()); - break; - case HTTP_UPDATE_NO_UPDATES: - Serial.println("HTTP_UPDATE_NO_UPDATES"); - break; - case HTTP_UPDATE_OK: - break; + case HTTP_UPDATE_FAILED: +#if defined(ESP32) + Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s", httpUpdate.getLastError(), + httpUpdate.getLastErrorString().c_str()); +#else + Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s", ESPhttpUpdate.getLastError(), + ESPhttpUpdate.getLastErrorString().c_str()); +#endif + break; + case HTTP_UPDATE_NO_UPDATES: + Serial.println("HTTP_UPDATE_NO_UPDATES"); + break; + case HTTP_UPDATE_OK: + break; } } else { Serial.println("No Firmware update necessary."); diff --git a/ArduinoUtils.h b/Arduino/ArduinoUtils.h similarity index 100% rename from ArduinoUtils.h rename to Arduino/ArduinoUtils.h diff --git a/Arduino/Participant.cpp b/Arduino/Participant.cpp new file mode 100644 index 0000000..e69de29 diff --git a/Arduino/Participant.h b/Arduino/Participant.h new file mode 100644 index 0000000..dc754c2 --- /dev/null +++ b/Arduino/Participant.h @@ -0,0 +1,22 @@ +#pragma once + +#include "../Participant.h" + +namespace Passer { +namespace RoboidControl { +namespace Arduino { + +class Participant : public RoboidControl::Participant { + public: + void Setup(int localPort, const char* remoteIpAddress, int remotePort); + void Receive(); + bool Send(RemoteParticipant* remoteParticipant, int bufferSize); + bool Publish(IMessage* msg); + + protected: + void GetBroadcastAddress(); +}; + +} // namespace Arduino +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 4e5d151..9b5e493 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,14 @@ else() . LinearAlgebra ) - file(GLOB srcs *.cpp Sensors/*.cpp Messages/*.cpp) + file(GLOB srcs + *.cpp + Sensors/*.cpp + Messages/*.cpp + Arduino/*.cpp + Posix/*.cpp + Windows/*.cpp + ) add_library(ControlCore STATIC ${srcs}) enable_testing() diff --git a/DoxyGen/Doxyfile b/DoxyGen/Doxyfile index 0ba0d5c..853c02a 100644 --- a/DoxyGen/Doxyfile +++ b/DoxyGen/Doxyfile @@ -42,7 +42,7 @@ DOXYFILE_ENCODING = UTF-8 # title of most generated pages and in a few other places. # The default value is: My Project. -PROJECT_NAME = "Control Core for C++" +PROJECT_NAME = "Roboid Control for C++" # The PROJECT_NUMBER tag can be used to enter a project or revision number. This # could be handy for archiving the generated documentation or if some version @@ -61,14 +61,14 @@ PROJECT_BRIEF = # pixels and the maximum width should not exceed 200 pixels. Doxygen will copy # the logo to the output directory. -PROJECT_LOGO = //intranet/home/Afbeeldingen/PasserVR/Logos/Logo3NameRight100.png +PROJECT_LOGO = //intranet/home/Afbeeldingen/PasserVR/Logos/PasserLife/PasserLifeLogoLeft_300.png # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path # into which the generated documentation will be written. If a relative path is # entered, it will be relative to the location where doxygen was started. If # left blank the current directory will be used. -OUTPUT_DIRECTORY = //intranet/web/passer_life/apis/ControlCore/Cpp/ +OUTPUT_DIRECTORY = //intranet/web/roboidcontrol_doc/Cpp/ # If the CREATE_SUBDIRS tag is set to YES then doxygen will create up to 4096 # sub-directories (in 2 levels) under the output directory of each output format @@ -777,7 +777,7 @@ MAX_INITIALIZER_LINES = 30 # list will mention the files that were used to generate the documentation. # The default value is: YES. -SHOW_USED_FILES = YES +SHOW_USED_FILES = NO # Set the SHOW_FILES tag to NO to disable the generation of the Files page. This # will remove the Files entry from the Quick Index and from the Folder Tree View @@ -1044,7 +1044,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = +EXCLUDE = ../LinearAlgebra # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded diff --git a/Messages/CustomMsg.cpp b/Messages/BinaryMsg.cpp similarity index 61% rename from Messages/CustomMsg.cpp rename to Messages/BinaryMsg.cpp index 40e91f1..fe8e380 100644 --- a/Messages/CustomMsg.cpp +++ b/Messages/BinaryMsg.cpp @@ -1,9 +1,9 @@ -#include "CustomMsg.h" +#include "BinaryMsg.h" namespace Passer { -namespace Control { +namespace RoboidControl { -CustomMsg::CustomMsg(char *buffer) { +BinaryMsg::BinaryMsg(char *buffer) { unsigned char ix = 1; this->networkId = buffer[ix++]; this->thingId = buffer[ix++]; @@ -12,17 +12,17 @@ CustomMsg::CustomMsg(char *buffer) { // lifetime is shorter than the buffer lifetime... } -CustomMsg::CustomMsg(unsigned char networkId, Thing *thing) { +BinaryMsg::BinaryMsg(unsigned char networkId, Thing *thing) { this->networkId = networkId; this->thingId = thing->id; this->thing = thing; } -CustomMsg::~CustomMsg() {} +BinaryMsg::~BinaryMsg() {} -unsigned char CustomMsg::Serialize(char *buffer) { +unsigned char BinaryMsg::Serialize(char *buffer) { unsigned char ix = this->length; - this->thing->SendBytes(buffer, &ix); + this->thing->GenerateBinary(buffer, &ix); if (ix <= this->length) // in this case, no data is actually sent return 0; @@ -32,10 +32,10 @@ unsigned char CustomMsg::Serialize(char *buffer) { return ix; } -CustomMsg CustomMsg::Receive(char *buffer, unsigned char bufferSize) { - CustomMsg msg = CustomMsg(buffer); +BinaryMsg BinaryMsg::Receive(char *buffer, unsigned char bufferSize) { + BinaryMsg msg = BinaryMsg(buffer); return msg; } -} // namespace Control +} // namespace RoboidControl } // namespace Passer diff --git a/Messages/CustomMsg.h b/Messages/BinaryMsg.h similarity index 57% rename from Messages/CustomMsg.h rename to Messages/BinaryMsg.h index 169562a..987b8a8 100644 --- a/Messages/CustomMsg.h +++ b/Messages/BinaryMsg.h @@ -3,9 +3,9 @@ #include "Messages.h" namespace Passer { -namespace Control { +namespace RoboidControl { -class CustomMsg : public IMessage { +class BinaryMsg : public IMessage { public: static const unsigned char id = 0xB1; static const unsigned length = 3; @@ -17,14 +17,14 @@ public: unsigned char bytesSize; char *bytes = nullptr; - CustomMsg(char *buffer); - CustomMsg(unsigned char networkId, Thing *thing); - virtual ~CustomMsg(); + BinaryMsg(char *buffer); + BinaryMsg(unsigned char networkId, Thing *thing); + virtual ~BinaryMsg(); virtual unsigned char Serialize(char *buffer) override; - static CustomMsg Receive(char *buffer, unsigned char bufferSize); + static BinaryMsg Receive(char *buffer, unsigned char bufferSize); }; -} // namespace Control +} // namespace RoboidControl } // namespace Passer diff --git a/Messages/ClientMsg.cpp b/Messages/ClientMsg.cpp deleted file mode 100644 index 083cb89..0000000 --- a/Messages/ClientMsg.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "ClientMsg.h" - -namespace Passer::Control { - -ClientMsg::ClientMsg(char networkId) { this->networkId = networkId; } - -ClientMsg::ClientMsg(const char *buffer) { this->networkId = buffer[1]; } - -ClientMsg::~ClientMsg() {} - -unsigned char ClientMsg::Serialize(char *buffer) { - unsigned char ix = 0; - buffer[ix++] = this->id; - buffer[ix++] = this->networkId; - return ClientMsg::length; -} - -// bool ClientMsg::Send(Participant *participant, unsigned char networkId) { -// ClientMsg msg = ClientMsg() -// } -// Client Msg - -} // namespace Passer::Control \ No newline at end of file diff --git a/Messages/ClientMsg.h b/Messages/ClientMsg.h deleted file mode 100644 index 4d45566..0000000 --- a/Messages/ClientMsg.h +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -#include "Messages.h" - -namespace Passer { -namespace Control { - -/// @brief A client message announces the presence of a participant -/// When received by another participant, it can be followed by a NetworkIdMsg -/// to announce that participant to this client such that it can join privately -class ClientMsg : public IMessage { -public: - static const unsigned char id = 0xA0; - static const unsigned char length = 2; - unsigned char networkId; - - ClientMsg(char networkId); - ClientMsg(const char *buffer); - virtual ~ClientMsg(); - - virtual unsigned char Serialize(char *buffer) override; -}; - -} // namespace Control -} // namespace Passer \ No newline at end of file diff --git a/Messages/DestroyMsg.cpp b/Messages/DestroyMsg.cpp index ecd1b1a..e2c04ad 100644 --- a/Messages/DestroyMsg.cpp +++ b/Messages/DestroyMsg.cpp @@ -1,13 +1,15 @@ #include "DestroyMsg.h" namespace Passer { -namespace Control { +namespace RoboidControl { DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) { this->networkId = networkId; this->thingId = thing->id; } +DestroyMsg::DestroyMsg(char* buffer) {} + DestroyMsg::~DestroyMsg() {} unsigned char DestroyMsg::Serialize(char *buffer) { @@ -18,5 +20,5 @@ unsigned char DestroyMsg::Serialize(char *buffer) { return ix; } -} // namespace Control +} // namespace RoboidControl } // namespace Passer diff --git a/Messages/DestroyMsg.h b/Messages/DestroyMsg.h index 4d83c17..66a6830 100644 --- a/Messages/DestroyMsg.h +++ b/Messages/DestroyMsg.h @@ -1,19 +1,31 @@ #include "Messages.h" namespace Passer { -namespace Control { +namespace RoboidControl { +/// @brief Message notifiying that a Thing no longer exists class DestroyMsg : public IMessage { public: + /// @brief The message ID static const unsigned char id = 0x20; + /// @brief The length of the message static const unsigned length = 3; + /// @brief The network ID of the thing unsigned char networkId; + /// @brief The ID of the thing unsigned char thingId; + /// @brief Create a message for sending + /// @param networkId The network ID of the thing + /// @param thing The ID of the thing DestroyMsg(unsigned char networkId, Thing *thing); + /// @copydoc Passer::RoboidControl::IMessage::IMessage(char*) + DestroyMsg(char * buffer); + /// @brief Destructor for the message virtual ~DestroyMsg(); + /// @copydoc Passer::RoboidControl::IMessage::Serialize virtual unsigned char Serialize(char *buffer) override; }; -} // namespace Control +} // namespace RoboidControl } // namespace Passer \ No newline at end of file diff --git a/Messages/InvestigateMsg.h b/Messages/InvestigateMsg.h index 21cf98d..09eb0a8 100644 --- a/Messages/InvestigateMsg.h +++ b/Messages/InvestigateMsg.h @@ -1,15 +1,32 @@ #include "Messages.h" +namespace Passer { +namespace RoboidControl { + +/// @brief Message to request details for a Thing class InvestigateMsg : public IMessage { -public: + public: + /// @brief The message ID static const unsigned char id = 0x81; + /// @brief The length of the message static const unsigned char length = 3; + /// @brief The network ID of the thing unsigned char networkId; + /// @brief the ID of the thing unsigned char thingId; - InvestigateMsg(char *buffer); + /// @brief Create a new message for sending + /// @param networkId The network ID for the thing + /// @param thingId The ID of the thing InvestigateMsg(unsigned char networkId, unsigned char thingId); + /// @copydoc Passer::RoboidControl::IMessage::IMessage(char*) + InvestigateMsg(char* buffer); + /// @brief Destructor for the message virtual ~InvestigateMsg(); - virtual unsigned char Serialize(char *buffer) override; + /// @copydoc Passer::RoboidControl::IMessage::Serialize + virtual unsigned char Serialize(char* buffer) override; }; + +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Messages/LowLevelMessages.cpp b/Messages/LowLevelMessages.cpp index 45c34f0..ccce3b4 100644 --- a/Messages/LowLevelMessages.cpp +++ b/Messages/LowLevelMessages.cpp @@ -57,7 +57,7 @@ Spherical16 LowLevelMessages::ReceiveSpherical16(const char *buffer, return s; } -void Passer::Control::LowLevelMessages::SendQuat32(char *buffer, +void Passer::RoboidControl::LowLevelMessages::SendQuat32(char *buffer, unsigned char *ix, SwingTwist16 rotation) { Quaternion q = rotation.ToQuaternion(); diff --git a/Messages/LowLevelMessages.h b/Messages/LowLevelMessages.h index 62ef3fe..0032cf4 100644 --- a/Messages/LowLevelMessages.h +++ b/Messages/LowLevelMessages.h @@ -2,7 +2,7 @@ #include "LinearAlgebra/SwingTwist.h" namespace Passer { -namespace Control { +namespace RoboidControl { class LowLevelMessages { public: @@ -20,6 +20,6 @@ public: static SwingTwist16 ReceiveQuat32(const char *buffer, unsigned char *ix); }; -} // namespace Control +} // namespace RoboidControl } // namespace Passer -using namespace Passer::Control; \ No newline at end of file +using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Messages/Messages.cpp b/Messages/Messages.cpp index dab6b01..147e764 100644 --- a/Messages/Messages.cpp +++ b/Messages/Messages.cpp @@ -1,7 +1,7 @@ #include "Messages.h" #include "LowLevelMessages.h" -// #include "Messages/CustomMsg.h" +// #include "Messages/BinaryMsg.h" #include "Participant.h" #include "string.h" @@ -11,7 +11,11 @@ IMessage::IMessage() {} // IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); } -unsigned char IMessage::Serialize(char *buffer) { return 0; } +IMessage::IMessage(char* buffer) {} + +unsigned char IMessage::Serialize(char* buffer) { + return 0; +} // void IMessage::Deserialize(unsigned char *buffer) {} @@ -20,9 +24,9 @@ unsigned char IMessage::Serialize(char *buffer) { return 0; } // return client->SendBuffer(msg.Serialize(client->buffer)); // } -unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) { - return nullptr; -} +// unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) { +// return nullptr; +// } // bool IMessage::Publish(Participant *participant) { // return participant->PublishBuffer(Serialize(participant->buffer)); diff --git a/Messages/ModelUrlMsg.cpp b/Messages/ModelUrlMsg.cpp index 01cf874..b70f1af 100644 --- a/Messages/ModelUrlMsg.cpp +++ b/Messages/ModelUrlMsg.cpp @@ -3,7 +3,7 @@ #include namespace Passer { -namespace Control { +namespace RoboidControl { // ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId, // unsigned char urlLength, const char *url, @@ -51,5 +51,5 @@ unsigned char ModelUrlMsg::Serialize(char *buffer) { return ix; } -} // namespace Control +} // namespace RoboidControl } // namespace Passer \ No newline at end of file diff --git a/Messages/ModelUrlMsg.h b/Messages/ModelUrlMsg.h index 0cce9f5..0b3f521 100644 --- a/Messages/ModelUrlMsg.h +++ b/Messages/ModelUrlMsg.h @@ -1,25 +1,39 @@ #include "Messages.h" namespace Passer { -namespace Control { +namespace RoboidControl { +/// @brief Message for communicating the URL for a model of the thing class ModelUrlMsg : public IMessage { public: + /// @brief The message ID static const unsigned char id = 0x90; + /// @brief The length of the message without the URL string itself + static const unsigned char length = 3; + /// @brief The network ID of the thing unsigned char networkId; + /// @brief The ID of the thing unsigned char thingId; - - float scale; + + /// @brief The length of the url st5ring, excluding the null terminator unsigned char urlLength; + /// @brief The url of the model, not terminated by a null character const char *url; - ModelUrlMsg(const char *buffer); + /// @brief Create a new message for sending + /// @param networkId The network ID of the thing + /// @param thing The thing for which to send the mode URL ModelUrlMsg(unsigned char networkId, Thing *thing); + /// @copydoc Passer::RoboidControl::IMessage::IMessage(char*) + ModelUrlMsg(const char *buffer); // ModelUrlMsg(unsigned char networkId, unsigned char thingId, // unsigned char urlLegth, const char *url, float scale = 1); + + /// @brief Destructor for the message virtual ~ModelUrlMsg(); + /// @copydoc Passer::RoboidControl::IMessage::Serialize virtual unsigned char Serialize(char *buffer) override; }; diff --git a/Messages/NameMsg.cpp b/Messages/NameMsg.cpp index 198cb03..9f926ce 100644 --- a/Messages/NameMsg.cpp +++ b/Messages/NameMsg.cpp @@ -3,7 +3,17 @@ #include namespace Passer { -namespace Control { +namespace RoboidControl { + +NameMsg::NameMsg(unsigned char networkId, Thing *thing) { + this->networkId = networkId; + this->thingId = thing->id; + if (thing->name == nullptr) + this->nameLength = 0; + else + this->nameLength = strlen(thing->name); + this->name = thing->name; // dangerous! +} NameMsg::NameMsg(const char *buffer) { unsigned char ix = 1; // first byte is msg id @@ -18,16 +28,6 @@ NameMsg::NameMsg(const char *buffer) { this->name = name; } -NameMsg::NameMsg(unsigned char networkId, Thing *thing) { - this->networkId = networkId; - this->thingId = thing->id; - if (thing->name == nullptr) - this->nameLength = 0; - else - this->nameLength = strlen(thing->name); - this->name = thing->name; // dangerous! -} - NameMsg::~NameMsg() { delete[] this->name; } diff --git a/Messages/NameMsg.h b/Messages/NameMsg.h index 58ec135..4a94237 100644 --- a/Messages/NameMsg.h +++ b/Messages/NameMsg.h @@ -1,25 +1,39 @@ #include "Messages.h" namespace Passer { -namespace Control { +namespace RoboidControl { +/// @brief Message for communicating the name of a thing class NameMsg : public IMessage { -public: + public: + /// @brief The message ID static const unsigned char id = 0x91; + /// @brief The length of the message static const unsigned char length = 4; + /// @brief The network ID of the thing unsigned char networkId; + /// @brief The ID of the thing unsigned char thingId; + /// @brief The length of the name, excluding the null terminator unsigned char nameLength; - const char *name; + /// @brief The name of the thing, not terminated with a null character + const char* name; - NameMsg(const char *buffer); - NameMsg(unsigned char networkId, Thing *thing); + /// @brief Create a new message for sending + /// @param networkId The network ID of the thing + /// @param thing The ID of the thing + NameMsg(unsigned char networkId, Thing* thing); // NameMsg(unsigned char networkId, unsigned char thingId, const char *name, // unsigned char nameLength); + + /// @copydoc Passer::RoboidControl::IMessage::IMessage(char*) + NameMsg(const char* buffer); + /// @brief Destructor for the message virtual ~NameMsg(); - virtual unsigned char Serialize(char *buffer) override; + /// @copydoc Passer::RoboidControl::IMessage::Serialize + virtual unsigned char Serialize(char* buffer) override; }; -} // namespace Control -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer diff --git a/Messages/NetworkIdMsg.cpp b/Messages/NetworkIdMsg.cpp index 21ceabc..b133d3c 100644 --- a/Messages/NetworkIdMsg.cpp +++ b/Messages/NetworkIdMsg.cpp @@ -1,7 +1,7 @@ #include "NetworkIdMsg.h" namespace Passer { -namespace Control { +namespace RoboidControl { NetworkIdMsg::NetworkIdMsg(const char *buffer) { this->networkId = buffer[1]; } diff --git a/Messages/NetworkIdMsg.h b/Messages/NetworkIdMsg.h index 21731bb..afc4fb8 100644 --- a/Messages/NetworkIdMsg.h +++ b/Messages/NetworkIdMsg.h @@ -1,20 +1,28 @@ #include "Messages.h" namespace Passer { -namespace Control { +namespace RoboidControl { +/// @brief A message communicating the network ID for that participant class NetworkIdMsg : public IMessage { public: + /// @brief The message ID static const unsigned char id = 0xA1; + /// @brief The length of the message static const unsigned char length = 2; + /// @brief The network ID for the participant unsigned char networkId; - NetworkIdMsg(const char *buffer); + /// @brief Create a new message for sending + /// @param networkId The network ID for the participant NetworkIdMsg(unsigned char networkId); + /// @copydoc Passer::RoboidControl::IMessage::IMessage(char*) + NetworkIdMsg(const char *buffer); + /// @brief Destructor for the message virtual ~NetworkIdMsg(); + /// @copydoc Passer::RoboidControl::IMessage::Serialize virtual unsigned char Serialize(char *buffer) override; - // static NetworkIdMsg Receive(char *buffer, unsigned char bufferSize); }; } // namespace Control diff --git a/Messages/ParticipantMsg.cpp b/Messages/ParticipantMsg.cpp new file mode 100644 index 0000000..e9452ea --- /dev/null +++ b/Messages/ParticipantMsg.cpp @@ -0,0 +1,23 @@ +#include "ParticipantMsg.h" + +namespace Passer::RoboidControl { + +ParticipantMsg::ParticipantMsg(char networkId) { this->networkId = networkId; } + +ParticipantMsg::ParticipantMsg(const char *buffer) { this->networkId = buffer[1]; } + +ParticipantMsg::~ParticipantMsg() {} + +unsigned char ParticipantMsg::Serialize(char *buffer) { + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + return ParticipantMsg::length; +} + +// bool ParticipantMsg::Send(Participant *participant, unsigned char networkId) { +// ParticipantMsg msg = ParticipantMsg() +// } +// Client Msg + +} // namespace Passer::RoboidControl \ No newline at end of file diff --git a/Messages/ParticipantMsg.h b/Messages/ParticipantMsg.h new file mode 100644 index 0000000..df742e9 --- /dev/null +++ b/Messages/ParticipantMsg.h @@ -0,0 +1,36 @@ +#pragma once + +#include "Messages.h" + +namespace Passer { +namespace RoboidControl { + +/// @brief A participant messages notifies other participants of its presence +/// When received by another participant, it can be followed by a NetworkIdMsg +/// to announce that participant to this client such that it can join privately +class ParticipantMsg : public IMessage { + public: + /// @brief The message ID + static const unsigned char id = 0xA0; + /// @brief The length of the message + static const unsigned char length = 2; + /// @brief The network ID known by the participant + unsigned char networkId; + + /// @brief Create a new message for sending + /// @param networkId The network ID known by the participant + ParticipantMsg(char networkId); + + /// @copydoc Passer::RoboidControl::IMessage::IMessage(char*) + ParticipantMsg(const char* buffer); + /// @brief Destructor for the message + virtual ~ParticipantMsg(); + + /// @brief Serialize the message into a byte array + /// @param buffer The buffer to serialize into + /// @return The length of the message in the buffer + virtual unsigned char Serialize(char* buffer) override; +}; + +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Messages/PoseMsg.h b/Messages/PoseMsg.h index a5facd9..ddb2e07 100644 --- a/Messages/PoseMsg.h +++ b/Messages/PoseMsg.h @@ -1,30 +1,66 @@ #include "Messages.h" +namespace Passer { +namespace RoboidControl { + +/// @brief Message to communicate the pose of the thing +/// The pose is in local space relative to the parent. If there is not parent (the thing is a root thing), the pose will +/// be in world space. class PoseMsg : public IMessage { -public: + public: + /// @brief The message ID static const unsigned char id = 0x10; + /// @brief The length of the message unsigned char length = 4 + 4 + 4; + /// @brief The network ID of the thing unsigned char networkId; + /// @brief The ID of the thing unsigned char thingId; + /// @brief Bit pattern stating which pose components are available unsigned char poseType; + /// @brief Bit pattern for a pose with position static const unsigned char Pose_Position = 0x01; + /// @brief Bit pattern for a pose with orientation static const unsigned char Pose_Orientation = 0x02; - static const unsigned char Pose_LinearVelocity = 0x04; // For future use - static const unsigned char Pose_AngularVelocity = 0x08; // For future use + /// @brief Bit pattern for a pose with linear velocity + static const unsigned char Pose_LinearVelocity = 0x04; + /// @brief Bit pattern for a pose with angular velocity + static const unsigned char Pose_AngularVelocity = 0x08; + /// @brief The position of the thing in local space in meters Spherical16 position; + /// @brief The orientation of the thing in local space SwingTwist16 orientation; + /// @brief The linear velocity of the thing in local space in meters per second Spherical16 linearVelocity; + /// @brief The angular velocity of the thing in local space Spherical16 angularVelocity; - PoseMsg(unsigned char networkId, unsigned char thingId, - unsigned char poseType, Spherical16 position, - SwingTwist16 orientation, Spherical16 linearVelocity = Spherical16(), + /// @brief Create a new message for sending + /// @param networkId The network ID of the thing + /// @param thingId The ID of the thing + /// @param poseType Bit pattern stating which pose components are available + /// @param position The position of the thing in local space in meters + /// @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(const char *buffer); + /// @copydoc Passer::RoboidControl::IMessage::IMessage(char*) + PoseMsg(const char* buffer); + /// @brief Destructor for the message virtual ~PoseMsg(); - virtual unsigned char Serialize(char *buffer) override; + /// @copydoc Passer::RoboidControl::IMessage::Serialize + virtual unsigned char Serialize(char* buffer) override; }; + +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Messages/TextMsg.cpp b/Messages/TextMsg.cpp new file mode 100644 index 0000000..854a821 --- /dev/null +++ b/Messages/TextMsg.cpp @@ -0,0 +1,37 @@ +#include "TextMsg.h" + +namespace Passer { +namespace RoboidControl { + +TextMsg::TextMsg(const char* text, unsigned char textLength) { + this->text = text; + this->textLength = textLength; +} + +TextMsg::TextMsg(char* buffer) { + unsigned char ix = 1; // first byte is msg id + + this->textLength = buffer[ix++]; + char* text = new char[this->textLength + 1]; + for (int i = 0; i < this->textLength; i++) + text[i] = buffer[ix++]; + text[this->textLength] = '\0'; + this->text = text; +} + +TextMsg::~TextMsg() {} + +unsigned char TextMsg::Serialize(char* buffer) { + if (this->textLength == 0 || this->text == nullptr) + return 0; + + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->textLength; + for (int nameIx = 0; nameIx < this->textLength; nameIx++) + buffer[ix++] = this->text[nameIx]; + + return ix;} + +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Messages/TextMsg.h b/Messages/TextMsg.h new file mode 100644 index 0000000..dcb6c85 --- /dev/null +++ b/Messages/TextMsg.h @@ -0,0 +1,35 @@ +#include "Messages.h" + +namespace Passer { +namespace RoboidControl { + +/// @brief Message for sending generic text +class TextMsg : public IMessage { + public: + /// @brief The message ID + static const unsigned char id = 0xB0; + /// @brief The length of the message without the text itself + static const unsigned char length = 2; + /// @brief The network ID of the thing + unsigned char networkId; + /// @brief the ID of the thing + unsigned char thingId; + /// @brief The text without the null terminator + const char* text; + /// @brief The length of the text + unsigned char textLength; + + /// @brief Create a new message for sending + /// @param text The text + TextMsg(const char* text, unsigned char textLength); + /// @copydoc Passer::RoboidControl::IMessage::IMessage(char*) + TextMsg(char* buffer); + /// @brief Destructor for the message + virtual ~TextMsg(); + + /// @copydoc Passer::RoboidControl::IMessage::Serialize + virtual unsigned char Serialize(char* buffer) override; +}; + +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Messages/ThingMsg.cpp b/Messages/ThingMsg.cpp index d0d5e2b..5242256 100644 --- a/Messages/ThingMsg.cpp +++ b/Messages/ThingMsg.cpp @@ -1,7 +1,7 @@ #include "ThingMsg.h" namespace Passer { -namespace Control { +namespace RoboidControl { ThingMsg::ThingMsg(const char *buffer) { unsigned char ix = 1; // first byte is msg id diff --git a/Messages/ThingMsg.h b/Messages/ThingMsg.h index 4fcdf56..0a6d006 100644 --- a/Messages/ThingMsg.h +++ b/Messages/ThingMsg.h @@ -1,25 +1,39 @@ #include "Messages.h" namespace Passer { -namespace Control { +namespace RoboidControl { +/// @brief Message providing generic information about a Thing class ThingMsg : public IMessage { -public: + public: + /// @brief The message ID static const unsigned char id = 0x80; + /// @brief The length of the message static const unsigned char length = 5; + /// @brief The network ID of the thing unsigned char networkId; + /// @brief The ID of the thing unsigned char thingId; + /// @brief The Thing.Type of the thing unsigned char thingType; + /// @brief The parent of the thing in the hierarachy. This is null for root Things unsigned char parentId; - ThingMsg(const char *buffer); - ThingMsg(unsigned char networkId, Thing *thing); + /// @brief Create a message for sending + /// @param networkId The network ID of the thing + /// @param thing The thing + ThingMsg(unsigned char networkId, Thing* thing); // ThingMsg(unsigned char networkId, unsigned char thingId, // unsigned char thingType, unsigned char parentId); + + /// @copydoc Passer::RoboidControl::IMessage::IMessage(char*) + ThingMsg(const char* buffer); + /// @brief Destructor for the message virtual ~ThingMsg(); - virtual unsigned char Serialize(char *buffer) override; + /// @copydoc Passer::RoboidControl::IMessage::Serialize + virtual unsigned char Serialize(char* buffer) override; }; -} // namespace Control -} // namespace Passer \ No newline at end of file +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Participant.cpp b/Participant.cpp index 91968a6..324a3bc 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -2,9 +2,9 @@ #include "Thing.h" -#include "UdpArduino.h" -#include "UdpPosix.h" -#include "UdpWindows.h" +#include "Arduino/Participant.h" +#include "Posix/Participant.h" +#include "Windows/Participant.h" #if defined(_WIN32) || defined(_WIN64) #include @@ -12,15 +12,15 @@ #pragma comment(lib, "ws2_32.lib") #elif defined(__unix__) || defined(__APPLE__) #include -#include -#include // For fcntl +#include // For fcntl #include #include #include +#include #endif namespace Passer { -namespace Control { +namespace RoboidControl { Participant::Participant() {} @@ -28,6 +28,7 @@ Participant::Participant(int port) { this->ipAddress = "0.0.0.0"; this->port = port; + this->senders.push_back(this); this->senders.push_back(this); // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152; @@ -35,14 +36,14 @@ Participant::Participant(int port) { // SetupUDP(randomPort, ipAddress, port); } -Participant::Participant(const char *ipAddress, int port) { +Participant::Participant(const char* ipAddress, int port) { this->ipAddress = ipAddress; this->port = port; this->senders.push_back(this); // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152; - this->localPort = port; // randomPort; + this->localPort = port; // randomPort; // SetupUDP(randomPort, ipAddress, port); } @@ -50,16 +51,15 @@ void Participant::begin() { SetupUDP(this->localPort, this->ipAddress, this->port); } -void Participant::SetupUDP(int localPort, const char *remoteIpAddress, - int remotePort) { +void Participant::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(_WIN32) || defined(_WIN64) - UdpWindows *thisWindows = static_cast(this); + Windows::Participant* thisWindows = static_cast(this); thisWindows->Setup(localPort, remoteIpAddress, remotePort); #elif defined(__unix__) || defined(__APPLE__) - UdpPosix *thisPosix = static_cast(this); + Posix::Participant* thisPosix = static_cast(this); thisPosix->Setup(localPort, remoteIpAddress, remotePort); #elif defined(ARDUINO) - UdpArduino *thisArduino = static_cast(this); + Arduino::Participant* thisArduino = static_cast(this); thisArduino->Setup(localPort, remoteIpAddress, remotePort); #endif this->connected = true; @@ -71,8 +71,7 @@ void Participant::Update(unsigned long currentTimeMs) { currentTimeMs = millis(); #elif defined(__unix__) || defined(__APPLE__) auto now = std::chrono::steady_clock::now(); - auto ms = std::chrono::duration_cast( - now.time_since_epoch()); + auto ms = std::chrono::duration_cast(now.time_since_epoch()); currentTimeMs = static_cast(ms.count()); #endif } @@ -81,41 +80,44 @@ void Participant::Update(unsigned long currentTimeMs) { begin(); if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { - ClientMsg *msg = new ClientMsg(this->networkId); + ParticipantMsg* msg = new ParticipantMsg(this->networkId); this->Publish(msg); delete msg; - std::cout << this->name << " published ClientMsg\n"; + std::cout << this->name << " published ParticipantMsg\n"; this->nextPublishMe = currentTimeMs + this->publishInterval; } this->ReceiveUDP(); - this->UpdateAll(currentTimeMs); + for (Thing* thing : this->things) { + if (thing != nullptr) //} && thing->GetParent() == nullptr) // update all root things + thing->Update(currentTimeMs); + } } void Participant::ReceiveUDP() { #if defined(_WIN32) || defined(_WIN64) - UdpWindows *thisWindows = static_cast(this); + Windows::Participant* thisWindows = static_cast(this); thisWindows->Receive(); #elif defined(__unix__) || defined(__APPLE__) - UdpPosix *thisPosix = static_cast(this); + Posix::Participant* thisPosix = static_cast(this); thisPosix->Receive(); #elif defined(ARDUINO) - UdpArduino *thisArduino = static_cast(this); + Arduino::Participant* thisArduino = static_cast(this); thisArduino->Receive(); #endif } -Participant *Participant::GetParticipant(const char *ipAddress, int port) { - for (Participant *sender : this->senders) { +Participant* Participant::GetParticipant(const char* ipAddress, int port) { + for (Participant* sender : this->senders) { if (sender->ipAddress == ipAddress && sender->port == port) return sender; } return nullptr; } -Participant *Participant::AddParticipant(const char *ipAddress, int port) { +Participant* Participant::AddParticipant(const char* ipAddress, int port) { std::cout << "New Participant " << ipAddress << ":" << port << "\n"; - Participant *participant = new Participant(ipAddress, port); + Participant* participant = new Participant(ipAddress, port); participant->networkId = (unsigned char)this->senders.size(); this->senders.push_back(participant); return participant; @@ -123,64 +125,63 @@ Participant *Participant::AddParticipant(const char *ipAddress, int port) { #pragma region Send -void Participant::SendThingInfo(RemoteParticipant *remoteParticipant, - Thing *thing) { +void Participant::SendThingInfo(RemoteParticipant* remoteParticipant, Thing* thing) { std::cout << "Send thing info\n"; - ThingMsg *thingMsg = new ThingMsg(this->networkId, thing); + ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); this->Send(remoteParticipant, thingMsg); delete thingMsg; - NameMsg *nameMsg = new NameMsg(this->networkId, thing); + NameMsg* nameMsg = new NameMsg(this->networkId, thing); this->Send(remoteParticipant, nameMsg); delete nameMsg; - ModelUrlMsg *modelMsg = new ModelUrlMsg(this->networkId, thing); + ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); this->Send(remoteParticipant, modelMsg); delete modelMsg; } -void Passer::Control::Participant::PublishThingInfo(Thing *thing) { +void Passer::RoboidControl::Participant::PublishThingInfo(Thing* thing) { // std::cout << "Publish thing info" << thing->networkId << "\n"; // Strange, when publishing, the network id is irrelevant, because it is // connected to a specific site... - ThingMsg *thingMsg = new ThingMsg(this->networkId, thing); + ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); this->Publish(thingMsg); delete thingMsg; - NameMsg *nameMsg = new NameMsg(this->networkId, thing); + NameMsg* nameMsg = new NameMsg(this->networkId, thing); this->Publish(nameMsg); delete nameMsg; - ModelUrlMsg *modelMsg = new ModelUrlMsg(this->networkId, thing); + ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); this->Publish(modelMsg); delete modelMsg; - CustomMsg *customMsg = new CustomMsg(this->networkId, thing); + BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing); this->Publish(customMsg); delete customMsg; } -bool Participant::Send(RemoteParticipant *remoteParticipant, IMessage *msg) { +bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) { int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) return true; #if defined(_WIN32) || defined(_WIN64) - UdpWindows *thisWindows = static_cast(this); + Windows::Participant* thisWindows = static_cast(this); return thisWindows->Send(remoteParticipant, bufferSize); #elif defined(__unix__) || defined(__APPLE__) - UdpPosix *thisPosix = static_cast(this); + Posix::Participant* thisPosix = static_cast(this); return thisPosix->Send(remoteParticipant, bufferSize); #elif defined(ARDUINO) - UdpArduino *thisArduino = static_cast(this); + Arduino::Participant* thisArduino = static_cast(this); return thisArduino->Send(remoteParticipant, bufferSize); #endif } -bool Participant::Publish(IMessage *msg) { +bool Participant::Publish(IMessage* msg) { #if defined(_WIN32) || defined(_WIN64) - UdpWindows *thisWindows = static_cast(this); + Windows::Participant* thisWindows = static_cast(this); return thisWindows->Publish(msg); #elif defined(__unix__) || defined(__APPLE__) - UdpPosix *thisPosix = static_cast(this); + Posix::Participant* thisPosix = static_cast(this); return thisPosix->Publish(msg); #elif defined(ARDUINO) - UdpArduino *thisArduino = static_cast(this); + Arduino::Participant* thisArduino = static_cast(this); return thisArduino->Publish(msg); #endif } @@ -190,93 +191,89 @@ bool Participant::Publish(IMessage *msg) { #pragma region Receive -void Participant::ReceiveData(unsigned char bufferSize, - RemoteParticipant *remoteParticipant) { +void Participant::ReceiveData(unsigned char bufferSize, RemoteParticipant* remoteParticipant) { unsigned char msgId = this->buffer[0]; // std::cout << "receive msg " << (int)msgId << "\n"; switch (msgId) { - case ClientMsg::id: { - ClientMsg *msg = new ClientMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case NetworkIdMsg::id: { - NetworkIdMsg *msg = new NetworkIdMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case InvestigateMsg::id: { - InvestigateMsg *msg = new InvestigateMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case ThingMsg::id: { - ThingMsg *msg = new ThingMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case NameMsg::id: { - NameMsg *msg = new NameMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case PoseMsg::id: { - PoseMsg *msg = new PoseMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; - case CustomMsg::id: { - CustomMsg *msg = new CustomMsg(this->buffer); - Process(remoteParticipant, msg); - delete msg; - } break; + case ParticipantMsg::id: { + ParticipantMsg* msg = new ParticipantMsg(this->buffer); + Process(remoteParticipant, msg); + delete msg; + } break; + case NetworkIdMsg::id: { + NetworkIdMsg* msg = new NetworkIdMsg(this->buffer); + Process(remoteParticipant, msg); + delete msg; + } break; + case InvestigateMsg::id: { + InvestigateMsg* msg = new InvestigateMsg(this->buffer); + Process(remoteParticipant, msg); + delete msg; + } break; + case ThingMsg::id: { + ThingMsg* msg = new ThingMsg(this->buffer); + Process(remoteParticipant, msg); + delete msg; + } break; + case NameMsg::id: { + NameMsg* msg = new NameMsg(this->buffer); + Process(remoteParticipant, msg); + delete msg; + } break; + case PoseMsg::id: { + PoseMsg* msg = new PoseMsg(this->buffer); + Process(remoteParticipant, msg); + delete msg; + } break; + case BinaryMsg::id: { + BinaryMsg* msg = new BinaryMsg(this->buffer); + Process(remoteParticipant, msg); + delete msg; + } break; }; } -void Participant::Process(RemoteParticipant *sender, ClientMsg *msg) {} +void Participant::Process(RemoteParticipant* sender, ParticipantMsg* msg) {} -void Participant::Process(RemoteParticipant *sender, NetworkIdMsg *msg) { - std::cout << this->name << ": process NetworkId [" << (int)this->networkId - << "/" << (int)msg->networkId << "]\n"; +void Participant::Process(RemoteParticipant* sender, NetworkIdMsg* msg) { + std::cout << this->name << ": process NetworkId [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n"; if (this->networkId != msg->networkId) { this->networkId = msg->networkId; - for (Thing *thing : this->things) + for (Thing* thing : this->things) this->SendThingInfo(sender, thing); } } -void Participant::Process(RemoteParticipant *sender, InvestigateMsg *msg) {} +void Participant::Process(RemoteParticipant* sender, InvestigateMsg* msg) {} -void Participant::Process(RemoteParticipant *sender, ThingMsg *msg) {} +void Participant::Process(RemoteParticipant* sender, ThingMsg* msg) {} -void Participant::Process(RemoteParticipant *sender, NameMsg *msg) { - Thing *thing = sender->Get(msg->networkId, msg->thingId); +void Participant::Process(RemoteParticipant* sender, NameMsg* msg) { + Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) { int nameLength = msg->nameLength; - char *thingName = new char[nameLength + 1]; + char* thingName = new char[nameLength + 1]; strcpy(thingName, msg->name); thingName[nameLength] = '\0'; thing->name = thingName; - std::cout << "thing name = " << thing->name << " length = " << nameLength - << "\n"; + std::cout << "thing name = " << thing->name << " length = " << nameLength << "\n"; } } -void Participant::Process(RemoteParticipant *sender, PoseMsg *msg) {} +void Participant::Process(RemoteParticipant* sender, PoseMsg* msg) {} -void Participant::Process(RemoteParticipant *sender, CustomMsg *msg) { +void Participant::Process(RemoteParticipant* sender, BinaryMsg* msg) { // std::cout << this->name << ": process Binary [" << (int)this->networkId << "/" // << (int)msg->networkId << "]\n"; - Thing *thing = sender->Get(msg->networkId, msg->thingId); + Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) - thing->ProcessBytes(msg->bytes); + thing->ProcessBinary(msg->bytes); else - std::cout << "custom msg for unknown thing " << (int)msg->networkId << ":" - << (int)msg->thingId << "\n"; + std::cout << "custom msg for unknown thing " << (int)msg->networkId << ":" << (int)msg->thingId << "\n"; } // Receive #pragma endregion -} // namespace Control -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer diff --git a/Participant.h b/Participant.h index 913467e..47a355d 100644 --- a/Participant.h +++ b/Participant.h @@ -1,8 +1,8 @@ #pragma once //#include "Messages/" -#include "Messages/ClientMsg.h" -#include "Messages/CustomMsg.h" +#include "Messages/ParticipantMsg.h" +#include "Messages/BinaryMsg.h" #include "Messages/InvestigateMsg.h" #include "Messages/ModelUrlMsg.h" #include "Messages/NameMsg.h" @@ -25,12 +25,12 @@ #endif namespace Passer { -namespace Control { +namespace RoboidControl { /// @brief A participant is device which can communicate with other participants class Participant : public RemoteParticipant { public: - char buffer[1024]; + unsigned char buffer[1024]; long publishInterval = 3000; // 3 seconds // unsigned char networkId = 0; @@ -94,15 +94,15 @@ protected: void ReceiveUDP(); - virtual void Process(RemoteParticipant *sender, ClientMsg *msg); + virtual void Process(RemoteParticipant *sender, ParticipantMsg *msg); virtual void Process(RemoteParticipant *sender, NetworkIdMsg *msg); virtual void Process(RemoteParticipant* sender, InvestigateMsg *msg); virtual void Process(RemoteParticipant* sender, ThingMsg *msg); virtual void Process(RemoteParticipant* sender, NameMsg *msg); virtual void Process(RemoteParticipant* sender, PoseMsg *msg); - virtual void Process(RemoteParticipant* sender, CustomMsg *msg); + virtual void Process(RemoteParticipant* sender, BinaryMsg *msg); }; } // namespace Control } // namespace Passer -using namespace Passer::Control; \ No newline at end of file +using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Posix/Participant.cpp b/Posix/Participant.cpp new file mode 100644 index 0000000..e69de29 diff --git a/Posix/Participant.h b/Posix/Participant.h new file mode 100644 index 0000000..295b495 --- /dev/null +++ b/Posix/Participant.h @@ -0,0 +1,19 @@ +#pragma once + +#include "../Participant.h" + +namespace Passer { +namespace RoboidControl { +namespace Posix { + +class Participant : public RoboidControl::Participant { + public: + void Setup(int localPort, const char* remoteIpAddress, int remotePort); + void Receive(); + bool Send(RemoteParticipant* remoteParticipant, int bufferSize); + bool Publish(IMessage* msg); +}; + +} // namespace Posix +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/README.md b/README.md index c1d45a6..ac14c63 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,14 @@ -\mainpage Control Core for C++ +\mainpage Roboid Control for C++ -Control Core contains generic functionality for Controlling Things. \ No newline at end of file +Roboid Control support for C++ applications. +Supporting: +- Windows +- MacOS +- Linux +- Arduino (using PlatformIO) + +# Basic components + +- Passer::RoboidControl::Thing +- Passer::RoboidControl::Participant +- Passer::RoboidControl::SiteServer \ No newline at end of file diff --git a/Sensors/DigitalSensor.cpp b/Sensors/DigitalSensor.cpp new file mode 100644 index 0000000..de21a15 --- /dev/null +++ b/Sensors/DigitalSensor.cpp @@ -0,0 +1,11 @@ +#include "DigitalSensor.h" + +namespace Passer { +namespace RoboidControl { + +DigitalSensor::DigitalSensor() {} + +DigitalSensor::DigitalSensor(unsigned char networkId, unsigned char thingId) {} + +} // namespace RoboidControl +} // namespace Passer diff --git a/Sensors/DigitalSensor.h b/Sensors/DigitalSensor.h new file mode 100644 index 0000000..8328b33 --- /dev/null +++ b/Sensors/DigitalSensor.h @@ -0,0 +1,23 @@ +#pragma once + +#include "Thing.h" + +namespace Passer { +namespace RoboidControl { + +/// @brief A digital (on/off, 1/0, true/false) sensor +class DigitalSensor : public Thing { + public: + /// @brief The sigital state + bool state = 0; + + /// @brief The default constructor + DigitalSensor(); + /// @brief Create a temperature sensor with the given ID + /// @param networkId The network ID of the sensor + /// @param thingId The ID of the thing + DigitalSensor(unsigned char networkId, unsigned char thingId); +}; + +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Sensors/TemperatureSensor.cpp b/Sensors/TemperatureSensor.cpp index 0792e60..ad63935 100644 --- a/Sensors/TemperatureSensor.cpp +++ b/Sensors/TemperatureSensor.cpp @@ -3,7 +3,7 @@ #include "Messages/LowLevelMessages.h" namespace Passer { -namespace Control { +namespace RoboidControl { // TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {} @@ -13,16 +13,16 @@ TemperatureSensor::TemperatureSensor(unsigned char networkId, unsigned char thingId) : Thing(nullptr, networkId, thingId, Type::TemperatureSensor) {} -void TemperatureSensor::SetTemperature(float temp) { this->temp = temp; } +void TemperatureSensor::SetTemperature(float temp) { this->temperature = temp; } -void TemperatureSensor::SendBytes(char *buffer, unsigned char *ix) { - std::cout << "Send temperature: " << this->temp << "\n"; - LowLevelMessages::SendFloat16(buffer, ix, this->temp); +void TemperatureSensor::GenerateBinary(char *buffer, unsigned char *ix) { + std::cout << "Send temperature: " << this->temperature << "\n"; + LowLevelMessages::SendFloat16(buffer, ix, this->temperature); } -void TemperatureSensor::ProcessBytes(char *bytes) { +void TemperatureSensor::ProcessBinary(char *bytes) { unsigned char ix = 0; - this->temp = LowLevelMessages::ReceiveFloat16(bytes, &ix); + this->temperature = LowLevelMessages::ReceiveFloat16(bytes, &ix); } } // namespace Control diff --git a/Sensors/TemperatureSensor.h b/Sensors/TemperatureSensor.h index d4f5142..8ce8284 100644 --- a/Sensors/TemperatureSensor.h +++ b/Sensors/TemperatureSensor.h @@ -3,21 +3,33 @@ #include "Thing.h" namespace Passer { -namespace Control { +namespace RoboidControl { +/// @brief A temperature sensor class TemperatureSensor : public Thing { -public: + public: + /// @brief The measured temperature + float temperature = 0; + + /// @brief The default constructor TemperatureSensor(); + /// @brief Create a temperature sensor with the given ID + /// @param networkId The network ID of the sensor + /// @param thingId The ID of the thing TemperatureSensor(unsigned char networkId, unsigned char thingId); - virtual void SetTemperature(float temp); + /// @brief Manually override the measured temperature + /// @param temperature The new temperature + virtual void SetTemperature(float temperature); - void SendBytes(char *buffer, unsigned char *ix) override; - virtual void ProcessBytes(char *bytes) override; - -protected: - float temp = 0; + /// @brief Function to create a binary message with the temperature + /// @param buffer The byte array for thw binary data + /// @param ix The starting position for writing the binary data + void GenerateBinary(char* bytes, unsigned char* ix) override; + /// @brief Function to extract the temperature received in the binary message + /// @param bytes The binary data + virtual void ProcessBinary(char* bytes) override; }; -} // namespace Control -} // namespace Passer \ No newline at end of file +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/SiteServer.cpp b/SiteServer.cpp index 1bb7bb1..6510a65 100644 --- a/SiteServer.cpp +++ b/SiteServer.cpp @@ -6,7 +6,7 @@ #include namespace Passer { -namespace Control { +namespace RoboidControl { SiteServer::SiteServer(int port) { this->name = "Site Server"; @@ -22,7 +22,7 @@ SiteServer::SiteServer(int port) { Register((unsigned char)Thing::Type::TemperatureSensor); } -void SiteServer::Process(RemoteParticipant *sender, ClientMsg *msg) { +void SiteServer::Process(RemoteParticipant *sender, ParticipantMsg *msg) { if (msg->networkId == 0) { std::cout << this->name << " received New Client -> " << sender->ipAddress << " " << (int)sender->networkId << "\n"; diff --git a/SiteServer.h b/SiteServer.h index 0500eb6..c9091b8 100644 --- a/SiteServer.h +++ b/SiteServer.h @@ -7,7 +7,7 @@ #include namespace Passer { -namespace Control { +namespace RoboidControl { /// @brief A participant is device which can communicate with other participants class SiteServer : public Participant { @@ -26,7 +26,7 @@ public: protected: unsigned long nextPublishMe = 0; - virtual void Process(RemoteParticipant *sender, ClientMsg *msg) override; + virtual void Process(RemoteParticipant *sender, ParticipantMsg *msg) override; virtual void Process(RemoteParticipant *sender, NetworkIdMsg *msg) override; virtual void Process(RemoteParticipant* sender, ThingMsg *msg) override; @@ -37,4 +37,4 @@ protected: } // namespace Control } // namespace Passer -using namespace Passer::Control; \ No newline at end of file +using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Thing.cpp b/Thing.cpp index bba3a7b..b1c2266 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -6,6 +6,9 @@ #include #include +namespace Passer { + namespace RoboidControl { + Thing::Thing(Type thingType) : Thing((unsigned char)thingType) {} Thing::Thing(unsigned char thingType) { @@ -20,7 +23,7 @@ Thing::Thing(unsigned char thingType) { this->angularVelocity = Spherical16::zero; } -Passer::Control::Thing::Thing(RemoteParticipant *participant, unsigned char networkId, +Thing::Thing(RemoteParticipant *participant, unsigned char networkId, unsigned char thingId, Type thingType) { // no participant reference yet.. this->participant = participant; @@ -121,7 +124,7 @@ Thing *Thing::RemoveChild(Thing *child) { return child; } -Thing *Passer::Control::Thing::GetChild(unsigned char id, bool recursive) { +Thing *Thing::GetChild(unsigned char id, bool recursive) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { Thing *child = this->children[childIx]; if (child == nullptr) @@ -138,12 +141,22 @@ Thing *Passer::Control::Thing::GetChild(unsigned char id, bool recursive) { return nullptr; } -Thing *Passer::Control::Thing::GetChildByIndex(unsigned char ix) { - return this->children[ix]; -} +Thing *Thing::GetChildByIndex(unsigned char ix) { return this->children[ix]; } void Thing::SetModel(const char *url) { this->modelUrl = url; } +#if defined(ARDUINO) +void Thing::Update() { + Update(millis()); +} +#endif + +void Thing::GenerateBinary(char *buffer, unsigned char *ix) { + (void)buffer; + (void)ix; +} +void Thing::ProcessBinary(char *bytes) { (void)bytes; }; + void Thing::SetPosition(Spherical16 position) { this->position = position; this->positionUpdated = true; @@ -206,4 +219,5 @@ Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; } // thing->Update(currentTimeMs); // } // } -//} \ No newline at end of file +//} + }} \ No newline at end of file diff --git a/Thing.h b/Thing.h index 1dd5e56..97845e4 100644 --- a/Thing.h +++ b/Thing.h @@ -5,7 +5,7 @@ #include namespace Passer { -namespace Control { +namespace RoboidControl { class RemoteParticipant; @@ -13,15 +13,15 @@ class RemoteParticipant; // IMPORTANT: values higher than 256 will need to change the Thing::id type // to 16-bit or higher, breaking the networking protocol! -/// @brief A thing is the basic building block +/// @brief A thing is the primitive building block class Thing { public: RemoteParticipant *participant; unsigned char networkId = 0; - /// @char The id of the thing + /// @brief The ID of the thing unsigned char id = 0; - /// @brief Basic Thing types + /// @brief Predefined thing types enum class Type { Undetermined, // Sensor, @@ -38,14 +38,27 @@ public: Humanoid, ExternalSensor, }; + /// @brief The type of Thing + unsigned char type = 0; + /// @brief Create a new thing of the given type + /// @param thingType The predefined type of thing Thing(Type thingType = Type::Undetermined); + /// @brief Create a new thing of the give type + /// @param thingType The custom type of the thing Thing(unsigned char thingType); + /// @brief Create a new thing for the given participant + /// @param participant The participant for which this thing is created + /// @param networkId The network ID of the thing + /// @param thingId The ID of the thing + /// @param thingType The type of thing Thing(RemoteParticipant *participant, unsigned char networkId, unsigned char thingId, Type thingType = Type::Undetermined); + /// @brief Find a thing by name + /// @param name Rhe name of the thing + /// @return The found thing or nullptr when nothing is found Thing *FindThing(const char *name); - // Thing *FindChild(unsigned char id); /// @brief Sets the parent Thing /// @param parent The Thing which should become the parnet @@ -60,10 +73,21 @@ public: /// @param child The Thing which should become a child /// @remark When the Thing is already a child, it will not be added again virtual void AddChild(Thing *child); + /// @brief Remove the given thing as a child of this thing + /// @param child The child to remove + /// @return The removed child or nullptr if the child could not be found Thing *RemoveChild(Thing *child); + /// @brief The number of children unsigned char childCount = 0; + /// @brief Get a child by thing Id + /// @param id The thing ID to find + /// @param recursive Look recursively through all descendants + /// @return The found thing of nullptr when nothing is found Thing *GetChild(unsigned char id, bool recursive = false); + /// @brief Get a child by index + /// @param ix The child index + /// @return The found thing of nullptr when nothing is found Thing *GetChildByIndex(unsigned char ix); protected: @@ -71,20 +95,31 @@ protected: Thing **children = nullptr; public: - /// @brief The type of Thing - unsigned char type = 0; + /// @brief The name of the thing const char *name = nullptr; + /// @brief An URL pointing to the location where a model of the thing can be found const char *modelUrl = nullptr; + /// @brief The scale of the model (deprecated I think) float modelScale = 1; - // protected Sensor sensor; + /// @brief Set the position of the thing + /// @param position The new position in local space, in meters void SetPosition(Spherical16 position); + /// @brief Get the position of the thing + /// @return The position in local space, in meters Spherical16 GetPosition(); + /// @brief Set the orientation of the thing + /// @param orientation The new orientation in local space void SetOrientation(SwingTwist16 orientation); + /// @brief Get the orientation of the thing + /// @return The orienation in local space SwingTwist16 GetOrientation(); + /// @brief The scale of the thing (deprecated I think) float scale = 1; // assuming uniform scale + /// @brief boolean indicating if the position was updated bool positionUpdated = false; + /// @brief boolean indicating if the orientation was updated bool orientationUpdated = false; protected: @@ -100,11 +135,16 @@ protected: public: Spherical16 linearVelocity; Spherical16 angularVelocity; + + /// @brief Get the linear velocity of the thing + /// @return The linear velocity in local space, in meters per second virtual Spherical16 GetLinearVelocity(); + /// @brief Get the angular velocity of the thing + /// @return The angular velocity in local space virtual Spherical16 GetAngularVelocity(); public: - /// @brief Terminated thins are no longer updated + /// @brief Terminated things are no longer updated void Terminate(); /// @brief Sets the location from where the 3D model of this Thing can be @@ -114,18 +154,24 @@ public: /// the only official supported model format is .obj void SetModel(const char *url); + #if defined(ARDUINO) + void Update(); + #endif + /// @brief Updates the state of the thing /// @param currentTimeMs The current clock time in milliseconds virtual void Update(unsigned long currentTimeMs) { (void)currentTimeMs; }; - virtual void SendBytes(char *buffer, unsigned char *ix) { - (void)buffer; - (void)ix; - }; - virtual void ProcessBytes(char *bytes) { (void)bytes; }; + /// @brief Function used to generate binary data for this thing + /// @param buffer The byte array for thw binary data + /// @param ix The starting position for writing the binary data + virtual void GenerateBinary(char *buffer, unsigned char *ix); + // /// @brief FUnction used to process binary data received for this thing + /// @param bytes The binary data + virtual void ProcessBinary(char *bytes); }; } // namespace Control } // namespace Passer -using namespace Passer::Control; \ No newline at end of file +using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/UdpArduino.cpp b/UdpArduino.cpp deleted file mode 100644 index cb9cab0..0000000 --- a/UdpArduino.cpp +++ /dev/null @@ -1,95 +0,0 @@ -#include "UdpArduino.h" - -#if defined(ARDUINO) -#include -#endif -namespace Passer { -namespace Control { - -void UdpArduino::Setup(int localPort, const char *remoteIpAddress, - int remotePort) { -#if ARDUINO - this->remoteIpAddress = remoteIpAddress; - this->remotePort = remotePort; - GetBroadcastAddress(); - - if (WiFi.isConnected() == false) { - std::cout << "No network available!\n"; - return; - } - udp.begin(this->localPort); - - std::cout << "Wifi sync started to port " << this->remotePort << "\n"; -#endif -} - -void UdpArduino::GetBroadcastAddress() { -#if ARDUINO - IPAddress broadcastAddress = WiFi.localIP(); - broadcastAddress[3] = 255; - String broadcastIpString = broadcastAddress.toString(); - this->broadcastIpAddress = new char[broadcastIpString.length() + 1]; - broadcastIpString.toCharArray(this->broadcastIpAddress, - broadcastIpString.length() + 1); - std::cout << "Broadcast address: " << broadcastIpAddress << "\n"; -#endif -} - -void UdpArduino::Receive() { -#if ARDUINO - int packetSize = udp.parsePacket(); - while (packetSize > 0) { - udp.read(buffer, packetSize); - - String senderAddress = udp.remoteIP().toString(); - char sender_ipAddress[16]; - senderAddress.toCharArray(sender_ipAddress, 16); - int sender_port = udp.remotePort(); - - Participant *remoteParticipant = - this->GetParticipant(sender_ipAddress, sender_port); - if (remoteParticipant == nullptr) { - remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port); - // std::cout << "New sender " << sender_ipAddress << ":" << sender_port - // << "\n"; - // std::cout << "New remote participant " << remoteParticipant->ipAddress - // << ":" << remoteParticipant->port << " " - // << (int)remoteParticipant->networkId << "\n"; - } - - ReceiveData(packetSize, remoteParticipant); - packetSize = udp.parsePacket(); - } -#endif -} - -bool UdpArduino::Send(RemoteParticipant* remoteParticipant, int bufferSize) { -#if ARDUINO - udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port); - udp.write(buffer, bufferSize); - udp.endPacket(); - - // std::cout << "Sent to " << this->remoteIpAddress << ":" - // << this->remotePort << "\n"; -#endif - return true; -} - -bool UdpArduino::Publish(IMessage *msg) { -#ifdef ARDUINO - int bufferSize = msg->Serialize(this->buffer); - if (bufferSize <= 0) - return true; - - udp.beginPacket(this->broadcastIpAddress, this->remotePort); - udp.write(buffer, bufferSize); - udp.endPacket(); - - // std::cout << "Publish to " << this->broadcastIpAddress << ":" - // << this->remotePort << "\n"; -#endif - return true; -}; - -} // namespace Control -} // namespace Passer \ No newline at end of file diff --git a/UdpArduino.h b/UdpArduino.h deleted file mode 100644 index 5b194b9..0000000 --- a/UdpArduino.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#include "Participant.h" - -namespace Passer { -namespace Control { - -class UdpArduino : public Participant { -public: - void Setup(int localPort, const char *remoteIpAddress, int remotePort); - void Receive(); - bool Send(RemoteParticipant* remoteParticipant, int bufferSize); - bool Publish(IMessage *msg); - -protected: - void GetBroadcastAddress(); -}; - -} // namespace Control -} // namespace Passer \ No newline at end of file diff --git a/UdpPosix.cpp b/UdpPosix.cpp deleted file mode 100644 index 08f216c..0000000 --- a/UdpPosix.cpp +++ /dev/null @@ -1,145 +0,0 @@ -#include "UdpPosix.h" - -#if defined(__unix__) || defined(__APPLE__) -#include -#include // For fcntl -#include -#include -#include -#endif - -namespace Passer { -namespace Control { - -void UdpPosix::Setup(int localPort, const char *remoteIpAddress, - int remotePort) { -#if defined(__unix__) || defined(__APPLE__) - - // Create a UDP socket - - this->sock = socket(AF_INET, SOCK_DGRAM, 0); - - if (this->sock < 0) { - std::cerr << "Error creating socket" << std::endl; - return; - } - -// Set the socket to non-blocking mode -#if defined(_WIN32) || defined(_WIN64) - u_long mode = 1; // 1 to enable non-blocking socket - ioctlsocket(this->sock, FIONBIO, &mode); -#elif defined(__unix__) || defined(__APPLE__) - int flags = fcntl(this->sock, F_GETFL, 0); - fcntl(this->sock, F_SETFL, flags | O_NONBLOCK); -#endif - - if (remotePort != 0) { - // Set up the address to send to - memset(&remote_addr, 0, sizeof(remote_addr)); - remote_addr.sin_family = AF_INET; - remote_addr.sin_port = htons(remotePort); - if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) { - std::cerr << "Invalid address" << std::endl; - close(sock); - return; - } - } - - // Set up the receiving address - memset(&server_addr, 0, sizeof(server_addr)); - server_addr.sin_family = AF_INET; - server_addr.sin_port = htons(localPort); - if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) { - std::cerr << "Invalid address" << std::endl; - close(sock); - return; - } - - // Bind the socket to the specified port - if (bind(this->sock, (const struct sockaddr *)&server_addr, - sizeof(server_addr)) < 0) { - std::cerr << "Bind failed" << std::endl; - close(sock); - } - -#endif -} - -void UdpPosix::Receive() { -#if defined(__unix__) || defined(__APPLE__) - sockaddr_in client_addr; - socklen_t len = sizeof(client_addr); - int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, - (struct sockaddr *)&client_addr, &len); - if (packetSize > 0) { - char sender_ipAddress[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, - INET_ADDRSTRLEN); - int sender_port = ntohs(client_addr.sin_port); - - Participant *remoteParticipant = - this->GetParticipant(sender_ipAddress, sender_port); - if (remoteParticipant == nullptr) { - remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port); - // std::cout << "New sender " << sender_ipAddress << ":" << sender_port - // << "\n"; - // std::cout << "New remote participant " << remoteParticipant->ipAddress - // << ":" << remoteParticipant->port << " " - // << (int)remoteParticipant->networkId << "\n"; - } - - ReceiveData(packetSize, remoteParticipant); - // std::cout << "Received data\n"; - } -#endif -} - -bool UdpPosix::Send(RemoteParticipant *remoteParticipant, int bufferSize) { -#if defined(__unix__) || defined(__APPLE__) - // Set up the destination address - // char ip_str[INET_ADDRSTRLEN]; - // inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN); - // std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port) - // << "\n"; - struct sockaddr_in dest_addr; - memset(&dest_addr, 0, sizeof(dest_addr)); - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(remoteParticipant->port); - dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress); - - // Send the message - int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, - (struct sockaddr *)&remote_addr, sizeof(remote_addr)); - if (sent_bytes < 0) { - std::cerr << "sendto failed with error: " << sent_bytes << std::endl; - close(sock); - return false; - } -#endif - return true; -} - -bool UdpPosix::Publish(IMessage *msg) { -#if defined(__unix__) || defined(__APPLE__) - int bufferSize = msg->Serialize(this->buffer); - if (bufferSize <= 0) - return true; - - char ip_str[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN); - std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) - << "\n"; - int sent_bytes = - sendto(sock, this->buffer, bufferSize, 0, - (struct sockaddr *)&broadcast_addr, sizeof(broadcast_addr)); - if (sent_bytes < 0) { - std::cerr << "sendto failed with error: " << sent_bytes << std::endl; - close(sock); - return false; - } -#endif - return true; -} - -} // namespace Control -} // namespace Passer \ No newline at end of file diff --git a/UdpPosix.h b/UdpPosix.h deleted file mode 100644 index 8dc2241..0000000 --- a/UdpPosix.h +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include "Participant.h" - -namespace Passer { -namespace Control { - -class UdpPosix : public Participant { -public: - void Setup(int localPort, const char *remoteIpAddress, int remotePort); - void Receive(); - bool Send(RemoteParticipant* remoteParticipant, int bufferSize); - bool Publish(IMessage *msg); -}; - -} // namespace Control -} // namespace Passer \ No newline at end of file diff --git a/UdpWindows.cpp b/UdpWindows.cpp deleted file mode 100644 index 9ddaf0a..0000000 --- a/UdpWindows.cpp +++ /dev/null @@ -1,207 +0,0 @@ -#include "UdpWindows.h" - -#if defined(_WIN32) || defined(_WIN64) -#include -#include -#pragma comment(lib, "ws2_32.lib") -#elif defined(__unix__) || defined(__APPLE__) -#include -#include // For fcntl -#include -#include -#include -#endif - -namespace Passer { -namespace Control { - -void UdpWindows::Setup(int localPort, const char *remoteIpAddress, - int remotePort) { -#if defined(_WIN32) || defined(_WIN64) - - // Create a UDP socket -#if defined(_WIN32) || defined(_WIN64) - // Windows-specific Winsock initialization - WSADATA wsaData; - if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) { - std::cerr << "WSAStartup failed" << std::endl; - return; - } -#endif - -#if defined(_WIN32) || defined(_WIN64) - this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); -#elif defined(__unix__) || defined(__APPLE__) - this->sock = socket(AF_INET, SOCK_DGRAM, 0); -#endif - - if (this->sock < 0) { - std::cerr << "Error creating socket" << std::endl; - return; - } - -// Set the socket to non-blocking mode -#if defined(_WIN32) || defined(_WIN64) - u_long mode = 1; // 1 to enable non-blocking socket - ioctlsocket(this->sock, FIONBIO, &mode); -#elif defined(__unix__) || defined(__APPLE__) - int flags = fcntl(this->sock, F_GETFL, 0); - fcntl(this->sock, F_SETFL, flags | O_NONBLOCK); -#endif - - if (remotePort != 0) { - // Set up the address to send to - memset(&remote_addr, 0, sizeof(remote_addr)); - remote_addr.sin_family = AF_INET; - remote_addr.sin_port = htons((u_short)remotePort); - if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) { - std::cerr << "Invalid address" << std::endl; -#if defined(_WIN32) || defined(_WIN64) - closesocket(sock); - WSACleanup(); -#elif defined(__unix__) || defined(__APPLE__) - close(sock); -#endif - return; - } - } - - // Set up the receiving address - memset(&server_addr, 0, sizeof(server_addr)); - server_addr.sin_family = AF_INET; - server_addr.sin_port = htons((u_short)localPort); - if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) { - std::cerr << "Invalid address" << std::endl; -#if defined(_WIN32) || defined(_WIN64) - closesocket(sock); - WSACleanup(); -#elif defined(__unix__) || defined(__APPLE__) - close(sock); -#endif - return; - } - - // Bind the socket to the specified port - if (bind(this->sock, (const struct sockaddr *)&server_addr, - sizeof(server_addr)) < 0) { - std::cerr << "Bind failed" << std::endl; -#if defined(_WIN32) || defined(_WIN64) - closesocket(sock); - WSACleanup(); -#elif defined(__unix__) || defined(__APPLE__) - close(sock); -#endif - } - -#endif -} - -void UdpWindows::Receive() { -#if defined(_WIN32) || defined(_WIN64) - // char ip_str[INET_ADDRSTRLEN]; - // inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN); - // std::cout << this->name << " Receive on " << ip_str << ":" - // << ntohs(server_addr.sin_port) << "\n"; - - sockaddr_in client_addr; -#if defined(_WIN32) || defined(_WIN64) - int len = sizeof(client_addr); -#elif defined(__unix__) || defined(__APPLE__) - socklen_t len = sizeof(client_addr); -#endif - int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, - (struct sockaddr *)&client_addr, &len); - // std::cout << "received data " << packetSize << "\n"; - if (packetSize < 0) { -#if defined(_WIN32) || defined(_WIN64) - int error_code = WSAGetLastError(); // Get the error code on Windows - if (error_code != WSAEWOULDBLOCK) - std::cerr << "recvfrom failed with error: " << error_code << std::endl; -#else - // std::cerr << "recvfrom failed with error: " << packetSize << std::endl; -#endif - } else if (packetSize > 0) { - char sender_ipAddress[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, - INET_ADDRSTRLEN); - int sender_port = ntohs(client_addr.sin_port); - - Participant *remoteParticipant = - this->GetParticipant(sender_ipAddress, sender_port); - if (remoteParticipant == nullptr) { - remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port); - // std::cout << "New sender " << sender_ipAddress << ":" - // << sender_port << "\n"; - // std::cout << "New remote participant " << - // remoteParticipant->ipAddress - // << ":" - // << remoteParticipant->port << " " - // << (int)remoteParticipant->networkId << "\n"; - } - - ReceiveData(packetSize, remoteParticipant); - } -#endif -} - -bool UdpWindows::Send(RemoteParticipant *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); - std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port) - << "\n"; - int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, - (struct sockaddr *)&remote_addr, sizeof(remote_addr)); -#if defined(_WIN32) || defined(_WIN64) - if (sent_bytes <= SOCKET_ERROR) { - int error_code = WSAGetLastError(); // Get the error code on Windows - std::cerr << "sendto failed with error: " << error_code << std::endl; - closesocket(sock); - WSACleanup(); - return false; - } -#elif defined(__unix__) || defined(__APPLE__) - if (sent_bytes < 0) { - std::cerr << "sendto failed with error: " << sent_bytes << std::endl; - close(sock); - return false; - } -#endif -#endif - return true; -} - -bool UdpWindows::Publish(IMessage *msg) { -#if defined(_WIN32) || defined(_WIN64) - int bufferSize = msg->Serialize(this->buffer); - if (bufferSize <= 0) - return true; - - char ip_str[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN); - std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) - << "\n"; - int sent_bytes = - sendto(sock, this->buffer, bufferSize, 0, - (struct sockaddr *)&broadcast_addr, sizeof(broadcast_addr)); -#if defined(_WIN32) || defined(_WIN64) - if (sent_bytes <= SOCKET_ERROR) { - int error_code = WSAGetLastError(); // Get the error code on Windows - std::cerr << "sendto failed with error: " << error_code << std::endl; - closesocket(sock); - WSACleanup(); - return false; - } -#elif defined(__unix__) || defined(__APPLE__) - if (sent_bytes < 0) { - std::cerr << "sendto failed with error: " << sent_bytes << std::endl; - close(sock); - return false; - } -#endif -#endif - return true; -} - -} // namespace Control -} // namespace Passer \ No newline at end of file diff --git a/UdpWindows.h b/UdpWindows.h deleted file mode 100644 index 4a7b3ce..0000000 --- a/UdpWindows.h +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include "Participant.h" - -namespace Passer { -namespace Control { - -class UdpWindows : public Participant { -public: - void Setup(int localPort, const char *remoteIpAddress, int remotePort); - void Receive(); - bool Send(RemoteParticipant* remoteParticipant, int bufferSize); - bool Publish(IMessage *msg); -}; - -} // namespace Control -} // namespace Passer \ No newline at end of file diff --git a/Windows/Participant.cpp b/Windows/Participant.cpp new file mode 100644 index 0000000..e69de29 diff --git a/Windows/Participant.h b/Windows/Participant.h new file mode 100644 index 0000000..a0487b4 --- /dev/null +++ b/Windows/Participant.h @@ -0,0 +1,19 @@ +#pragma once + +#include "../Participant.h" + +namespace Passer { +namespace RoboidControl { +namespace Windows { + +class Participant : public RoboidControl::Participant { + public: + void Setup(int localPort, const char* remoteIpAddress, int remotePort); + void Receive(); + bool Send(RemoteParticipant* remoteParticipant, int bufferSize); + bool Publish(IMessage* msg); +}; + +} // namespace Windows +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file