diff --git a/Arduino/Participant.cpp b/Arduino/Participant.cpp index e69de29..838aa1c 100644 --- a/Arduino/Participant.cpp +++ b/Arduino/Participant.cpp @@ -0,0 +1,99 @@ +#include "Participant.h" + +#if defined(ARDUINO) +#if defined(ARDUINO_ARCH_ESP8266) +#include +#elif defined(ESP32) +#include +#endif +#endif + +namespace Passer { +namespace RoboidControl { +namespace Arduino { + +void Participant::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 Participant::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 Participant::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(); + + RoboidControl::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 Participant::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 Participant::Publish(IMessage* msg) { +#ifdef ARDUINO + int bufferSize = msg->Serialize((char*)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 Arduino +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/Messages/Messages.cpp b/Messages/Messages.cpp index 147e764..dfd2a4a 100644 --- a/Messages/Messages.cpp +++ b/Messages/Messages.cpp @@ -1,17 +1,19 @@ #include "Messages.h" #include "LowLevelMessages.h" -// #include "Messages/BinaryMsg.h" #include "Participant.h" #include "string.h" +namespace Passer { + namespace RoboidControl { + #pragma region IMessage IMessage::IMessage() {} // IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); } -IMessage::IMessage(char* buffer) {} +// IMessage::IMessage(char* buffer) {} unsigned char IMessage::Serialize(char* buffer) { return 0; @@ -37,3 +39,5 @@ unsigned char IMessage::Serialize(char* buffer) { // IMessage #pragma endregion + + }} \ No newline at end of file diff --git a/Messages/Messages.h b/Messages/Messages.h index b34f247..72263c2 100644 --- a/Messages/Messages.h +++ b/Messages/Messages.h @@ -6,7 +6,7 @@ #include "float16.h" namespace Passer { -namespace Control { +namespace RoboidControl { class Participant; @@ -24,4 +24,4 @@ public: } // 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/Participant.cpp b/Participant.cpp index 324a3bc..1f432a9 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -165,7 +165,7 @@ bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) { Windows::Participant* thisWindows = static_cast(this); return thisWindows->Send(remoteParticipant, bufferSize); #elif defined(__unix__) || defined(__APPLE__) - Posix::Participant* thisPosix = static_cast(this); + Posix::Participant* thisPosix = static_cast(this); return thisPosix->Send(remoteParticipant, bufferSize); #elif defined(ARDUINO) Arduino::Participant* thisArduino = static_cast(this); diff --git a/Participant.h b/Participant.h index 47a355d..fdf53c1 100644 --- a/Participant.h +++ b/Participant.h @@ -1,6 +1,5 @@ #pragma once -//#include "Messages/" #include "Messages/ParticipantMsg.h" #include "Messages/BinaryMsg.h" #include "Messages/InvestigateMsg.h" @@ -30,7 +29,7 @@ namespace RoboidControl { /// @brief A participant is device which can communicate with other participants class Participant : public RemoteParticipant { public: - unsigned char buffer[1024]; + char buffer[1024]; long publishInterval = 3000; // 3 seconds // unsigned char networkId = 0; diff --git a/Posix/Participant.cpp b/Posix/Participant.cpp index e69de29..5205486 100644 --- a/Posix/Participant.cpp +++ b/Posix/Participant.cpp @@ -0,0 +1,138 @@ +#include "Participant.h" + +#if defined(__unix__) || defined(__APPLE__) +#include +#include // For fcntl +#include +#include +#include +#endif + +namespace Passer { +namespace RoboidControl { +namespace Posix { + +void Participant::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 Participant::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); + + RoboidControl::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 Participant::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 Participant::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 Posix +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file diff --git a/RemoteParticipant.cpp b/RemoteParticipant.cpp index 51e3b8d..7ff7b13 100644 --- a/RemoteParticipant.cpp +++ b/RemoteParticipant.cpp @@ -1,7 +1,7 @@ #include "RemoteParticipant.h" namespace Passer { -namespace Control { +namespace RoboidControl { RemoteParticipant::RemoteParticipant() {} diff --git a/RemoteParticipant.h b/RemoteParticipant.h index bf0f36d..b3ed828 100644 --- a/RemoteParticipant.h +++ b/RemoteParticipant.h @@ -2,7 +2,7 @@ #include "Thing.h" namespace Passer { -namespace Control { +namespace RoboidControl { class RemoteParticipant { public: diff --git a/Windows/Participant.cpp b/Windows/Participant.cpp index e69de29..d6fa76f 100644 --- a/Windows/Participant.cpp +++ b/Windows/Participant.cpp @@ -0,0 +1,199 @@ +#include "Participant.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 RoboidControl { +namespace Windows { + +void Participant::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 Participant::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); + + RoboidControl::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 Participant::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 Participant::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 Windows +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file