diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index 7594171..2b9330a 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -19,7 +19,7 @@ namespace RoboidControl { namespace Arduino { -void LocalParticipant::Setup(int localPort, +void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(ARDUINO) && defined(HAS_WIFI) @@ -44,7 +44,7 @@ void LocalParticipant::Setup(int localPort, #endif } -void LocalParticipant::GetBroadcastAddress() { +void ParticipantUDP::GetBroadcastAddress() { #if defined(ARDUINO) && defined(HAS_WIFI) IPAddress broadcastAddress = WiFi.localIP(); broadcastAddress[3] = 255; @@ -56,7 +56,7 @@ void LocalParticipant::GetBroadcastAddress() { #endif } -void LocalParticipant::Receive() { +void ParticipantUDP::Receive() { #if defined(ARDUINO) && defined(HAS_WIFI) int packetSize = udp.parsePacket(); while (packetSize > 0) { @@ -86,7 +86,7 @@ void LocalParticipant::Receive() { #endif } -bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { #if defined(ARDUINO) && defined(HAS_WIFI) // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" // << remoteParticipant->port << "\n"; @@ -106,7 +106,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { return true; } -bool LocalParticipant::Publish(IMessage* msg) { +bool ParticipantUDP::Publish(IMessage* msg) { #if defined(ARDUINO) && defined(HAS_WIFI) int bufferSize = msg->Serialize((char*)this->buffer); if (bufferSize <= 0) diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index 8887ade..94196d7 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -1,6 +1,6 @@ #pragma once -#include "../LocalParticipant.h" +#include "Participants/ParticipantUDP.h" #if defined(HAS_WIFI) #include @@ -9,7 +9,7 @@ namespace RoboidControl { namespace Arduino { -class LocalParticipant : public RoboidControl::LocalParticipant { +class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); diff --git a/CMakeLists.txt b/CMakeLists.txt index 934c5ad..09c1b91 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,22 @@ cmake_minimum_required(VERSION 3.13) # CMake version check + +file(GLOB srcs + *.cpp + Things/*.cpp + Messages/*.cpp + Arduino/*.cpp + Posix/*.cpp + Windows/*.cpp + EspIdf/*.cpp + LinearAlgebra/*.cpp + Participants/*.cpp +) + if(ESP_PLATFORM) idf_component_register( - SRC_DIRS "." - INCLUDE_DIRS "." + SRCS ${srcs} + INCLUDE_DIRS "." "LinearAlgebra" + REQUIRES esp_netif esp_wifi ) else() project(RoboidControl) @@ -28,14 +42,6 @@ else() . LinearAlgebra ) - file(GLOB srcs - *.cpp - Things/*.cpp - Messages/*.cpp - Arduino/*.cpp - Posix/*.cpp - Windows/*.cpp - ) add_library(RoboidControl STATIC ${srcs}) enable_testing() diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp new file mode 100644 index 0000000..22e8160 --- /dev/null +++ b/EspIdf/EspIdfParticipant.cpp @@ -0,0 +1,166 @@ +#include "EspIdfParticipant.h" + +#if defined(IDF_VER) +#include "esp_wifi.h" +#endif + +namespace RoboidControl { +namespace EspIdf { + +void ParticipantUDP::Setup(int localPort, + const char* remoteIpAddress, + int remotePort) { +#if defined(IDF_VER) + std::cout << "Set up UDP\n"; + GetBroadcastAddress(); + + wifi_ap_record_t ap_info; + esp_err_t result = esp_wifi_sta_get_ap_info(&ap_info); + if (result != ESP_OK) { + std::cout << "No network available!\n"; + return; + } + + // Create a UDP socket + this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (this->sockfd < 0) { + std::cout << "Unable to create UDP socket: errno " << errno << "\n"; + vTaskDelete(NULL); + return; + } + + // Set up the server address structure + struct sockaddr_in local_addr; + memset(&local_addr, 0, sizeof(local_addr)); + local_addr.sin_family = AF_INET; + local_addr.sin_port = htons(this->port); + local_addr.sin_addr.s_addr = + htonl(INADDR_ANY); // Listen on all available network interfaces + + // Bind the socket to the address and port + if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < + 0) { + std::cout << "Unable to bind UDP socket: errno " << errno << "\n"; + close(sockfd); + vTaskDelete(NULL); + return; + } + + // Initialize the dest_addr structure + memset(&this->dest_addr, 0, + sizeof(this->dest_addr)); // Clear the entire structure + this->dest_addr.sin_family = AF_INET; + this->dest_addr.sin_port = htons(this->remoteSite->port); + inet_pton(AF_INET, this->remoteSite->ipAddress, + &this->dest_addr.sin_addr.s_addr); + + std::cout << "Wifi sync started local " << this->port << ", remote " + << this->remoteSite->ipAddress << ":" << this->remoteSite->port + << "\n"; +#endif // IDF_VER +} + +void ParticipantUDP::GetBroadcastAddress() { +#if defined(IDF_VER) + // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION... + + esp_netif_ip_info_t ip_info; + esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); + + // Get IP information (IP address, netmask, gateway) + if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) { + std::cout << "Failed to get IP info\n"; + return; + } + + ip_addr_t broadcast_addr = {}; + broadcast_addr.u_addr.ip4.addr = + (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr; + + snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR, + IP2STR(&broadcast_addr.u_addr.ip4)); + std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n"; +#endif // IDF_VER +} + +void ParticipantUDP::Receive() { +#if defined(IDF_VER) + struct pollfd fds[1]; + fds[0].fd = sockfd; + fds[0].events = POLLIN; // We're looking for data available to read + + // Use poll() with a timeout of 0 to return immediately + int ret = poll(fds, 1, 0); + if (ret == -1) { + std::cout << "poll() error\n"; + return; + } + + // char buffer[1024]; + struct sockaddr_in source_addr; + + socklen_t addr_len = sizeof(source_addr); + char sender_ipAddress[INET_ADDRSTRLEN]; + + while (ret > 0 && fds[0].revents & POLLIN) { + int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0, + (struct sockaddr*)&source_addr, &addr_len); + if (packetSize < 0) { + std::cout << "recvfrom() error\n"; + return; + } else if (packetSize == 0) { + break; + } + + // std::cout << "receiving " << packetSize << " bytes, msgId " << + // (int)this->buffer[0] << "\n"; + inet_ntoa_r(source_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); + unsigned int sender_port = ntohs(source_addr.sin_port); + + ReceiveData(packetSize, sender_ipAddress, sender_port); + + ret = poll(fds, 1, 0); + if (ret == -1) { + std::cout << "poll() error\n"; + return; + } + } + // std::cout << "no more messages\n"; + +#endif // IDF_VER +} + +bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +#if defined(IDF_VER) + // std::cout << "Sending to " << remoteParticipant->ipAddress << ":" + // << remoteParticipant->port << "\n"; + + int err = sendto(this->sockfd, buffer, bufferSize, 0, + (struct sockaddr*)&dest_addr, sizeof(dest_addr)); + if (errno != 0) + std::cout << "Send error " << err << " or " << errno << "\n"; + +#endif + return true; +} + +bool ParticipantUDP::Publish(IMessage* msg) { +#if defined(IDF_VER) + int bufferSize = msg->Serialize((char*)this->buffer); + if (bufferSize <= 0) + return true; + + struct sockaddr_in dest_addr; + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(this->port); + inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr); + int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, + sizeof(dest_addr)); + if (err != 0) + std::cout << "Publish error\n"; +#endif + return true; +}; + +} // namespace EspIdf +} // namespace RoboidControl diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h new file mode 100644 index 0000000..cf1c6bf --- /dev/null +++ b/EspIdf/EspIdfParticipant.h @@ -0,0 +1,32 @@ +#pragma once + +#include "Participants/ParticipantUDP.h" + +#if defined(IDF_VER) +#include "lwip/sockets.h" +#endif + +namespace RoboidControl { +namespace EspIdf { + +class ParticipantUDP : public RoboidControl::ParticipantUDP { + public: + void Setup(int localPort, const char* remoteIpAddress, int remotePort); + void Receive(); + bool Send(Participant* remoteParticipant, int bufferSize); + bool Publish(IMessage* msg); + + protected: +#if defined(IDF_VER) + char broadcastIpAddress[INET_ADDRSTRLEN]; + + int sockfd; + struct sockaddr_in dest_addr; + // struct sockaddr_in src_addr; +#endif + + void GetBroadcastAddress(); +}; + +} // namespace EspIdf +} // namespace RoboidControl diff --git a/EspIdf/EspIdfUtils.cpp b/EspIdf/EspIdfUtils.cpp new file mode 100644 index 0000000..045f3b9 --- /dev/null +++ b/EspIdf/EspIdfUtils.cpp @@ -0,0 +1,100 @@ +#include "EspIdfUtils.h" + +#if defined(IDF_VER) +#include +// #include "esp_event.h" +// #include "esp_log.h" +#include "esp_netif.h" +#include "esp_wifi.h" +// #include "lwip/inet.h" +// #include "lwip/ip_addr.h" +#include "string.h" + +const char* hotspotSSID = "Roboid"; +const char* hotspotPassword = "alchemy7000"; + +esp_netif_t* wifi_netif = nullptr; +// Semaphore to signal Wi-Fi connection status +// SemaphoreHandle_t wifi_semaphore; +static bool wifi_connected = false; + +static void wifi_event_handler(void* arg, + esp_event_base_t event_base, + int32_t event_id, + void* event_data) { + if (event_base == WIFI_EVENT) { + if (event_id == WIFI_EVENT_STA_START) + esp_wifi_connect(); + else if (event_id == WIFI_EVENT_STA_DISCONNECTED) + esp_wifi_connect(); + } else if (event_base == IP_EVENT) { + if (event_id == IP_EVENT_STA_GOT_IP) { + // ip_event_got_ip_t* event = (ip_event_got_ip_t*)event_data; + // const char* ipaddr = IP2STR(&event->ip_info.ip); + wifi_connected = true; + // xSemaphoreGive(wifi_semaphore); // Signal that connection is + // established + } + } +} + +bool StartWifi(const char* wifiSsid, const char* wifiPassword) { + std::cout << "Connecting to WiFi " << wifiSsid << "\n"; + + esp_netif_init(); + esp_event_loop_create_default(); + + wifi_netif = esp_netif_create_default_wifi_sta(); + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + esp_wifi_init(&cfg); + + esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, + NULL); + esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, + NULL); + + wifi_config_t wifi_config = {}; + strncpy((char*)wifi_config.sta.ssid, wifiSsid, strlen(wifiSsid) + 1); + strncpy((char*)wifi_config.sta.password, wifiPassword, + strlen(wifiPassword) + 1); + + esp_wifi_set_mode(WIFI_MODE_STA); + esp_wifi_set_config(WIFI_IF_STA, &wifi_config); + esp_wifi_start(); + + // Wait for connection with a timeout of 10 seconds + TickType_t xLastWakeTime = xTaskGetTickCount(); + bool success = false; + for (int i = 0; i < 20; i++) { // 20 iterations, each 500ms + if (wifi_connected) { + success = true; + std::cout << " Connected.\n"; + break; + } + std::cout << "."; + fflush(stdout); // Ensure output is printed immediately + vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(500)); // Wait 500ms + } + + if (wifi_connected) { + esp_netif_ip_info_t ip_info = {}; + esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); + + // Get IP information (IP address, netmask, gateway) + if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) { + std::cout << "Failed to get IP info\n"; + return false; + } + + // Convert the IP address to string format using inet_ntoa + char ip_str[16]; // IPv4 address can have a max of 15 characters + null + // terminator + snprintf(ip_str, sizeof(ip_str), IPSTR, IP2STR(&ip_info.ip)); + std::cout << "IP address = " << ip_str << "\n"; + } else + std::cout << "\nCould not connect to home network.\n"; + + return success; +} + +#endif diff --git a/EspIdf/EspIdfUtils.h b/EspIdf/EspIdfUtils.h new file mode 100644 index 0000000..afe4d02 --- /dev/null +++ b/EspIdf/EspIdfUtils.h @@ -0,0 +1,6 @@ +#pragma once +#if defined(IDF_VER) + +bool StartWifi(const char *wifiSsid, const char *wifiPassword); + +#endif \ No newline at end of file diff --git a/LinearAlgebra/Angle.h b/LinearAlgebra/Angle.h index 1e47726..e82b3af 100644 --- a/LinearAlgebra/Angle.h +++ b/LinearAlgebra/Angle.h @@ -21,7 +21,7 @@ template class AngleOf { public: /// @brief Create a new angle with a zero value - AngleOf(); + AngleOf(); /// @brief An zero value angle const static AngleOf zero; @@ -209,7 +209,7 @@ class AngleOf { private: T value; - AngleOf(T rawValue); + AngleOf(T rawValue); }; using AngleSingle = AngleOf; diff --git a/LinearAlgebra/Direction.h b/LinearAlgebra/Direction.h index efd8c4b..57f6647 100644 --- a/LinearAlgebra/Direction.h +++ b/LinearAlgebra/Direction.h @@ -30,11 +30,11 @@ class DirectionOf { AngleOf vertical; /// @brief Create a new direction with zero angles - DirectionOf(); + DirectionOf(); /// @brief Create a new direction /// @param horizontal The horizontal angle /// @param vertical The vertical angle. - DirectionOf(AngleOf horizontal, AngleOf vertical); + DirectionOf(AngleOf horizontal, AngleOf vertical); /// @brief Convert the direction into a carthesian vector /// @return The carthesian vector corresponding to this direction. diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 2b83a1d..b725f55 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,6 +1,284 @@ #include "Matrix.h" +#include -template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { +namespace LinearAlgebra { + +#pragma region Matrix1 + +Matrix1::Matrix1(int size) : size(size) { + if (this->size == 0) + data = nullptr; + else { + this->data = new float[size](); + this->externalData = false; + } +} + +Matrix1::Matrix1(float* data, int size) : data(data), size(size) { + this->externalData = true; +} + +Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) { + Matrix1 r = Matrix1(4); + float* data = r.data; + data[0] = q.x; + data[1] = q.y; + data[2] = q.z; + data[3] = q.w; + return r; +} + +Quaternion LinearAlgebra::Matrix1::ToQuaternion() { + return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]); +} + +// Matrix1 +#pragma endregion + +#pragma region Matrix2 + +Matrix2::Matrix2() {} + +Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { + this->nValues = nRows * nCols; + if (this->nValues == 0) + this->data = nullptr; + else { + this->data = new float[this->nValues]; + this->externalData = false; + } +} + +Matrix2::Matrix2(float* data, int nRows, int nCols) + : nRows(nRows), nCols(nCols), data(data) { + this->nValues = nRows * nCols; + this->externalData = true; +} + +Matrix2::Matrix2(const Matrix2& m) + : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) { + if (this->nValues == 0) + this->data = nullptr; + else { + this->data = new float[this->nValues]; + std::copy(m.data, m.data + nValues, this->data); + } +} + +Matrix2& Matrix2::operator=(const Matrix2& m) { + if (this != &m) { + delete[] this->data; // Free the current memory + + this->nRows = m.nRows; + this->nCols = m.nCols; + this->nValues = m.nValues; + if (this->nValues == 0) + this->data = nullptr; + else { + this->data = new float[this->nValues]; + std::copy(m.data, m.data + this->nValues, this->data); + } + } + return *this; +} + +Matrix2::~Matrix2() { + if (!this->externalData) + delete[] data; +} + +Matrix2 Matrix2::Clone() const { + Matrix2 r = Matrix2(this->nRows, this->nCols); + std::copy(this->data, this->data + this->nValues, r.data); + return r; +} + +// Move constructor +Matrix2::Matrix2(Matrix2&& other) noexcept + : nRows(other.nRows), + nCols(other.nCols), + nValues(other.nValues), + data(other.data) { + other.data = nullptr; // Set the other object's pointer to nullptr to avoid + // double deletion +} + +// Move assignment operator +Matrix2& Matrix2::operator=(Matrix2&& other) noexcept { + if (this != &other) { + delete[] data; // Clean up current data + nRows = other.nRows; + nCols = other.nCols; + nValues = other.nValues; + data = other.data; + other.data = nullptr; // Avoid double deletion + } + return *this; +} + +Matrix2 Matrix2::Zero(int nRows, int nCols) { + Matrix2 r = Matrix2(nRows, nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = 0; + return r; +} + +void Matrix2::Clear() { + for (int ix = 0; ix < this->nValues; ix++) + this->data[ix] = 0; +} + +Matrix2 Matrix2::Identity(int size) { + return Diagonal(1, size); +} + +Matrix2 Matrix2::Diagonal(float f, int size) { + Matrix2 r = Matrix2(size, size); + float* data = r.data; + int valueIx = 0; + for (int ix = 0; ix < size; ix++) { + data[valueIx] = f; + valueIx += size + 1; + } + return r; +} + +Matrix2 Matrix2::SkewMatrix(const Vector3& v) { + Matrix2 r = Matrix2(3, 3); + float* data = r.data; + data[0 * 3 + 1] = -v.z; // result(0, 1) + data[0 * 3 + 2] = v.y; // result(0, 2) + data[1 * 3 + 0] = v.z; // result(1, 0) + data[1 * 3 + 2] = -v.x; // result(1, 2) + data[2 * 3 + 0] = -v.y; // result(2, 0) + data[2 * 3 + 1] = v.x; // result(2, 1) + return r; +} + +Matrix2 Matrix2::Transpose() const { + Matrix2 r = Matrix2(this->nCols, this->nRows); + + for (uint rowIx = 0; rowIx < this->nRows; rowIx++) { + for (uint colIx = 0; colIx < this->nCols; colIx++) + r.data[colIx * this->nCols + rowIx] = + this->data[rowIx * this->nCols + colIx]; + } + return r; +} + +Matrix2 LinearAlgebra::Matrix2::operator-() const { + Matrix2 r = Matrix2(this->nRows, this->nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = -this->data[ix]; + return r; +} + +Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const { + Matrix2 r = Matrix2(this->nRows, this->nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = this->data[ix] + v.data[ix]; + return r; +} + +Matrix2 Matrix2::operator+=(const Matrix2& v) { + for (int ix = 0; ix < this->nValues; ix++) + this->data[ix] += v.data[ix]; + return *this; +} + +Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { + Matrix2 r = Matrix2(this->nRows, B.nCols); + + int ACols = this->nCols; + int BCols = B.nCols; + int ARows = this->nRows; + // int BRows = B.nRows; + + for (int i = 0; i < ARows; ++i) { + // Pre-compute row offsets + int ARowOffset = i * ACols; // ARowOffset is constant for each row of A + int BColOffset = i * BCols; // BColOffset is constant for each row of B + for (int j = 0; j < BCols; ++j) { + float sum = 0; + // std::cout << " 0"; + int BIndex = j; + for (int k = 0; k < ACols; ++k) { + // std::cout << " + " << this->data[ARowOffset + k] << " * " + // << B.data[BIndex]; + sum += this->data[ARowOffset + k] * B.data[BIndex]; + BIndex += BCols; + } + r.data[BColOffset + j] = sum; + // std::cout << " = " << sum << " ix: " << BColOffset + j << "\n"; + } + } + return r; +} + +Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) { + Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart); + + int resultRowIx = 0; + int resultColIx = 0; + for (int i = rowStart; i < rowStop; i++) { + for (int j = colStart; j < colStop; j++) + r.data[resultRowIx * r.nCols + resultColIx] = + this->data[i * this->nCols + j]; + } + return r; +} + +void Matrix2::UpdateSlice(int rowStart, + int rowStop, + int colStart, + int colStop, + const Matrix2& m) const { + // for (int i = rowStart; i < rowStop; i++) { + // for (int j = colStart; j < colStop; j++) + // this->data[i * this->nCols + j] = + // m.data[(i - rowStart) * m.nCols + (j - colStart)]; + // } + + int rRowDataIx = rowStart * this->nCols; + int mRowDataIx = 0; + for (int rowIx = rowStart; rowIx < rowStop; rowIx++) { + rRowDataIx = rowIx * this->nCols; + // rRowDataIx += this->nCols; + mRowDataIx += m.nCols; + for (int colIx = colStart; colIx < colStop; colIx++) { + this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)]; + } + } +} + +/// @brief Compute the Omega matrix of a 3D vector +/// @param v The vector +/// @return 4x4 Omega matrix +Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) { + Matrix2 r = Matrix2::Zero(4, 4); + r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v)); + + // set last row to -v + int ix = 3 * 4; + r.data[ix++] = -v.x; + r.data[ix++] = -v.y; + r.data[ix] = -v.z; + + // Set last column to v + ix = 3; + r.data[ix += 4] = v.x; + r.data[ix += 4] = v.y; + r.data[ix] = v.z; + return r; +} + +// Matrix2 +#pragma endregion + +} // namespace LinearAlgebra + +template <> +MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { if (rows <= 0 || cols <= 0) { this->rows = 0; this->cols = 0; @@ -14,15 +292,17 @@ template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { this->data = new float[matrixSize]{0.0f}; } -template <> MatrixOf::MatrixOf(Vector3 v) : MatrixOf(3, 1) { +template <> +MatrixOf::MatrixOf(Vector3 v) : MatrixOf(3, 1) { Set(0, 0, v.Right()); Set(1, 0, v.Up()); Set(2, 0, v.Forward()); } template <> -void MatrixOf::Multiply(const MatrixOf *m1, - const MatrixOf *m2, MatrixOf *r) { +void MatrixOf::Multiply(const MatrixOf* m1, + const MatrixOf* m2, + MatrixOf* r) { for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) { for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) { unsigned int rDataIx = colIx2 * m2->cols + rowIx1; @@ -37,7 +317,7 @@ void MatrixOf::Multiply(const MatrixOf *m1, } template <> -Vector3 MatrixOf::Multiply(const MatrixOf *m, Vector3 v) { +Vector3 MatrixOf::Multiply(const MatrixOf* m, Vector3 v) { MatrixOf v_m = MatrixOf(v); MatrixOf r_m = MatrixOf(3, 1); @@ -47,10 +327,11 @@ Vector3 MatrixOf::Multiply(const MatrixOf *m, Vector3 v) { return r; } -template Vector3 MatrixOf::operator*(const Vector3 v) const { - float *vData = new float[3]{v.Right(), v.Up(), v.Forward()}; +template +Vector3 MatrixOf::operator*(const Vector3 v) const { + float* vData = new float[3]{v.Right(), v.Up(), v.Forward()}; MatrixOf v_m = MatrixOf(3, 1, vData); - float *rData = new float[3]{}; + float* rData = new float[3]{}; MatrixOf r_m = MatrixOf(3, 1, rData); Multiply(this, &v_m, &r_m); diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index 925cb5a..ef72922 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -1,10 +1,119 @@ #ifndef MATRIX_H #define MATRIX_H +#include "Quaternion.h" #include "Vector3.h" namespace LinearAlgebra { +/// @brief A 1-dimensional matrix or vector of arbitrary size +class Matrix1 { + public: + float* data = nullptr; + int size = 0; + + Matrix1(int size); + Matrix1(float* data, int size); + + static Matrix1 FromQuaternion(Quaternion q); + Quaternion ToQuaternion(); + + private: + bool externalData = true; +}; + +/// @brief A 2-dimensional matrix of arbitrary size +class Matrix2 { + public: + int nRows = 0; + int nCols = 0; + int nValues = 0; + float* data = nullptr; + + Matrix2(); + Matrix2(int nRows, int nCols); + Matrix2(float* data, int nRows, int nCols); + Matrix2(const Matrix2& m); + Matrix2& operator=(const Matrix2& other); + + ~Matrix2(); + + Matrix2 Clone() const; + + static Matrix2 Zero(int nRows, int nCols); + void Clear(); + + static Matrix2 Identity(int size); + + static Matrix2 Diagonal(float f, int size); + + static Matrix2 SkewMatrix(const Vector3& v); + + Matrix2 Transpose() const; + + Matrix2 operator-() const; + + /// @brief Add a matrix to this matrix + /// @param m The matrix to add to this matrix + /// @return The result of the addition + Matrix2 operator+(const Matrix2& v) const; + Matrix2 operator+=(const Matrix2& v); + + Matrix2 operator*(const Matrix2& m) const; + friend Matrix2 operator*(const Matrix2& m, float f) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = m.data[ix] * f; + return r; + } + friend Matrix2 operator*(float f, const Matrix2& m) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = f * m.data[ix]; + return r; + } + + friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) { + Matrix1 r = Matrix1(m.nRows); + for (int rowIx = 0; rowIx < m.nRows; rowIx++) { + int mRowIx = rowIx * m.nCols; + for (int colIx = 0; colIx < m.nCols; colIx++) + r.data[rowIx] += m.data[mRowIx + colIx] * v.data[rowIx]; + } + return r; + } + + friend Matrix2 operator/(const Matrix2& m, float f) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = m.data[ix] / f; + return r; + } + friend Matrix2 operator/(float f, const Matrix2& m) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = f / m.data[ix]; + return r; + } + + Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop); + + void UpdateSlice(int rowStart, + int rowStop, + int colStart, + int colStop, + const Matrix2& m) const; + // private: + // move constructor and move assignment operator + Matrix2(Matrix2&& other) noexcept; + Matrix2& operator=(Matrix2&& other) noexcept; + + static Matrix2 Omega(const Vector3& v); + + private: + bool externalData = true; +}; + /// @brief Single precision float matrix template class MatrixOf { @@ -116,6 +225,6 @@ class MatrixOf { }; } // namespace LinearAlgebra -using namespace LinearAlgebra; +// using namespace LinearAlgebra; #endif \ No newline at end of file diff --git a/LinearAlgebra/Quaternion.cpp b/LinearAlgebra/Quaternion.cpp index 1a7352e..dda8c18 100644 --- a/LinearAlgebra/Quaternion.cpp +++ b/LinearAlgebra/Quaternion.cpp @@ -6,6 +6,7 @@ #include #include #include "Angle.h" +#include "Matrix.h" #include "Vector3.h" void CopyQuat(const Quat& q1, Quat& q2) { @@ -97,6 +98,28 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) { } } +Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() { + Matrix2 r = Matrix2(3, 3); + + float x = this->x; + float y = this->y; + float z = this->z; + float w = this->w; + + float* data = r.data; + data[0 * 3 + 0] = 1 - 2 * (y * y + z * z); + data[0 * 3 + 1] = 2 * (x * y - w * z); + data[0 * 3 + 2] = 2 * (x * z + w * y); + data[1 * 3 + 0] = 2 * (x * y + w * z); + data[1 * 3 + 1] = 1 - 2 * (x * x + z * z); + data[1 * 3 + 2] = 2 * (y * z - w * x); + data[2 * 3 + 0] = 2 * (x * z - w * y); + data[2 * 3 + 1] = 2 * (y * z + w * x); + data[2 * 3 + 2] = 1 - 2 * (x * x + y * y); + + return r; +} + Quaternion Quaternion::operator*(const Quaternion& r2) const { return Quaternion( this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x, diff --git a/LinearAlgebra/Quaternion.h b/LinearAlgebra/Quaternion.h index d1ddd4e..1687dc9 100644 --- a/LinearAlgebra/Quaternion.h +++ b/LinearAlgebra/Quaternion.h @@ -34,6 +34,8 @@ typedef struct Quat { namespace LinearAlgebra { +class Matrix2; + /// /// A quaternion /// @@ -89,6 +91,8 @@ struct Quaternion : Quat { /// The euler angles performed in the order: Z, X, Y static Vector3 ToAngles(const Quaternion& q); + Matrix2 ToRotationMatrix(); + /// /// Rotate a vector using this quaterion /// diff --git a/LinearAlgebra/Spherical.h b/LinearAlgebra/Spherical.h index c27b558..5cad52c 100644 --- a/LinearAlgebra/Spherical.h +++ b/LinearAlgebra/Spherical.h @@ -21,17 +21,12 @@ class SphericalOf { /// @brief The distance in meters /// @remark The distance should never be negative float distance; - /// @brief The angle in the horizontal plane in degrees, clockwise rotation - /// @details The angle is automatically normalized to -180 .. 180 - // AngleOf horizontal; - /// @brief The angle in the vertical plane in degrees. Positive is upward. - /// @details The angle is automatically normalized to -180 .. 180 - // AngleOf vertical; + /// @brief The direction of the vector DirectionOf direction; - SphericalOf(); - SphericalOf(float distance, AngleOf horizontal, AngleOf vertical); - SphericalOf(float distance, DirectionOf direction); + SphericalOf(); + SphericalOf(float distance, AngleOf horizontal, AngleOf vertical); + SphericalOf(float distance, DirectionOf direction); /// @brief Create spherical vector without using AngleOf type. All given /// angles are in degrees diff --git a/LinearAlgebra/SwingTwist.h b/LinearAlgebra/SwingTwist.h index dc512ce..1800e43 100644 --- a/LinearAlgebra/SwingTwist.h +++ b/LinearAlgebra/SwingTwist.h @@ -21,9 +21,9 @@ class SwingTwistOf { DirectionOf swing; AngleOf twist; - SwingTwistOf(); - SwingTwistOf(DirectionOf swing, AngleOf twist); - SwingTwistOf(AngleOf horizontal, AngleOf vertical, AngleOf twist); + SwingTwistOf(); + SwingTwistOf(DirectionOf swing, AngleOf twist); + SwingTwistOf(AngleOf horizontal, AngleOf vertical, AngleOf twist); static SwingTwistOf Degrees(float horizontal, float vertical = 0, diff --git a/LinearAlgebra/Vector3.h b/LinearAlgebra/Vector3.h index 8775b2d..914d72a 100644 --- a/LinearAlgebra/Vector3.h +++ b/LinearAlgebra/Vector3.h @@ -14,7 +14,7 @@ extern "C" { /// This is a C-style implementation /// This uses the right-handed coordinate system. typedef struct Vec3 { - protected: + public: /// /// The right axis of the vector /// diff --git a/LinearAlgebra/test/Matrix_test.cc b/LinearAlgebra/test/Matrix_test.cc index a1267cf..4cb66b8 100644 --- a/LinearAlgebra/test/Matrix_test.cc +++ b/LinearAlgebra/test/Matrix_test.cc @@ -1,10 +1,90 @@ #if GTEST #include -#include #include +#include #include "Matrix.h" +TEST(Matrix2, Zero) { + // Test case 1: 2x2 zero matrix + Matrix2 zeroMatrix = Matrix2::Zero(2, 2); + EXPECT_TRUE(zeroMatrix.nRows == 2); + EXPECT_TRUE(zeroMatrix.nCols == 2); + for (int i = 0; i < zeroMatrix.nValues; ++i) { + EXPECT_TRUE(zeroMatrix.data[i] == 0.0f); + } + std::cout << "Test case 1 passed: 2x2 zero matrix\n"; + + // Test case 2: 3x3 zero matrix + zeroMatrix = Matrix2::Zero(3, 3); + EXPECT_TRUE(zeroMatrix.nRows == 3); + EXPECT_TRUE(zeroMatrix.nCols == 3); + for (int i = 0; i < zeroMatrix.nValues; ++i) { + EXPECT_TRUE(zeroMatrix.data[i] == 0.0f); + } + std::cout << "Test case 2 passed: 3x3 zero matrix\n"; + + // Test case 3: 1x1 zero matrix + zeroMatrix = Matrix2::Zero(1, 1); + EXPECT_TRUE(zeroMatrix.nRows == 1); + EXPECT_TRUE(zeroMatrix.nCols == 1); + EXPECT_TRUE(zeroMatrix.data[0] == 0.0f); + std::cout << "Test case 3 passed: 1x1 zero matrix\n"; + + // Test case 4: 0x0 matrix (edge case) + zeroMatrix = Matrix2::Zero(0, 0); + EXPECT_TRUE(zeroMatrix.nRows == 0); + EXPECT_TRUE(zeroMatrix.nCols == 0); + EXPECT_TRUE(zeroMatrix.data == nullptr); + std::cout << "Test case 4 passed: 0x0 matrix\n"; +} + +TEST(Matrix2, Multiplication) { + // Test 1: Multiplying two 2x2 matrices + float dataA[] = {1, 2, 3, 4}; + float dataB[] = {5, 6, 7, 8}; + Matrix2 A(dataA, 2, 2); + Matrix2 B(dataB, 2, 2); + + Matrix2 result = A * B; + + float expectedData[] = {19, 22, 43, 50}; + for (int i = 0; i < 4; ++i) + EXPECT_TRUE(result.data[i] == expectedData[i]); + std::cout << "Test 1 passed: 2x2 matrix multiplication.\n"; + + + // Test 2: Multiplying a 3x2 matrix with a 2x3 matrix + float dataC[] = {1, 2, 3, 4, 5, 6}; + float dataD[] = {7, 8, 9, 10, 11, 12}; + Matrix2 C(dataC, 3, 2); + Matrix2 D(dataD, 2, 3); + + Matrix2 result2 = C * D; + + float expectedData2[] = {27, 30, 33, 61, 68, 75, 95, 106, 117}; + for (int i = 0; i < 9; ++i) + EXPECT_TRUE(result2.data[i] == expectedData2[i]); + std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n"; + + // Test 3: Multiplying with a zero matrix + Matrix2 zeroMatrix = Matrix2::Zero(2, 2); + Matrix2 result3 = A * zeroMatrix; + + for (int i = 0; i < 4; ++i) + EXPECT_TRUE(result3.data[i] == 0); + std::cout << "Test 3 passed: Multiplication with zero matrix.\n"; + + // Test 4: Multiplying with an identity matrix + Matrix2 identityMatrix = Matrix2::Identity(2); + Matrix2 result4 = A * identityMatrix; + + for (int i = 0; i < 4; ++i) + EXPECT_TRUE(result4.data[i] == A.data[i]); + std::cout << "Test 4 passed: Multiplication with identity matrix.\n"; + +} + TEST(MatrixSingle, Init) { // zero MatrixOf m0 = MatrixOf(0, 0); diff --git a/Messages/BinaryMsg.cpp b/Messages/BinaryMsg.cpp index 7ab7f5d..0b0b940 100644 --- a/Messages/BinaryMsg.cpp +++ b/Messages/BinaryMsg.cpp @@ -2,33 +2,45 @@ namespace RoboidControl { -BinaryMsg::BinaryMsg(char* buffer) { - unsigned char ix = 1; - this->networkId = buffer[ix++]; - this->thingId = buffer[ix++]; - this->bytes = buffer + ix; // This is only valid because the code ensures the the msg - // lifetime is shorter than the buffer lifetime... -} - BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; this->thing = thing; + unsigned char ix = BinaryMsg::length; + this->dataLength = this->thing->GenerateBinary(this->data, &ix); } -BinaryMsg::~BinaryMsg() {} +BinaryMsg::BinaryMsg(char* buffer) { + unsigned char ix = 1; + this->networkId = buffer[ix++]; + this->thingId = buffer[ix++]; + this->dataLength = buffer[ix++]; + char* data = new char[this->dataLength]; + for (int i = 0; i < this->dataLength; i++) + data[i] = buffer[ix++]; + this->data = data; +} + +BinaryMsg::~BinaryMsg() { + delete[] this->data; +} unsigned char BinaryMsg::Serialize(char* buffer) { - unsigned char ix = this->length; - this->thing->GenerateBinary(buffer, &ix); - if (ix <= this->length) // in this case, no data is actually sent + // unsigned char ix = this->length; + // this->dataLength = this->thing->GenerateBinary(buffer, &ix); + if (this->dataLength <= 0) // in this case, no data is actually sent return 0; - buffer[0] = this->id; - buffer[1] = this->networkId; - buffer[2] = this->thingId; - return ix; +#if defined(DEBUG) + std::cout << "Send BinaryMsg [" << (int)this->networkId << "/" << (int)this->thingId + << "] " << (int)this->dataLength << std::endl; +#endif + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + buffer[ix++] = this->thingId; + buffer[ix++] = this->dataLength; + return this->length + this->dataLength; } - } // namespace RoboidControl diff --git a/Messages/BinaryMsg.h b/Messages/BinaryMsg.h index 0b03111..10adb90 100644 --- a/Messages/BinaryMsg.h +++ b/Messages/BinaryMsg.h @@ -10,7 +10,7 @@ class BinaryMsg : public IMessage { /// @brief The message ID static const unsigned char id = 0xB1; /// @brief The length of the message without the binary data itslef - static const unsigned length = 3; + static const unsigned length = 4; /// @brief The network ID of the thing unsigned char networkId; @@ -19,8 +19,9 @@ class BinaryMsg : public IMessage { /// @brief The thing for which the binary data is communicated Thing* thing; + unsigned char dataLength; /// @brief The binary data which is communicated - char* bytes = nullptr; + char* data = nullptr; /// @brief Create a new message for sending /// @param networkId The network ID of the thing diff --git a/Messages/DestroyMsg.cpp b/Messages/DestroyMsg.cpp index 24fbb63..6141582 100644 --- a/Messages/DestroyMsg.cpp +++ b/Messages/DestroyMsg.cpp @@ -12,6 +12,10 @@ DestroyMsg::DestroyMsg(char* buffer) {} DestroyMsg::~DestroyMsg() {} unsigned char DestroyMsg::Serialize(char *buffer) { + #if defined(DEBUG) + std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId + << "] " << std::endl; +#endif unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->networkId; diff --git a/Messages/InvestigateMsg.cpp b/Messages/InvestigateMsg.cpp index 7bbde42..6420e42 100644 --- a/Messages/InvestigateMsg.cpp +++ b/Messages/InvestigateMsg.cpp @@ -14,6 +14,10 @@ InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) { InvestigateMsg::~InvestigateMsg() {} unsigned char InvestigateMsg::Serialize(char* buffer) { + #if defined(DEBUG) + std::cout << "Send InvestigateMsg [" << (int)this->networkId << "/" << (int)this->thingId + << "] " << std::endl; +#endif unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->networkId; diff --git a/Messages/Messages.cpp b/Messages/Messages.cpp index 422c687..d9f1a8c 100644 --- a/Messages/Messages.cpp +++ b/Messages/Messages.cpp @@ -1,7 +1,6 @@ #include "Messages.h" #include "LowLevelMessages.h" -//#include "Participant.h" #include "string.h" namespace RoboidControl { @@ -10,26 +9,10 @@ namespace RoboidControl { IMessage::IMessage() {} -// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); } - -// IMessage::IMessage(char* buffer) {} - unsigned char IMessage::Serialize(char* buffer) { return 0; } -// bool IMessage::SendMsg(LocalParticipant *client, IMessage msg) { -// // return SendMsg(client, client.buffer, );nameLength -// return client->SendBuffer(msg.Serialize(client->buffer)); -// } - -// bool IMessage::Publish(LocalParticipant *participant) { -// return participant->PublishBuffer(Serialize(participant->buffer)); -// } -// bool IMessage::SendTo(LocalParticipant *participant) { -// return participant->SendBuffer(Serialize(participant->buffer)); -// } - // IMessage #pragma endregion diff --git a/Messages/Messages.h b/Messages/Messages.h index 4d1a2f8..df22544 100644 --- a/Messages/Messages.h +++ b/Messages/Messages.h @@ -6,7 +6,7 @@ namespace RoboidControl { -class LocalParticipant; +class ParticipantUDP; class IMessage { public: @@ -15,8 +15,8 @@ class IMessage { static unsigned char* ReceiveMsg(unsigned char packetSize); - // bool Publish(LocalParticipant *participant); - // bool SendTo(LocalParticipant *participant); + // bool Publish(ParticipantUDP *participant); + // bool SendTo(ParticipantUDP *participant); }; } // namespace RoboidControl diff --git a/Messages/ModelUrlMsg.cpp b/Messages/ModelUrlMsg.cpp index 5a61165..df003be 100644 --- a/Messages/ModelUrlMsg.cpp +++ b/Messages/ModelUrlMsg.cpp @@ -4,16 +4,6 @@ namespace RoboidControl { -// ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId, -// unsigned char urlLength, const char *url, -// float scale) { -// this->networkId = networkId; -// this->thingId = thingId; -// this->urlLength = urlLength; -// this->url = url; -// this->scale = scale; -// } - ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) { this->networkId = networkId; this->thingId = thing->id; @@ -22,21 +12,21 @@ ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) { else this->urlLength = (unsigned char)strlen(thing->modelUrl); - //this->url = thing->modelUrl; // dangerous! - // the url string in the buffer is not \0 terminated! char* url = new char[this->urlLength + 1]; for (int i = 0; i < this->urlLength; i++) url[i] = thing->modelUrl[i]; url[this->urlLength] = '\0'; - this->url = url;} + this->url = url; +} ModelUrlMsg::ModelUrlMsg(const char* buffer) { unsigned char ix = 1; // first byte is msg id this->networkId = buffer[ix++]; this->thingId = buffer[ix++]; this->urlLength = buffer[ix++]; - // this->url = &buffer[ix]; // dangerous! name should not be used anymore after + // this->url = &buffer[ix]; // dangerous! name should not be used anymore + // after // // buffer has been re-used... // the url string in the buffer is not \0 terminated! @@ -54,6 +44,11 @@ ModelUrlMsg::~ModelUrlMsg() { unsigned char ModelUrlMsg::Serialize(char* buffer) { if (this->urlLength == 0 || this->url == nullptr) return 0; +#if defined(DEBUG) + std::cout << "Send ModelUrlMsg [" << (int)this->networkId << "/" + << (int)this->thingId << "] " << (int)this->urlLength << " " + << this->url << std::endl; +#endif unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->networkId; diff --git a/Messages/ModelUrlMsg.h b/Messages/ModelUrlMsg.h index 27e5e25..7c493eb 100644 --- a/Messages/ModelUrlMsg.h +++ b/Messages/ModelUrlMsg.h @@ -8,14 +8,14 @@ class ModelUrlMsg : public IMessage { /// @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; + 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 url st5ring, excluding the null terminator + /// @brief The length of the url string, excluding the null terminator unsigned char urlLength; /// @brief The url of the model, not terminated by a null character const char* url; diff --git a/Messages/NameMsg.cpp b/Messages/NameMsg.cpp index 31b5d0e..da91c79 100644 --- a/Messages/NameMsg.cpp +++ b/Messages/NameMsg.cpp @@ -25,6 +25,7 @@ NameMsg::NameMsg(const char* buffer) { this->networkId = buffer[ix++]; this->thingId = buffer[ix++]; this->nameLength = buffer[ix++]; + // the name string in the buffer is not \0 terminated! char* name = new char[this->nameLength + 1]; for (int i = 0; i < this->nameLength; i++) @@ -41,6 +42,11 @@ unsigned char NameMsg::Serialize(char* buffer) { if (this->nameLength == 0 || this->name == nullptr) return 0; +#if defined(DEBUG) + std::cout << "Send NameMsg [" << (int)this->networkId << "/" + << (int)this->thingId << "] " << (int)this->nameLength << " " + << this->name << std::endl; +#endif unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->networkId; diff --git a/Messages/ParticipantMsg.cpp b/Messages/ParticipantMsg.cpp index e989f39..c91fce9 100644 --- a/Messages/ParticipantMsg.cpp +++ b/Messages/ParticipantMsg.cpp @@ -13,13 +13,19 @@ ParticipantMsg::ParticipantMsg(const char* buffer) { ParticipantMsg::~ParticipantMsg() {} unsigned char ParticipantMsg::Serialize(char* buffer) { +#if defined(DEBUG) + std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] " + << std::endl; +#endif + unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->networkId; return ParticipantMsg::length; } -// bool ParticipantMsg::Send(LocalParticipant *participant, unsigned char networkId) { +// bool ParticipantMsg::Send(ParticipantUDP *participant, unsigned char +// networkId) { // ParticipantMsg msg = ParticipantMsg() // } // Client Msg diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index 9bafa6f..c6d41aa 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -11,12 +11,10 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { if (thing->positionUpdated || force) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; - thing->positionUpdated = false; } if (thing->orientationUpdated || force) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; - thing->orientationUpdated = false; } if (thing->linearVelocityUpdated) { this->linearVelocity = thing->GetLinearVelocity(); @@ -47,6 +45,10 @@ unsigned char PoseMsg::Serialize(char* buffer) { if (this->poseType == 0) return 0; +#if defined(DEBUG) + std::cout << "Send PoseMsg [" << (int)this->networkId << "/" + << (int)this->thingId << "] " << (int)this->poseType << std::endl; +#endif unsigned char ix = 0; buffer[ix++] = PoseMsg::id; buffer[ix++] = this->networkId; diff --git a/Messages/SiteMsg.cpp b/Messages/SiteMsg.cpp index d9f7afd..598a841 100644 --- a/Messages/SiteMsg.cpp +++ b/Messages/SiteMsg.cpp @@ -13,6 +13,9 @@ SiteMsg::SiteMsg(unsigned char networkId) { SiteMsg::~SiteMsg() {} unsigned char SiteMsg::Serialize(char* buffer) { +#if defined(DEBUG) + std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl; +#endif unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->networkId; diff --git a/Messages/TextMsg.cpp b/Messages/TextMsg.cpp index 371bf92..119a907 100644 --- a/Messages/TextMsg.cpp +++ b/Messages/TextMsg.cpp @@ -24,6 +24,9 @@ unsigned char TextMsg::Serialize(char* buffer) { if (this->textLength == 0 || this->text == nullptr) return 0; +#if defined(DEBUG) + std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl; +#endif unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->textLength; diff --git a/Messages/ThingMsg.cpp b/Messages/ThingMsg.cpp index c22d58d..932530f 100644 --- a/Messages/ThingMsg.cpp +++ b/Messages/ThingMsg.cpp @@ -32,6 +32,10 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) { ThingMsg::~ThingMsg() {} unsigned char ThingMsg::Serialize(char* buffer) { +#if defined(DEBUG) + std::cout << "Send ThingMsg [" << (int)this->networkId << "/" << (int)this->thingId + << "] " << (int)this->thingType << " " << (int)this->parentId << std::endl; +#endif unsigned char ix = 0; buffer[ix++] = this->id; buffer[ix++] = this->networkId; diff --git a/Participant.cpp b/Participant.cpp index c690c75..65a1f51 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -27,15 +27,20 @@ Participant::~Participant() { delete[] this->ipAddress; } -Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { +void Participant::Update(unsigned long currentTimeMs) { + for (Thing* thing : this->things) { + if (thing != nullptr) + thing->Update(currentTimeMs, true); + } +} + +Thing* Participant::Get(unsigned char thingId) { for (Thing* thing : this->things) { - // if (thing->networkId == networkId && thing->id == thingId) if (thing->id == thingId) return thing; } - // std::cout << "Could not find thing " << this->ipAddress << ":" << - // this->port - // << "[" << (int)networkId << "/" << (int)thingId << "]\n"; + // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port + // << "[" << (int)thingId << "]\n"; return nullptr; } @@ -49,25 +54,22 @@ void Participant::Add(Thing* thing, bool checkId) { thing->id = (unsigned char)this->things.size() + 1; this->things.push_back(thing); #endif - // std::cout << "Add thing with generated ID " << this->ipAddress << ":" << - // this->port << "[" << (int)thing->networkId << "/" - // << (int)thing->id << "]\n"; + // std::cout << "Add thing with generated ID " << this->ipAddress << ":" + // << this->port << "[" << (int)thing->id << "]\n"; } else { - Thing* foundThing = Get(thing->networkId, thing->id); + Thing* foundThing = Get(thing->id); if (foundThing == nullptr) { #if defined(NO_STD) this->things[this->thingCount++] = thing; #else this->things.push_back(thing); #endif - // std::cout << "Add thing " << this->ipAddress << ":" << this->port << - // "[" << (int)thing->networkId << "/" + // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "[" // << (int)thing->id << "]\n"; + } else { + // std::cout << "Did not add, existing thing " << this->ipAddress << ":" + // << this->port << "[" << (int)thing->id << "]\n"; } - // else - // std::cout << "Did not add, existing thing " << this->ipAddress << ":" - // << this->port << "[" - // << (int)thing->networkId << "/" << (int)thing->id << "]\n"; } } diff --git a/Participant.h b/Participant.h index 94c5861..5fdffc4 100644 --- a/Participant.h +++ b/Participant.h @@ -33,6 +33,8 @@ class Participant { /// @brief Destructor for the participant ~Participant(); + virtual void Update(unsigned long currentTimeMs = 0); + protected: #if defined(NO_STD) unsigned char thingCount = 0; @@ -48,7 +50,7 @@ class Participant { /// @param thingId The ID of the thing /// @return The thing if found or nullptr when no thing has been found /// @note The use of the network ID is likely to disappear in future versions. - Thing* Get(unsigned char networkId, unsigned char thingId); + Thing* Get(unsigned char thingId); /// @brief Add a new thing for this participant. /// @param thing The thing to add /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing diff --git a/Participants/IsolatedParticipant.cpp b/Participants/IsolatedParticipant.cpp new file mode 100644 index 0000000..a89eab4 --- /dev/null +++ b/Participants/IsolatedParticipant.cpp @@ -0,0 +1,14 @@ +#include "IsolatedParticipant.h" +#include "ParticipantUDP.h" + +namespace RoboidControl { + +static ParticipantUDP* isolatedParticipant = nullptr; + +Participant* IsolatedParticipant::Isolated() { + if (isolatedParticipant == nullptr) + isolatedParticipant = new ParticipantUDP(0); + return isolatedParticipant; +} + +} // namespace RoboidControl \ No newline at end of file diff --git a/Participants/IsolatedParticipant.h b/Participants/IsolatedParticipant.h new file mode 100644 index 0000000..27e9e80 --- /dev/null +++ b/Participants/IsolatedParticipant.h @@ -0,0 +1,13 @@ +#include "Participant.h" + +namespace RoboidControl { + +class IsolatedParticipant { + public: + /// @brief Isolated participant is used when the application is run without + /// networking + /// @return A participant without networking support + static Participant* Isolated(); +}; + +} \ No newline at end of file diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp new file mode 100644 index 0000000..f643587 --- /dev/null +++ b/Participants/ParticipantUDP.cpp @@ -0,0 +1,421 @@ +#include "ParticipantUDP.h" + +#include "Thing.h" + +#include "Arduino/ArduinoParticipant.h" +#include "EspIdf/EspIdfParticipant.h" + +#if defined(_WIN32) || defined(_WIN64) +#include +#include +#include "Windows/WindowsParticipant.h" +#pragma comment(lib, "ws2_32.lib") + +#elif defined(__unix__) || defined(__APPLE__) +#include +#include // For fcntl +#include +#include +#include +#include +#include "Posix/PosixParticipant.h" +#endif + +#include + +namespace RoboidControl { + +ParticipantUDP::ParticipantUDP(int port) { + this->ipAddress = "0.0.0.0"; + this->port = port; + this->remoteSite = nullptr; + if (this->port == 0) + this->isIsolated = true; +} + +ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) + : Participant("127.0.0.1", localPort) { + if (this->port == 0) + this->isIsolated = true; + else + this->remoteSite = new Participant(ipAddress, port); +} + +static ParticipantUDP* isolatedParticipant = nullptr; + +ParticipantUDP* ParticipantUDP::Isolated() { + if (isolatedParticipant == nullptr) + isolatedParticipant = new ParticipantUDP(0); + return isolatedParticipant; +} + +void ParticipantUDP::begin() { + if (this->isIsolated) + return; + + SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port); +} + +void ParticipantUDP::SetupUDP(int localPort, + const char* remoteIpAddress, + int remotePort) { +#if defined(_WIN32) || defined(_WIN64) + Windows::ParticipantUDP* thisWindows = + static_cast(this); + thisWindows->Setup(localPort, remoteIpAddress, remotePort); +#elif defined(__unix__) || defined(__APPLE__) + Posix::ParticipantUDP* thisPosix = static_cast(this); + thisPosix->Setup(localPort, remoteIpAddress, remotePort); +#elif defined(ARDUINO) + Arduino::ParticipantUDP* thisArduino = + static_cast(this); + thisArduino->Setup(localPort, remoteIpAddress, remotePort); +#elif defined(IDF_VER) + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); + thisEspIdf->Setup(localPort, remoteIpAddress, remotePort); +#endif + this->connected = true; +} + +void ParticipantUDP::Update(unsigned long currentTimeMs) { + if (currentTimeMs == 0) + currentTimeMs = Thing::GetTimeMs(); + + if (this->isIsolated == false) { + if (this->connected == false) + begin(); + + if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { + ParticipantMsg* msg = new ParticipantMsg(this->networkId); + if (this->remoteSite == nullptr) + this->Publish(msg); + else + this->Send(this->remoteSite, msg); + delete msg; + + this->nextPublishMe = currentTimeMs + this->publishInterval; + } + + this->ReceiveUDP(); + } + + for (Thing* thing : this->things) { + if (thing == nullptr) + continue; + + if (this->isIsolated == false) { + PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); + this->Send(thing->owner, poseMsg); + BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + this->Send(thing->owner, binaryMsg); + delete poseMsg; + } + thing->Update(currentTimeMs, true); + } +} + +void ParticipantUDP::ReceiveUDP() { +#if defined(_WIN32) || defined(_WIN64) + Windows::ParticipantUDP* thisWindows = + static_cast(this); + thisWindows->Receive(); +#elif defined(__unix__) || defined(__APPLE__) + Posix::ParticipantUDP* thisPosix = static_cast(this); + thisPosix->Receive(); +#elif defined(ARDUINO) + Arduino::ParticipantUDP* thisArduino = + static_cast(this); + thisArduino->Receive(); +#elif defined(IDF_VER) + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); + thisEspIdf->Receive(); +#endif +} + +Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) { + for (Participant* sender : this->senders) { + if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port) + return sender; + } + return nullptr; +} + +Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) { + // std::cout << "New Participant " << ipAddress << ":" << port << "\n"; + Participant* participant = new Participant(ipAddress, port); +#if defined(NO_STD) + participant->networkId = this->senderCount; + this->senders[this->senderCount++] = participant; +#else + participant->networkId = (unsigned char)this->senders.size(); + this->senders.push_back(participant); +#endif + return participant; +} + +#pragma region Send + +void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, + Thing* thing) { + // std::cout << "Send thing info [" << (int)thing->id << "] \n"; + ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); + this->Send(remoteParticipant, thingMsg); + delete thingMsg; + NameMsg* nameMsg = new NameMsg(this->networkId, thing); + this->Send(remoteParticipant, nameMsg); + delete nameMsg; + ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); + this->Send(remoteParticipant, modelMsg); + delete modelMsg; + PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true); + this->Send(remoteParticipant, poseMsg); + delete poseMsg; + BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing); + this->Send(remoteParticipant, customMsg); + delete customMsg; +} + +bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { + int bufferSize = msg->Serialize(this->buffer); + if (bufferSize <= 0) + return true; + +#if defined(_WIN32) || defined(_WIN64) + Windows::ParticipantUDP* thisWindows = + static_cast(this); + return thisWindows->Send(remoteParticipant, bufferSize); +#elif defined(__unix__) || defined(__APPLE__) + Posix::ParticipantUDP* thisPosix = static_cast(this); + return thisPosix->Send(remoteParticipant, bufferSize); +#elif defined(ARDUINO) + Arduino::ParticipantUDP* thisArduino = + static_cast(this); + return thisArduino->Send(remoteParticipant, bufferSize); +#elif defined(IDF_VER) + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); + return thisEspIdf->Send(remoteParticipant, bufferSize); +#else + return false; +#endif +} + +void ParticipantUDP::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); + this->Publish(thingMsg); + delete thingMsg; + NameMsg* nameMsg = new NameMsg(this->networkId, thing); + this->Publish(nameMsg); + delete nameMsg; + ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); + this->Publish(modelMsg); + delete modelMsg; + PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true); + this->Publish(poseMsg); + delete poseMsg; + BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing); + this->Publish(customMsg); + delete customMsg; +} + +bool ParticipantUDP::Publish(IMessage* msg) { +#if defined(_WIN32) || defined(_WIN64) + Windows::ParticipantUDP* thisWindows = + static_cast(this); + return thisWindows->Publish(msg); +#elif defined(__unix__) || defined(__APPLE__) + Posix::ParticipantUDP* thisPosix = static_cast(this); + return thisPosix->Publish(msg); +#elif defined(ARDUINO) + Arduino::ParticipantUDP* thisArduino = + static_cast(this); + return thisArduino->Publish(msg); +#elif defined(IDF_VER) + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); + return thisEspIdf->Publish(msg); +#else + return false; +#endif +} + +// Send +#pragma endregion + +#pragma region Receive + +void ParticipantUDP::ReceiveData(unsigned char packetSize, + char* senderIpAddress, + unsigned int senderPort) { + Participant* sender = this->GetParticipant(senderIpAddress, senderPort); + if (sender == nullptr) { + sender = this->AddParticipant(senderIpAddress, senderPort); + std::cout << "New remote participant " << sender->ipAddress << ":" + << sender->port << std::endl; + } + + ReceiveData(packetSize, sender); +} + +void ParticipantUDP::ReceiveData(unsigned char bufferSize, + Participant* sender) { + unsigned char msgId = this->buffer[0]; + // std::cout << "receive msg " << (int)msgId << "\n"; + // std::cout << " buffer size = " <<(int) bufferSize << "\n"; + switch (msgId) { + case ParticipantMsg::id: { + ParticipantMsg* msg = new ParticipantMsg(this->buffer); + bufferSize -= msg->length; + Process(sender, msg); + delete msg; + } break; + case SiteMsg::id: { + SiteMsg* msg = new SiteMsg(this->buffer); + bufferSize -= msg->length; + Process(sender, msg); + delete msg; + } break; + case InvestigateMsg::id: { + InvestigateMsg* msg = new InvestigateMsg(this->buffer); + Process(sender, msg); + delete msg; + } break; + case ThingMsg::id: { + ThingMsg* msg = new ThingMsg(this->buffer); + bufferSize -= msg->length; + Process(sender, msg); + delete msg; + } break; + case NameMsg::id: { + NameMsg* msg = new NameMsg(this->buffer); + bufferSize -= msg->length + msg->nameLength; + Process(sender, msg); + delete msg; + } break; + case ModelUrlMsg::id: { + ModelUrlMsg* msg = new ModelUrlMsg(this->buffer); + bufferSize -= msg->length + msg->urlLength; + Process(sender, msg); + delete msg; + } break; + case PoseMsg::id: { + PoseMsg* msg = new PoseMsg(this->buffer); + bufferSize -= msg->length; + Process(sender, msg); + delete msg; + } break; + case BinaryMsg::id: { + BinaryMsg* msg = new BinaryMsg(this->buffer); + bufferSize -= msg->length + msg->dataLength; + Process(sender, msg); + delete msg; + } break; + }; + + // Check if the buffer has been read completely + if (bufferSize > 0) + std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n"; +} + +void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId + << "\n"; +#endif +} + +void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process SiteMsg " << (int)this->networkId + << " -> " << (int)msg->networkId << "\n"; +#endif + + if (this->networkId != msg->networkId) { + this->networkId = msg->networkId; + // std::cout << this->things.size() << " things\n"; + for (Thing* thing : this->things) + this->SendThingInfo(sender, thing); + } +} + +void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId + << "/" << (int)msg->thingId << "]\n"; +#endif +} + +void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId + << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " << (int)msg->parentId << "\n"; +#endif +} + +void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/" + << (int)msg->thingId << "] "; +#endif + + Thing* thing = sender->Get(msg->thingId); + if (thing != nullptr) { + int nameLength = msg->nameLength; + int stringLen = nameLength + 1; + char* thingName = new char[stringLen]; +#if defined(_WIN32) || defined(_WIN64) + strncpy_s(thingName, stringLen, msg->name, + stringLen - 1); // Leave space for null terminator +#else + // Use strncpy with bounds checking for other platforms (Arduino, POSIX, + // ESP-IDF) + strncpy(thingName, msg->name, + nameLength); // Leave space for null terminator +#endif + thingName[nameLength] = '\0'; + thing->name = thingName; + + std::cout << thing->name; + } + std::cout << std::endl; +} + +void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId + << "/" << (int)msg->thingId << "]\n"; +#endif +} + +void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process PoseMsg [" << (int)this->networkId + << "/" << (int)msg->networkId << "]\n"; +#endif +} + +void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { +#if defined(DEBUG) + std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId + << "/" << (int)msg->thingId << "] "; +#endif + + Thing* thing = sender->Get(msg->thingId); + if (thing != nullptr) + thing->ProcessBinary(msg->data); + else { + std::cout << " unknown thing [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]"; + } + std::cout << std::endl; +} + +// Receive +#pragma endregion + +} // namespace RoboidControl diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h new file mode 100644 index 0000000..ec93af9 --- /dev/null +++ b/Participants/ParticipantUDP.h @@ -0,0 +1,173 @@ +#pragma once + +#include "Messages/BinaryMsg.h" +#include "Messages/InvestigateMsg.h" +#include "Messages/ModelUrlMsg.h" +#include "Messages/NameMsg.h" +#include "Messages/ParticipantMsg.h" +#include "Messages/PoseMsg.h" +#include "Messages/SiteMsg.h" +#include "Messages/ThingMsg.h" +#include "Participant.h" + +#if !defined(NO_STD) +#include +#include +// #include +#endif + +#if defined(_WIN32) || defined(_WIN64) +#include +#elif defined(__unix__) || defined(__APPLE__) +#include +#include +#include +#include +#endif + +namespace RoboidControl { + +constexpr int MAX_SENDER_COUNT = 256; + +/// @brief A local participant is the local device which can communicate with +/// other participants It manages all local things and communcation with other +/// participants. Each application has a local participant which is usually +/// explicit in the code. An participant can be isolated. In that case it is +/// standalong and does not communicate with other participants. +/// +/// It is possible to work with an hidden participant by creating things without +/// specifying a participant in the constructor. In that case an hidden isolated +/// participant is created which can be obtained using +/// RoboidControl::IsolatedParticipant::Isolated(). +/// @sa RoboidControl::Thing::Thing() +class ParticipantUDP : public Participant { + public: + /// @brief Create a participant without connecting to a site + /// @param port The port on which the participant communicates + /// These participant typically broadcast Participant messages to let site + /// servers on the local network know their presence. Alternatively they can + /// broadcast information which can be used directly by other participants. + ParticipantUDP(int port = 7681); + /// @brief Create a participant which will try to connect to a site. + /// @param ipAddress The IP address of the site + /// @param port The port used by the site + ParticipantUDP(const char* ipAddress, + int port = 7681, + int localPort = 7681); + // Note to self: one cannot specify the port used by the local participant + // now!! + + /// @brief Isolated participant is used when the application is run without + /// networking + /// @return A participant without networking support + static ParticipantUDP* Isolated(); + + /// @brief True if the participant is running isolated. + /// Isolated participants do not communicate with other participants + bool isIsolated = false; + + /// The interval in milliseconds for publishing (broadcasting) data on the + /// local network + long publishInterval = 3000; // 3 seconds + + /// @brief The name of the participant + const char* name = "ParticipantUDP"; + + // int localPort = 0; + + /// @brief The remote site when this participant is connected to a site + Participant* remoteSite = nullptr; + +#if defined(ARDUINO) + // const char* remoteIpAddress = nullptr; + // unsigned short remotePort = 0; + // char* broadcastIpAddress = nullptr; + + // WiFiUDP udp; +#else + +#if defined(__unix__) || defined(__APPLE__) + int sock; +#elif defined(_WIN32) || defined(_WIN64) + sockaddr_in remote_addr; + sockaddr_in server_addr; + sockaddr_in broadcast_addr; +#endif + +#endif + + void begin(); + bool connected = false; + + virtual void Update(unsigned long currentTimeMs = 0) override; + + void SendThingInfo(Participant* remoteParticipant, Thing* thing); + void PublishThingInfo(Thing* thing); + + bool Send(Participant* remoteParticipant, IMessage* msg); + bool Publish(IMessage* msg); + + void ReceiveData(unsigned char bufferSize, + char* senderIpAddress, + unsigned int senderPort); + void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant); + +#if defined(NO_STD) + unsigned char senderCount = 0; + Participant* senders[MAX_SENDER_COUNT]; +#else + std::list senders; +#endif + + protected: + unsigned long nextPublishMe = 0; + + char buffer[1024]; + + void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort); + + Participant* GetParticipant(const char* ipAddress, int port); + Participant* AddParticipant(const char* ipAddress, int port); + + void ReceiveUDP(); + + virtual void Process(Participant* sender, ParticipantMsg* msg); + virtual void Process(Participant* sender, SiteMsg* msg); + virtual void Process(Participant* sender, InvestigateMsg* msg); + virtual void Process(Participant* sender, ThingMsg* msg); + virtual void Process(Participant* sender, NameMsg* msg); + virtual void Process(Participant* sender, ModelUrlMsg* msg); + virtual void Process(Participant* sender, PoseMsg* msg); + virtual void Process(Participant* sender, BinaryMsg* msg); + +#if !defined(NO_STD) +// public: +// using ThingConstructor = std::function; + +// template +// void Register(unsigned char thingType) { +// thingMsgProcessors[thingType] = [](Participant* participant, +// unsigned char networkId, +// unsigned char thingId) { +// return new ThingClass(participant, networkId, thingId); +// }; +// }; + +// template +// void Register2(unsigned char thingType, ThingConstructor f) { +// thingMsgProcessors[thingType] = [f](Participant* participant, +// unsigned char networkId, +// unsigned char thingId) { +// return f(participant, networkId, thingId); +// }; +// }; + +// protected: +// std::unordered_map thingMsgProcessors; + +#endif +}; + +} // namespace RoboidControl diff --git a/SiteServer.cpp b/Participants/SiteServer.cpp similarity index 58% rename from SiteServer.cpp rename to Participants/SiteServer.cpp index ae82d84..8bde494 100644 --- a/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -25,7 +25,7 @@ SiteServer::SiteServer(int port) { SetupUDP(port, ipAddress, 0); #if !defined(NO_STD) - Register((unsigned char)Thing::Type::TemperatureSensor); + // Register((unsigned char)Thing::Type::TemperatureSensor); #endif } @@ -43,21 +43,23 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { void SiteServer::Process(Participant* sender, SiteMsg* msg) {} void SiteServer::Process(Participant* sender, ThingMsg* msg) { - Thing* thing = sender->Get(msg->networkId, msg->thingId); + Thing* thing = sender->Get(msg->thingId); if (thing == nullptr) { -#if defined(NO_STD) - new Thing(sender, msg->networkId, msg->thingId, - (Thing::Type)msg->thingType); -#else - auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType); - Thing* newThing; - if (thingMsgProcessor != thingMsgProcessors.end()) // found item - newThing = - thingMsgProcessor->second(sender, msg->networkId, msg->thingId); - else - newThing = new Thing(sender, msg->networkId, msg->thingId, - (Thing::Type)msg->thingType); -#endif + // #if defined(NO_STD) + new Thing(sender, (Thing::Type)msg->thingType, + msg->thingId); + // #else + // auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType); + // //Thing* newThing; + // if (thingMsgProcessor != thingMsgProcessors.end()) // found item + // //newThing = + // thingMsgProcessor->second(sender, msg->networkId, + // msg->thingId); + // else + // //newThing = + // new Thing(sender, msg->networkId, msg->thingId, + // (Thing::Type)msg->thingType); + // #endif } } diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h new file mode 100644 index 0000000..0fe2974 --- /dev/null +++ b/Participants/SiteServer.h @@ -0,0 +1,46 @@ +#pragma once + +#include "ParticipantUDP.h" + +#if !defined(NO_STD) +#include +#include +#include +#endif + +namespace RoboidControl { + +/// @brief A participant is device which can communicate with other participants +class SiteServer : public ParticipantUDP { + public: + SiteServer(int port = 7681); + + // virtual void Update(unsigned long currentTimeMs = 0) override; + +// #if !defined(NO_STD) +// template +// void Register(unsigned char thingType) { +// thingMsgProcessors[thingType] = [](Participant* participant, +// unsigned char networkId, +// unsigned char thingId) { +// return new ThingClass(participant, networkId, thingId); +// }; +// }; +// #endif + + protected: + unsigned long nextPublishMe = 0; + + virtual void Process(Participant* sender, ParticipantMsg* msg) override; + virtual void Process(Participant* sender, SiteMsg* msg) override; + virtual void Process(Participant* sender, ThingMsg* msg) override; + +// #if !defined(NO_STD) +// using ThingConstructor = std::function; +// std::unordered_map thingMsgProcessors; +// #endif +}; + +} // namespace RoboidControl diff --git a/Posix/PosixParticipant.cpp b/Posix/PosixParticipant.cpp index ff23848..88e18af 100644 --- a/Posix/PosixParticipant.cpp +++ b/Posix/PosixParticipant.cpp @@ -11,7 +11,7 @@ namespace RoboidControl { namespace Posix { -void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) { +void Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(__unix__) || defined(__APPLE__) // Create a UDP socket @@ -63,7 +63,7 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem #endif } -void LocalParticipant::Receive() { +void Receive() { #if defined(__unix__) || defined(__APPLE__) sockaddr_in client_addr; socklen_t len = sizeof(client_addr); @@ -90,7 +90,7 @@ void LocalParticipant::Receive() { #endif } -bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { +bool Send(Participant* remoteParticipant, int bufferSize) { #if defined(__unix__) || defined(__APPLE__) // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port) // << "\n"; @@ -113,7 +113,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { return true; } -bool LocalParticipant::Publish(IMessage* msg) { +bool Publish(IMessage* msg) { #if defined(__unix__) || defined(__APPLE__) int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h index 5f75a05..ec16d32 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -1,11 +1,11 @@ #pragma once -#include "../LocalParticipant.h" +#include "Participants/ParticipantUDP.h" namespace RoboidControl { namespace Posix { -class LocalParticipant : public RoboidControl::LocalParticipant { +class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); diff --git a/README.md b/README.md index f01cb53..28e4abd 100644 --- a/README.md +++ b/README.md @@ -9,9 +9,7 @@ Supporting: - ESP8266 - ESP32 - UNO R4 WiFi - - ESP8266 - - ESP32 - - UNO R4 WiFi + - UNO (without networking support) # Basic components diff --git a/SiteServer.h b/SiteServer.h deleted file mode 100644 index 4b1f9bb..0000000 --- a/SiteServer.h +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include "LocalParticipant.h" - -#if !defined(NO_STD) -#include -#include -#include -#endif - -namespace RoboidControl { - -/// @brief A participant is device which can communicate with other participants -class SiteServer : public LocalParticipant { - public: - SiteServer(int port = 7681); - - // virtual void Update(unsigned long currentTimeMs = 0) override; - -#if !defined(NO_STD) - template - void Register(unsigned char thingType) { - thingMsgProcessors[thingType] = [](Participant* participant, - unsigned char networkId, - unsigned char thingId) { - return new ThingClass(participant, networkId, thingId); - }; - }; -#endif - - protected: - unsigned long nextPublishMe = 0; - - virtual void Process(Participant* sender, ParticipantMsg* msg) override; - virtual void Process(Participant* sender, SiteMsg* msg) override; - virtual void Process(Participant* sender, ThingMsg* msg) override; - -#if !defined(NO_STD) - using ThingConstructor = std::function; - std::unordered_map thingMsgProcessors; -#endif -}; - -} // namespace RoboidControl diff --git a/Thing.cpp b/Thing.cpp index 19f5da9..86e4008 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -1,15 +1,18 @@ #include "Thing.h" -#include "LocalParticipant.h" +#include "Messages/PoseMsg.h" +#include "Participant.h" +#include "Participants/IsolatedParticipant.h" #include -// #include -// #include -// #include -// #include #if defined(ARDUINO) #include "Arduino.h" +#else +#include +#include +#include +#include #endif namespace RoboidControl { @@ -20,14 +23,15 @@ namespace RoboidControl { // return isolatedParticipant; // } -Thing::Thing(int thingType) : Thing(LocalParticipant::Isolated(), thingType) {} +Thing::Thing(int thingType) + : Thing(IsolatedParticipant::Isolated(), thingType) {} -Thing::Thing(Participant* owner, Type thingType) - : Thing(owner, (unsigned char)thingType) {} +Thing::Thing(Participant* owner, Type thingType, unsigned char thingId) + : Thing(owner, (unsigned char)thingType, thingId) {} -Thing::Thing(Participant* owner, int thingType) { +Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { this->owner = owner; - this->id = 0; + this->id = thingId; this->type = thingType; this->networkId = 0; @@ -37,27 +41,27 @@ Thing::Thing(Participant* owner, int thingType) { this->linearVelocity = Spherical::zero; this->angularVelocity = Spherical::zero; - // std::cout << "add thing to participant\n"; - owner->Add(this); + // std::cout << "add thing [" << (int)this->id << "] to owner " + // << this->owner->ipAddress << ":" << this->owner->port << std::endl; + this->owner->Add(this, false); } -Thing::Thing(Participant* owner, - unsigned char networkId, - unsigned char thingId, - Type thingType) { - // no participant reference yet.. - this->owner = owner; - this->networkId = networkId; - this->id = thingId; - this->type = (unsigned char)thingType; +// Thing::Thing(Participant* owner, +// Type thingType, +// int thingId) { +// // no participant reference yet.. +// this->owner = owner; +// this->networkId = networkId; +// this->id = thingId; +// this->type = (unsigned char)thingType; - this->linearVelocity = Spherical::zero; - this->angularVelocity = Spherical::zero; - // std::cout << "Created thing " << (int)this->networkId << "/" << - // (int)this->id - // << "\n"; - owner->Add(this, false); -} +// this->linearVelocity = Spherical::zero; +// this->angularVelocity = Spherical::zero; +// // std::cout << "Created thing " << (int)this->networkId << "/" << +// // (int)this->id +// // << "\n"; +// owner->Add(this, false); +// } void Thing::Terminate() { // Thing::Remove(this); @@ -188,24 +192,29 @@ void Thing::Update(bool recursive) { } void Thing::Update(unsigned long currentTimeMs, bool recursive) { - (void)currentTimeMs; + // if (this->positionUpdated || this->orientationUpdated) + // OnPoseChanged callback + this->positionUpdated = false; + this->orientationUpdated = false; + if (recursive) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { Thing* child = this->children[childIx]; if (child == nullptr) continue; child->Update(currentTimeMs, recursive); -} + } } } void Thing::UpdateThings(unsigned long currentTimeMs) { - LocalParticipant::Isolated()->Update(currentTimeMs); + IsolatedParticipant::Isolated()->Update(currentTimeMs); } -void Thing::GenerateBinary(char* buffer, unsigned char* ix) { +int Thing::GenerateBinary(char* buffer, unsigned char* ix) { (void)buffer; (void)ix; + return 0; } void Thing::ProcessBinary(char* bytes) { (void)bytes; diff --git a/Thing.h b/Thing.h index d7086e8..febe7a9 100644 --- a/Thing.h +++ b/Thing.h @@ -10,7 +10,7 @@ namespace RoboidControl { class Participant; -class LocalParticipant; +class ParticipantUDP; #define THING_STORE_SIZE 256 // IMPORTANT: values higher than 256 will need to change the Thing::id type @@ -43,19 +43,21 @@ class Thing { Thing(int thingType = Type::Undetermined); /// @brief Create a new thing of the given type /// @param thingType The predefined type of thing - Thing(Participant* participant, Type thingType = Type::Undetermined); + Thing(Participant* participant, + Type thingType = Type::Undetermined, + unsigned char thingId = 0); /// @brief Create a new thing of the give type /// @param thingType The custom type of the thing - Thing(Participant* participant, int thingType); + Thing(Participant* participant, int thingType, unsigned char thingId = 0); /// @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(Participant* participant, - unsigned char networkId, - unsigned char thingId, - Type thingType = Type::Undetermined); + // Thing(Participant* participant, + // unsigned char networkId, + // unsigned char thingId, + // Type thingType = Type::Undetermined); /// @brief The participant managing this thing Participant* owner; @@ -192,7 +194,8 @@ class Thing { /// @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); + /// @returns The size of the binary data + virtual int 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); diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index 2667d73..45718a5 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -4,22 +4,19 @@ namespace RoboidControl { -// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {} - -// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {} - TemperatureSensor::TemperatureSensor(Participant* participant, - unsigned char networkId, unsigned char thingId) - : Thing(participant, networkId, thingId, Type::TemperatureSensor) {} + : Thing(participant, Type::TemperatureSensor, thingId) {} void TemperatureSensor::SetTemperature(float temp) { this->temperature = temp; } -void TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) { +int TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) { + unsigned char startIx = *ix; // std::cout << "Send temperature: " << this->temperature << "\n"; LowLevelMessages::SendFloat16(buffer, ix, this->temperature); + return *ix - startIx; } void TemperatureSensor::ProcessBinary(char* bytes) { diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index e6ea177..8d58d9c 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -15,7 +15,7 @@ class TemperatureSensor : public Thing { /// @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(Participant* participant, unsigned char networkId, unsigned char thingId); + TemperatureSensor(Participant* participant, unsigned char thingId); /// @brief Manually override the measured temperature /// @param temperature The new temperature @@ -24,7 +24,7 @@ class TemperatureSensor : public Thing { /// @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; + int 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; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index f7f65f0..b5743f6 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -10,7 +10,9 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) { this->SetParent(parent); } -void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {} +int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) { + return 0; +} void TouchSensor::ProcessBinary(char* bytes) { // if (bytes[0] == 1) diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index 83ecc8b..39638e7 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -25,7 +25,7 @@ class TouchSensor : public Thing { /// @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; + int 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; diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index 9b5a8b3..d64eb1e 100644 --- a/Windows/WindowsParticipant.cpp +++ b/Windows/WindowsParticipant.cpp @@ -4,36 +4,22 @@ #include #include #pragma comment(lib, "ws2_32.lib") - -#elif defined(__unix__) || defined(__APPLE__) -#include -#include // For fcntl -#include -#include -#include #endif namespace RoboidControl { namespace Windows { -void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) { +void ParticipantUDP::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; @@ -41,13 +27,8 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem } // 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 @@ -56,12 +37,8 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem 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; } } @@ -72,30 +49,22 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem 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 +#endif // _WIN32 || _WIN64 } -void LocalParticipant::Receive() { +void ParticipantUDP::Receive() { #if defined(_WIN32) || defined(_WIN64) // char ip_str[INET_ADDRSTRLEN]; // inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN); @@ -103,28 +72,20 @@ void LocalParticipant::Receive() { // << 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); unsigned int sender_port = ntohs(client_addr.sin_port); ReceiveData(packetSize, sender_ipAddress, sender_port); - // RoboidControl::LocalParticipant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port); + // RoboidControl::ParticipantUDP* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port); // if (remoteParticipant == nullptr) { // remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port); // // std::cout << "New sender " << sender_ipAddress << ":" @@ -138,16 +99,16 @@ void LocalParticipant::Receive() { // ReceiveData(packetSize, remoteParticipant); } -#endif +#endif // _WIN32 || _WIN64 } -bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::Send(Participant* 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; @@ -155,18 +116,11 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { 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 +#endif // _WIN32 || _WIN64 return true; } -bool LocalParticipant::Publish(IMessage* msg) { +bool ParticipantUDP::Publish(IMessage* msg) { #if defined(_WIN32) || defined(_WIN64) int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) @@ -176,7 +130,7 @@ bool LocalParticipant::Publish(IMessage* msg) { 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; @@ -184,14 +138,7 @@ bool LocalParticipant::Publish(IMessage* msg) { 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 +#endif // _WIN32 || _WIN64 return true; } diff --git a/Windows/WindowsParticipant.h b/Windows/WindowsParticipant.h index c0ec0ed..9a747d9 100644 --- a/Windows/WindowsParticipant.h +++ b/Windows/WindowsParticipant.h @@ -1,11 +1,11 @@ #pragma once -#include "../LocalParticipant.h" +#include "Participants/ParticipantUDP.h" namespace RoboidControl { namespace Windows { -class LocalParticipant : public RoboidControl::LocalParticipant { +class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); diff --git a/test/participant_test.cc b/test/participant_test.cc index 70a5e09..c826863 100644 --- a/test/participant_test.cc +++ b/test/participant_test.cc @@ -9,7 +9,7 @@ // #include #include "Participant.h" -#include "SiteServer.h" +#include "Participants/SiteServer.h" #include "Thing.h" using namespace RoboidControl; @@ -52,8 +52,8 @@ protected: -// TEST_F(ParticipantSuite, LocalParticipant) { -// LocalParticipant* participant = new LocalParticipant("127.0.0.1", 7681); +// TEST_F(ParticipantSuite, ParticipantUDP) { +// ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681); // unsigned long milliseconds = get_time_ms(); // unsigned long startTime = milliseconds; @@ -80,7 +80,7 @@ protected: // TEST_F(ParticipantSuite, SiteParticipant) { // SiteServer site = SiteServer(7681); -// LocalParticipant participant = LocalParticipant("127.0.0.1", 7681); +// ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681); // unsigned long milliseconds = get_time_ms(); // unsigned long startTime = milliseconds; @@ -96,7 +96,7 @@ protected: // TEST_F(ParticipantSuite, Thing) { // SiteServer site = SiteServer(7681); -// LocalParticipant participant = LocalParticipant("127.0.0.1", 7681); +// ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681); // Thing thing = Thing(&participant); // unsigned long milliseconds = get_time_ms(); diff --git a/test/thing_test.cc b/test/thing_test.cc index bc613a8..b8ceea1 100644 --- a/test/thing_test.cc +++ b/test/thing_test.cc @@ -4,7 +4,7 @@ // not supported using Visual Studio 2022 compiler... #include -#include "LocalParticipant.h" +#include "Participants/ParticipantUDP.h" #include "Thing.h" using namespace RoboidControl; @@ -23,7 +23,7 @@ TEST(RoboidControlSuite, HiddenParticipant) { } TEST(RoboidControlSuite, IsolatedParticipant) { - LocalParticipant* participant = LocalParticipant::Isolated(); + ParticipantUDP* participant = ParticipantUDP::Isolated(); Thing* thing = new Thing(participant); unsigned long milliseconds = Thing::GetTimeMs();