Updated ConttrolCore

Arduino/Participant.cpp

@@ -0,0 +1,99 @@
+#include "Participant.h"
+
+#if defined(ARDUINO)
+#if defined(ARDUINO_ARCH_ESP8266)
+#include <ESP8266WiFi.h>
+#elif defined(ESP32)
+#include <WiFi.h>
+#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

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
+
+  }}

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;
+using namespace Passer::RoboidControl;

Participant.cpp

@@ -165,7 +165,7 @@ bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) {
   Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);
   return thisWindows->Send(remoteParticipant, bufferSize);
 #elif defined(__unix__) || defined(__APPLE__)
-  Posix::Participant* thisPosix = static_cast<Posix::Particiapant*>(this);
+  Posix::Participant* thisPosix = static_cast<Posix::Participant*>(this);
   return thisPosix->Send(remoteParticipant, bufferSize);
 #elif defined(ARDUINO)
   Arduino::Participant* thisArduino = static_cast<Arduino::Participant*>(this);

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;
 

Posix/Participant.cpp

@@ -0,0 +1,138 @@
+#include "Participant.h"
+
+#if defined(__unix__) || defined(__APPLE__)
+#include <arpa/inet.h>
+#include <fcntl.h>  // For fcntl
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#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

RemoteParticipant.cpp

@@ -1,7 +1,7 @@
 #include "RemoteParticipant.h"
 
 namespace Passer {
-namespace Control {
+namespace RoboidControl {
 
 RemoteParticipant::RemoteParticipant() {}
 

RemoteParticipant.h

@@ -2,7 +2,7 @@
 #include "Thing.h"
 
 namespace Passer {
-namespace Control {
+namespace RoboidControl {
 
 class RemoteParticipant {
 public:

Windows/Participant.cpp

@@ -0,0 +1,199 @@
+#include "Participant.h"
+
+#if defined(_WIN32) || defined(_WIN64)
+#include <winsock2.h>
+#include <ws2tcpip.h>
+#pragma comment(lib, "ws2_32.lib")
+#elif defined(__unix__) || defined(__APPLE__)
+#include <arpa/inet.h>
+#include <fcntl.h>  // For fcntl
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#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