From 6ed5bf3ace10c973180308a54b92c12210cf962b Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 7 Mar 2025 12:05:32 +0100 Subject: [PATCH 01/28] Fixed include --- Thing.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/Thing.cpp b/Thing.cpp index 19f5da9..64d0be9 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -3,13 +3,14 @@ #include "LocalParticipant.h" #include -// #include -// #include -// #include -// #include #if defined(ARDUINO) #include "Arduino.h" +#else +#include +#include +#include +#include #endif namespace RoboidControl { @@ -195,7 +196,7 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) { if (child == nullptr) continue; child->Update(currentTimeMs, recursive); -} + } } } From 8ccc826be9f291e7cb04cb8285479a4616cbbd3f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 12 Mar 2025 17:53:00 +0100 Subject: [PATCH 02/28] Cleanup --- LinearAlgebra/Spherical.h | 7 +------ README.md | 4 +--- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/LinearAlgebra/Spherical.h b/LinearAlgebra/Spherical.h index c27b558..86705f9 100644 --- a/LinearAlgebra/Spherical.h +++ b/LinearAlgebra/Spherical.h @@ -21,12 +21,7 @@ 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(); 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 From fdf4d3aff6e9ebd3c9928350d0269afeae61c4bb Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 1 Apr 2025 12:47:23 +0200 Subject: [PATCH 03/28] Successfully compiled the first code with RoboidControl --- LocalParticipant.cpp | 4 ++++ LocalParticipant.h | 6 +++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index 5cb4793..5a968c7 100644 --- a/LocalParticipant.cpp +++ b/LocalParticipant.cpp @@ -196,6 +196,8 @@ bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) { Arduino::LocalParticipant* thisArduino = static_cast(this); return thisArduino->Send(remoteParticipant, bufferSize); +#else + return false; #endif } @@ -233,6 +235,8 @@ bool LocalParticipant::Publish(IMessage* msg) { Arduino::LocalParticipant* thisArduino = static_cast(this); return thisArduino->Publish(msg); +#else + return false; #endif } diff --git a/LocalParticipant.h b/LocalParticipant.h index d0b34af..aa8d7d5 100644 --- a/LocalParticipant.h +++ b/LocalParticipant.h @@ -87,9 +87,9 @@ class LocalParticipant : public Participant { #if defined(__unix__) || defined(__APPLE__) int sock; #endif - sockaddr_in remote_addr; - sockaddr_in server_addr; - sockaddr_in broadcast_addr; + // sockaddr_in remote_addr; + // sockaddr_in server_addr; + // sockaddr_in broadcast_addr; #endif From 91ff401ecfa9ed0e91f98ee315b734914348940a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 1 Apr 2025 15:24:20 +0200 Subject: [PATCH 04/28] First steps implementation Matrix operations --- CMakeLists.txt | 4 +- LinearAlgebra/Matrix.cpp | 118 ++++++++++++++++++++++++++++-- LinearAlgebra/Matrix.h | 27 +++++++ LinearAlgebra/Quaternion.cpp | 23 ++++++ LinearAlgebra/Quaternion.h | 4 + LinearAlgebra/Vector3.h | 2 +- LinearAlgebra/test/Matrix_test.cc | 52 +++++++++++++ 7 files changed, 219 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 934c5ad..f660483 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.13) # CMake version check if(ESP_PLATFORM) idf_component_register( - SRC_DIRS "." - INCLUDE_DIRS "." + SRC_DIRS "." "LinearAlgebra" + INCLUDE_DIRS "." "LinearAlgebra" ) else() project(RoboidControl) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 2b83a1d..689f86d 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,6 +1,105 @@ #include "Matrix.h" -template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { +#pragma region Matrix2 + +Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { + this->nValues = nRows * nCols; + data = new float[nValues](); +} + +Matrix2::Matrix2(float* data, int nRows, int nCols) + : nRows(nRows), nCols(nCols), data(data) { + this->nValues = nRows * nCols; +} + +Matrix2::~Matrix2() { + delete[] data; +} + +Matrix2 Matrix2::Zero(int nRows, int nCols) { + Matrix2 m = Matrix2(nRows, nCols); + for (int ix = 0; ix < m.nValues; ix++) + m.data[ix] = 0; + return m; +} + +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 < r.nValues; 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 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& 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; + int BIndex = j; + for (int k = 0; k < ACols; ++k) { + sum += this->data[ARowOffset + k] * B.data[BIndex]; + BIndex += BCols; + } + r.data[BColOffset + j] = sum; + } + } + return r; +} + +void LinearAlgebra::Matrix2::SetSlice(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)]; + // this->data[i, j] = m.data[i - rowStart, j - colStart]; + } +} + +// Matrix2 +#pragma endregion + +template <> +MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { if (rows <= 0 || cols <= 0) { this->rows = 0; this->cols = 0; @@ -14,15 +113,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 +138,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 +148,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..5348f58 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -5,6 +5,33 @@ namespace LinearAlgebra { +class Matrix2 { + public: + int nRows = 0; + int nCols = 0; + int nValues = 0; + float* data = nullptr; + + Matrix2(int nRows, int nCols); + Matrix2(float* data, int nRows, int nCols); + + ~Matrix2(); + + static Matrix2 Zero(int nRows, int nCols); + + static Matrix2 Identity(int size); + + static Matrix2 Diagonal(float f, int size); + + static Matrix2 SkewMatrix(const Vector3& v); + + Matrix2 operator-() const; + + Matrix2 operator*(const Matrix2& m) const; + + void SetSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const; +}; + /// @brief Single precision float matrix template class MatrixOf { 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/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..a782afc 100644 --- a/LinearAlgebra/test/Matrix_test.cc +++ b/LinearAlgebra/test/Matrix_test.cc @@ -5,6 +5,58 @@ #include "Matrix.h" +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) { + //assert(result.data[i] == expectedData[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[] = {29, 32, 35, 65, 72, 79, 101, 112, 123}; + for (int i = 0; i < 9; ++i) { + assert(result2.data[i] == expectedData2[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) { + assert(result3.data[i] == 0); + 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) { + assert(result4.data[i] == A.data[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); From 2a49d5084476a80ef631ed657e65d2ff3a748130 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 1 Apr 2025 17:27:06 +0200 Subject: [PATCH 05/28] Fix matrix2 tests --- LinearAlgebra/Matrix.cpp | 46 +++++++++-- LinearAlgebra/Matrix.h | 7 +- LinearAlgebra/test/Matrix_test.cc | 122 ++++++++++++++++++------------ LocalParticipant.h | 7 +- 4 files changed, 123 insertions(+), 59 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index 689f86d..cbb7b7d 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,26 +1,53 @@ #include "Matrix.h" +#include #pragma region Matrix2 Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { this->nValues = nRows * nCols; - data = new float[nValues](); + if (this->nValues == 0) + data = nullptr; + else { + this->data = new float[nValues](); + this->externalData = false; + } } Matrix2::Matrix2(float* data, int nRows, int nCols) - : nRows(nRows), nCols(nCols), data(data) { + : nRows(nRows), nCols(nCols), data(data){ this->nValues = nRows * nCols; + this->externalData = true; } Matrix2::~Matrix2() { - delete[] data; + if (data != nullptr && !this->externalData) + delete[] data; +} + +// 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 m = Matrix2(nRows, nCols); - for (int ix = 0; ix < m.nValues; ix++) - m.data[ix] = 0; - return m; + Matrix2 r = Matrix2(nRows, nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = 0; + return r; } Matrix2 Matrix2::Identity(int size) { @@ -31,7 +58,7 @@ Matrix2 Matrix2::Diagonal(float f, int size) { Matrix2 r = Matrix2(size, size); float* data = r.data; int valueIx = 0; - for (int ix = 0; ix < r.nValues; ix++) { + for (int ix = 0; ix < size; ix++) { data[valueIx] = f; valueIx += size + 1; } @@ -71,12 +98,15 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { 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; diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index 5348f58..cd5bd76 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -11,11 +11,12 @@ class Matrix2 { int nCols = 0; int nValues = 0; float* data = nullptr; + bool externalData = true; Matrix2(int nRows, int nCols); Matrix2(float* data, int nRows, int nCols); - ~Matrix2(); + ~Matrix2(); static Matrix2 Zero(int nRows, int nCols); @@ -30,6 +31,10 @@ class Matrix2 { Matrix2 operator*(const Matrix2& m) const; void SetSlice(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; }; /// @brief Single precision float matrix diff --git a/LinearAlgebra/test/Matrix_test.cc b/LinearAlgebra/test/Matrix_test.cc index a782afc..4cb66b8 100644 --- a/LinearAlgebra/test/Matrix_test.cc +++ b/LinearAlgebra/test/Matrix_test.cc @@ -1,60 +1,88 @@ #if GTEST #include -#include #include +#include #include "Matrix.h" -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) { - //assert(result.data[i] == expectedData[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[] = {29, 32, 35, 65, 72, 79, 101, 112, 123}; - for (int i = 0; i < 9; ++i) { - assert(result2.data[i] == expectedData2[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 +TEST(Matrix2, Zero) { + // Test case 1: 2x2 zero matrix Matrix2 zeroMatrix = Matrix2::Zero(2, 2); - Matrix2 result3 = A * zeroMatrix; - - for (int i = 0; i < 4; ++i) { - assert(result3.data[i] == 0); - EXPECT_TRUE(result3.data[i] == 0); + 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 3 passed: Multiplication with zero matrix.\n"; + std::cout << "Test case 1 passed: 2x2 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) { - assert(result4.data[i] == A.data[i]); - EXPECT_TRUE(result4.data[i] == A.data[i]); + // 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 4 passed: Multiplication with identity matrix.\n"; + 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) { diff --git a/LocalParticipant.h b/LocalParticipant.h index aa8d7d5..07c0ca2 100644 --- a/LocalParticipant.h +++ b/LocalParticipant.h @@ -86,10 +86,11 @@ class LocalParticipant : public Participant { #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 - // sockaddr_in remote_addr; - // sockaddr_in server_addr; - // sockaddr_in broadcast_addr; #endif From 39abc0247bf75bf2bc9b4bdf457af67da27226da Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 2 Apr 2025 10:03:31 +0200 Subject: [PATCH 06/28] Implemented integrate function --- LinearAlgebra/Matrix.cpp | 104 +++++++++++++++++++++++++++++++++------ LinearAlgebra/Matrix.h | 72 ++++++++++++++++++++++++--- 2 files changed, 154 insertions(+), 22 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index cbb7b7d..c1f60a5 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,8 +1,48 @@ #include "Matrix.h" #include +#pragma region Matrix1 + +LinearAlgebra::Matrix1::Matrix1(int size) : size(size) { + if (this->size == 0) + data = nullptr; + else { + this->data = new float[size](); + this->externalData = false; + } +} + +LinearAlgebra::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) @@ -14,7 +54,7 @@ Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { } Matrix2::Matrix2(float* data, int nRows, int nCols) - : nRows(nRows), nCols(nCols), data(data){ + : nRows(nRows), nCols(nCols), data(data) { this->nValues = nRows * nCols; this->externalData = true; } @@ -25,22 +65,26 @@ Matrix2::~Matrix2() { } // 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 +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; + 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) { @@ -84,13 +128,20 @@ Matrix2 LinearAlgebra::Matrix2::operator-() const { 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 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; + // int BRows = B.nRows; for (int i = 0; i < ARows; ++i) { // Pre-compute row offsets @@ -101,7 +152,8 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { std::cout << " 0"; int BIndex = j; for (int k = 0; k < ACols; ++k) { - std::cout << " + " << this->data[ARowOffset + k] << " * " << B.data[BIndex]; + std::cout << " + " << this->data[ARowOffset + k] << " * " + << B.data[BIndex]; sum += this->data[ARowOffset + k] * B.data[BIndex]; BIndex += BCols; } @@ -121,10 +173,30 @@ void LinearAlgebra::Matrix2::SetSlice(int rowStart, for (int j = colStart; j < colStop; j++) this->data[i * this->nCols + j] = m.data[(i - rowStart) * m.nCols + (j - colStart)]; - // this->data[i, j] = m.data[i - rowStart, j - 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.SetSlice(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 diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index cd5bd76..f66eeeb 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -1,22 +1,40 @@ #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: + int size = 0; + float* data = nullptr; + + 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; - bool externalData = true; + Matrix2(); Matrix2(int nRows, int nCols); Matrix2(float* data, int nRows, int nCols); - ~Matrix2(); + ~Matrix2(); static Matrix2 Zero(int nRows, int nCols); @@ -28,13 +46,55 @@ class Matrix2 { Matrix2 operator-() const; - Matrix2 operator*(const Matrix2& m) 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; - void SetSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const; -//private: - // move constructor and move assignment operator + 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*(float f, const Matrix2& v) { + Matrix2 r = Matrix2(v.nRows, v.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = f * v.data[ix]; + return r; + } + + void SetSlice(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 From 42d3600cd8ebeaca8348d291615d70a797b86297 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 2 Apr 2025 12:45:59 +0200 Subject: [PATCH 07/28] Added ESP-IDF Wifi/UDP support (but it still give linker errors) --- EspIdf/EspIdfParticipant.cpp | 161 +++++++++++++++++++++++++++++++++++ EspIdf/EspIdfParticipant.h | 30 +++++++ EspIdf/EspIdfUtils.cpp | 90 ++++++++++++++++++++ EspIdf/EspIdfUtils.h | 7 ++ LinearAlgebra/Angle.h | 2 +- LinearAlgebra/Matrix.cpp | 4 +- LinearAlgebra/Matrix.h | 16 ++-- LocalParticipant.cpp | 38 +++++---- LocalParticipant.h | 2 +- 9 files changed, 323 insertions(+), 27 deletions(-) create mode 100644 EspIdf/EspIdfParticipant.cpp create mode 100644 EspIdf/EspIdfParticipant.h create mode 100644 EspIdf/EspIdfUtils.cpp create mode 100644 EspIdf/EspIdfUtils.h diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp new file mode 100644 index 0000000..ef5c7f7 --- /dev/null +++ b/EspIdf/EspIdfParticipant.cpp @@ -0,0 +1,161 @@ +#include "EspIdfParticipant.h" + +namespace RoboidControl { +namespace EspIdf { + +// #include +// #include +#include "esp_log.h" +// #include "esp_system.h" +#include "esp_netif.h" +// #include "esp_event_loop.h" +#include "esp_wifi.h" +#include "lwip/sockets.h" +// #include "lwip/err.h" +// #include "lwip/sys.h" +// #include "lwip/ip_addr.h" +// #include "lwip/inet.h" + +void LocalParticipant::Setup(int localPort, + const char* remoteIpAddress, + int remotePort) { +#if defined(IDF_VER) + this->remoteIpAddress = remoteIpAddress; + this->remotePort = remotePort; + 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; + } + + struct sockaddr_in server_addr, client_addr; + socklen_t addr_len = sizeof(client_addr); + char recv_buffer[1024]; + // int sockfd; + + // Create a UDP socket + sockfd = socket(AF_INET, SOCK_DGRAM, 0); + if (sockfd < 0) { + std::cout << "Unable to create UDP socket: errno " << errno << "\n"; + vTaskDelete(NULL); + return; + } + + // Set socket to non-blocking mode + int flags = fcntl(sockfd, F_GETFL, 0); + if (flags < 0) { + std::cout << "fcntl failed"; + close(sockfd); + return; + } + fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); + + // Set up the server address structure + memset(&server_addr, 0, sizeof(server_addr)); + server_addr.sin_family = AF_INET; + server_addr.sin_port = htons(localPort); + server_addr.sin_addr.s_addr = + htonl(INADDR_ANY); // Listen on all available network interfaces + + // Bind the socket to the address and port + if (bind(sockfd, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) { + std::cout << "Unable to bind UDP socket: errno " << errno << "\n"; + close(sockfd); + vTaskDelete(NULL); + return; + } + + std::cout << "Wifi sync started to port " << this->remotePort << "\n"; +#endif +} + +void LocalParticipant::GetBroadcastAddress() { + 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; + + std::cout << "Broadcast address: " << broadcastIpAddress << "\n"; +} + +void LocalParticipant::Receive() { +#if defined(IDF_VER) + struct sockaddr_in client_addr; + socklen_t addr_len = sizeof(client_addr); + int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, + (struct sockaddr*)&client_addr, &addr_len); + while (packetSize > 0) { + + char sender_ipAddress[16]; + inet_ntoa_r(client_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); + unsigned int sender_port = client_addr.sin_port; + + // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, + // sender_port); if (remoteParticipant == nullptr) { + // remoteParticipant = this->AddParticipant(sender_ipAddress, + // sender_port); + // // std::cout << "New sender " << sender_ipAddress << ":" << sender_port + // // << "\n"; + // // std::cout << "New remote participant " << + // remoteParticipant->ipAddress + // // << ":" << remoteParticipant->port << " " + // // << (int)remoteParticipant->networkId << "\n"; + // } + + // ReceiveData(packetSize, remoteParticipant); + ReceiveData(packetSize, sender_ipAddress, sender_port); + packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, + (struct sockaddr*)&client_addr, &addr_len); + } +#endif +} + +bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { +#if defined(IDF_VER) + // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" + // << remoteParticipant->port << "\n"; + + struct sockaddr_in dest_addr; + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(remoteParticipant->port); + inet_pton(AF_INET, remoteParticipant->ipAddress, &dest_addr.sin_addr.s_addr); + int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr)); + +#endif + return true; +} + +bool LocalParticipant::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->remotePort); + 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)); +// udp.beginPacket(this->broadcastIpAddress, this->remotePort); +// udp.write((unsigned char*)buffer, bufferSize); +// udp.endPacket(); + + // std::cout << "Publish to " << this->broadcastIpAddress << ":" + // << this->remotePort << "\n"; +#endif + return true; +}; + +} // namespace EspIdf +} // namespace RoboidControl diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h new file mode 100644 index 0000000..ec0d147 --- /dev/null +++ b/EspIdf/EspIdfParticipant.h @@ -0,0 +1,30 @@ +#pragma once + +#include "../LocalParticipant.h" + +// #if defined(HAS_WIFI) +// #include +// #endif + +namespace RoboidControl { +namespace EspIdf { + +class LocalParticipant : public RoboidControl::LocalParticipant { + public: + void Setup(int localPort, const char* remoteIpAddress, int remotePort); + void Receive(); + bool Send(Participant* remoteParticipant, int bufferSize); + bool Publish(IMessage* msg); + + protected: + const char* remoteIpAddress = nullptr; + unsigned short remotePort = 0; + char* broadcastIpAddress = nullptr; + + int sockfd; + + void GetBroadcastAddress(); +}; + +} // namespace EspIdf +} // namespace RoboidControl diff --git a/EspIdf/EspIdfUtils.cpp b/EspIdf/EspIdfUtils.cpp new file mode 100644 index 0000000..2954e49 --- /dev/null +++ b/EspIdf/EspIdfUtils.cpp @@ -0,0 +1,90 @@ +#include "EspIdfUtils.h" + +#if defined(IDF_VER) +#include +#include "esp_event.h" +#include "esp_netif.h" +#include "esp_wifi.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; + std::cout << "Wifi connected, IP address " << (IP2STR(&event->ip_info.ip)) + << "\n"; + wifi_connected = true; + xSemaphoreGive(wifi_semaphore); // Signal that connection is established + } + } +} + +// void log_dots_task(void* pvParameters) { +// while (!wifi_connected) { +// printf("."); // Log a dot +// vTaskDelay(500 / portTICK_PERIOD_MS); // Wait 500ms +// } +// printf("\nWi-Fi connected!\n"); +// vTaskDelete(NULL); +// } + +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 = {.sta = {.bssid_set = false}}; + // Copy the ssid in the config + strncpy((char*)wifi_config.sta.ssid, wifiSsid, sizeof(wifiSsid)); + // Copy the wifiPassword in the config + strncpy((char*)wifi_config.sta.password, wifiPassword, sizeof(wifiPassword)); + + 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; + break; + } + printf("."); // Log a dot + vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(500)); // Wait 500ms + } + + if (!wifi_connected) + std::cout << "\nCould not connect to home network.\n"; + + return success; +} + +#endif \ No newline at end of file diff --git a/EspIdf/EspIdfUtils.h b/EspIdf/EspIdfUtils.h new file mode 100644 index 0000000..c57f8f4 --- /dev/null +++ b/EspIdf/EspIdfUtils.h @@ -0,0 +1,7 @@ +#pragma once +#if defined(IDF_VER) + +bool StartWifi(const char *wifiSsid, const char *wifiPassword, + bool hotspotFallback = false); + +#endif \ No newline at end of file diff --git a/LinearAlgebra/Angle.h b/LinearAlgebra/Angle.h index 1e47726..4e23f9c 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; diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index c1f60a5..fca4360 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -3,7 +3,7 @@ #pragma region Matrix1 -LinearAlgebra::Matrix1::Matrix1(int size) : size(size) { +Matrix1::Matrix1(int size) : size(size) { if (this->size == 0) data = nullptr; else { @@ -12,7 +12,7 @@ LinearAlgebra::Matrix1::Matrix1(int size) : size(size) { } } -LinearAlgebra::Matrix1::Matrix1(float* data, int size) +Matrix1::Matrix1(float* data, int size) : data(data), size(size) { this->externalData = true; } diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index f66eeeb..a269bee 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -9,8 +9,8 @@ namespace LinearAlgebra { /// @brief A 1-dimensional matrix or vector of arbitrary size class Matrix1 { public: - int size = 0; float* data = nullptr; + int size = 0; Matrix1(int size); Matrix1(float* data, int size); @@ -74,12 +74,12 @@ class Matrix2 { } return r; } - friend Matrix2 operator*(float f, const Matrix2& v) { - Matrix2 r = Matrix2(v.nRows, v.nCols); - for (int ix = 0; ix < r.nValues; ix++) - r.data[ix] = f * v.data[ix]; - return r; - } + // friend Matrix2 operator*(float f, const Matrix2& v) { + // Matrix2 r = Matrix2(v.nRows, v.nCols); + // for (int ix = 0; ix < r.nValues; ix++) + // r.data[ix] = f * v.data[ix]; + // return r; + // } void SetSlice(int rowStart, int rowStop, @@ -208,6 +208,6 @@ class MatrixOf { }; } // namespace LinearAlgebra -using namespace LinearAlgebra; +//using namespace LinearAlgebra; #endif \ No newline at end of file diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index 5a968c7..5ebf3a6 100644 --- a/LocalParticipant.cpp +++ b/LocalParticipant.cpp @@ -3,6 +3,7 @@ #include "Thing.h" #include "Arduino/ArduinoParticipant.h" +#include "EspIdf/EspIdfParticipant.h" #if defined(_WIN32) || defined(_WIN64) #include @@ -33,10 +34,10 @@ LocalParticipant::LocalParticipant(int port) { this->isIsolated = true; } -LocalParticipant::LocalParticipant(const char* ipAddress, int port) { - this->ipAddress = "0.0.0.0"; // ipAddress; // maybe this is not needed - // anymore, keeping it to "0.0.0.0" - this->port = port; +LocalParticipant::LocalParticipant(const char* ipAddress, + int port, + int localPort) + : Participant("127.0.0.1", localPort) { if (this->port == 0) this->isIsolated = true; else @@ -73,22 +74,17 @@ void LocalParticipant::SetupUDP(int localPort, Arduino::LocalParticipant* thisArduino = static_cast(this); thisArduino->Setup(localPort, remoteIpAddress, remotePort); +#elif defined(IDF_VER) + EspIdf::LocalParticipant* thisEspIdf = + static_cast(this); + thisEspIdf->Setup(localPort, remoteIpAddress, remotePort); #endif this->connected = true; } void LocalParticipant::Update(unsigned long currentTimeMs) { - if (currentTimeMs == 0) { + if (currentTimeMs == 0) currentTimeMs = Thing::GetTimeMs(); - // #if defined(ARDUINO) - // currentTimeMs = millis(); - // #elif defined(__unix__) || defined(__APPLE__) - // auto now = std::chrono::steady_clock::now(); - // auto ms = - // std::chrono::duration_cast(now.time_since_epoch()); - // currentTimeMs = static_cast(ms.count()); - // #endif - } if (this->isIsolated == false) { if (this->connected == false) @@ -133,6 +129,10 @@ void LocalParticipant::ReceiveUDP() { Arduino::LocalParticipant* thisArduino = static_cast(this); thisArduino->Receive(); +#elif defined(IDF_VER) + EspIdf::LocalParticipant* thisEspIdf = + static_cast(this); + thisEspIdf->Receive(); #endif } @@ -196,8 +196,12 @@ bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) { Arduino::LocalParticipant* thisArduino = static_cast(this); return thisArduino->Send(remoteParticipant, bufferSize); +#elif defined(IDF_VER) + EspIdf::LocalParticipant* thisEspIdf = + static_cast(this); + return thisEspIdf->Send(remoteParticipant, bufferSize); #else - return false; + return false; #endif } @@ -235,6 +239,10 @@ bool LocalParticipant::Publish(IMessage* msg) { Arduino::LocalParticipant* thisArduino = static_cast(this); return thisArduino->Publish(msg); +#elif defined(IDF_VER) + EspIdf::LocalParticipant* thisEspIdf = + static_cast(this); + return thisEspIdf->Publish(msg); #else return false; #endif diff --git a/LocalParticipant.h b/LocalParticipant.h index 07c0ca2..76742a7 100644 --- a/LocalParticipant.h +++ b/LocalParticipant.h @@ -51,7 +51,7 @@ class LocalParticipant : public Participant { /// @brief Create a participant which will try to connect to a site. /// @param ipAddress The IP address of the site /// @param port The port used by the site - LocalParticipant(const char* ipAddress, int port = 7681); + LocalParticipant(const char* ipAddress, int port = 7681, int localPort = 7681); // Note to self: one cannot specify the port used by the local participant // now!! From 0c826d24be68ceb4a82be15c5cc2cbf011062ac8 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 2 Apr 2025 17:25:11 +0200 Subject: [PATCH 08/28] Linking works --- CMakeLists.txt | 23 +++++++++------ EspIdf/EspIdfParticipant.cpp | 57 ++++++++++++++++-------------------- EspIdf/EspIdfParticipant.h | 4 --- EspIdf/EspIdfUtils.cpp | 10 +++---- LinearAlgebra/Angle.h | 2 +- LinearAlgebra/Direction.h | 4 +-- LinearAlgebra/Spherical.h | 6 ++-- LinearAlgebra/SwingTwist.h | 6 ++-- 8 files changed, 54 insertions(+), 58 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f660483..18722d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,21 @@ 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 +) + if(ESP_PLATFORM) idf_component_register( - SRC_DIRS "." "LinearAlgebra" + SRCS ${srcs} INCLUDE_DIRS "." "LinearAlgebra" + REQUIRES esp_netif esp_wifi ) else() project(RoboidControl) @@ -28,14 +41,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 index ef5c7f7..9f8004c 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -1,21 +1,11 @@ #include "EspIdfParticipant.h" +#include "esp_wifi.h" +#include "lwip/sockets.h" + namespace RoboidControl { namespace EspIdf { -// #include -// #include -#include "esp_log.h" -// #include "esp_system.h" -#include "esp_netif.h" -// #include "esp_event_loop.h" -#include "esp_wifi.h" -#include "lwip/sockets.h" -// #include "lwip/err.h" -// #include "lwip/sys.h" -// #include "lwip/ip_addr.h" -// #include "lwip/inet.h" - void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) { @@ -31,10 +21,10 @@ void LocalParticipant::Setup(int localPort, return; } - struct sockaddr_in server_addr, client_addr; - socklen_t addr_len = sizeof(client_addr); - char recv_buffer[1024]; - // int sockfd; + struct sockaddr_in server_addr; //, client_addr; + // socklen_t addr_len = sizeof(client_addr); + // char recv_buffer[1024]; + // int sockfd; // Create a UDP socket sockfd = socket(AF_INET, SOCK_DGRAM, 0); @@ -96,7 +86,6 @@ void LocalParticipant::Receive() { int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, (struct sockaddr*)&client_addr, &addr_len); while (packetSize > 0) { - char sender_ipAddress[16]; inet_ntoa_r(client_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); unsigned int sender_port = client_addr.sin_port; @@ -116,7 +105,7 @@ void LocalParticipant::Receive() { // ReceiveData(packetSize, remoteParticipant); ReceiveData(packetSize, sender_ipAddress, sender_port); packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, - (struct sockaddr*)&client_addr, &addr_len); + (struct sockaddr*)&client_addr, &addr_len); } #endif } @@ -127,10 +116,13 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { // << remoteParticipant->port << "\n"; struct sockaddr_in dest_addr; - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(remoteParticipant->port); - inet_pton(AF_INET, remoteParticipant->ipAddress, &dest_addr.sin_addr.s_addr); - int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr *)&dest_addr, sizeof(dest_addr)); + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(remoteParticipant->port); + inet_pton(AF_INET, remoteParticipant->ipAddress, &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 << "Send error\n"; #endif return true; @@ -142,14 +134,17 @@ bool LocalParticipant::Publish(IMessage* msg) { if (bufferSize <= 0) return true; - struct sockaddr_in dest_addr; - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(this->remotePort); - 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)); -// udp.beginPacket(this->broadcastIpAddress, this->remotePort); -// udp.write((unsigned char*)buffer, bufferSize); -// udp.endPacket(); + struct sockaddr_in dest_addr; + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(this->remotePort); + 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"; + // udp.beginPacket(this->broadcastIpAddress, this->remotePort); + // udp.write((unsigned char*)buffer, bufferSize); + // udp.endPacket(); // std::cout << "Publish to " << this->broadcastIpAddress << ":" // << this->remotePort << "\n"; diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index ec0d147..2d90d2e 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -2,10 +2,6 @@ #include "../LocalParticipant.h" -// #if defined(HAS_WIFI) -// #include -// #endif - namespace RoboidControl { namespace EspIdf { diff --git a/EspIdf/EspIdfUtils.cpp b/EspIdf/EspIdfUtils.cpp index 2954e49..951a31f 100644 --- a/EspIdf/EspIdfUtils.cpp +++ b/EspIdf/EspIdfUtils.cpp @@ -2,7 +2,7 @@ #if defined(IDF_VER) #include -#include "esp_event.h" +// #include "esp_event.h" #include "esp_netif.h" #include "esp_wifi.h" #include "string.h" @@ -26,9 +26,9 @@ static void wifi_event_handler(void* arg, 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; - std::cout << "Wifi connected, IP address " << (IP2STR(&event->ip_info.ip)) - << "\n"; + //ip_event_got_ip_t* event = (ip_event_got_ip_t*)event_data; + //const char* ipaddr = IP2STR(&event->ip_info.ip); + std::cout << "Wifi connected, IP address (tbd)\n"; wifi_connected = true; xSemaphoreGive(wifi_semaphore); // Signal that connection is established } @@ -82,7 +82,7 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword) { } if (!wifi_connected) - std::cout << "\nCould not connect to home network.\n"; + std::cout << "\nCould not connect to home network.\n"; return success; } diff --git a/LinearAlgebra/Angle.h b/LinearAlgebra/Angle.h index 4e23f9c..e82b3af 100644 --- a/LinearAlgebra/Angle.h +++ b/LinearAlgebra/Angle.h @@ -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/Spherical.h b/LinearAlgebra/Spherical.h index 86705f9..5cad52c 100644 --- a/LinearAlgebra/Spherical.h +++ b/LinearAlgebra/Spherical.h @@ -24,9 +24,9 @@ class SphericalOf { /// @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, From a5cd5c89d3efc4f88901d642d40ba5b7237981db Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Apr 2025 09:40:59 +0200 Subject: [PATCH 09/28] Wifi connection is set up --- EspIdf/EspIdfUtils.cpp | 56 +++++++++++++++++++++++++----------------- EspIdf/EspIdfUtils.h | 3 +-- 2 files changed, 34 insertions(+), 25 deletions(-) diff --git a/EspIdf/EspIdfUtils.cpp b/EspIdf/EspIdfUtils.cpp index 951a31f..045f3b9 100644 --- a/EspIdf/EspIdfUtils.cpp +++ b/EspIdf/EspIdfUtils.cpp @@ -3,8 +3,11 @@ #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"; @@ -12,7 +15,7 @@ const char* hotspotPassword = "alchemy7000"; esp_netif_t* wifi_netif = nullptr; // Semaphore to signal Wi-Fi connection status -SemaphoreHandle_t wifi_semaphore; +// SemaphoreHandle_t wifi_semaphore; static bool wifi_connected = false; static void wifi_event_handler(void* arg, @@ -26,26 +29,17 @@ static void wifi_event_handler(void* arg, 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); - std::cout << "Wifi connected, IP address (tbd)\n"; + // 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 + // xSemaphoreGive(wifi_semaphore); // Signal that connection is + // established } } } -// void log_dots_task(void* pvParameters) { -// while (!wifi_connected) { -// printf("."); // Log a dot -// vTaskDelay(500 / portTICK_PERIOD_MS); // Wait 500ms -// } -// printf("\nWi-Fi connected!\n"); -// vTaskDelete(NULL); -// } - bool StartWifi(const char* wifiSsid, const char* wifiPassword) { - std::cout << "Connecting to WiFi" << wifiSsid << "\n"; + std::cout << "Connecting to WiFi " << wifiSsid << "\n"; esp_netif_init(); esp_event_loop_create_default(); @@ -59,11 +53,10 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword) { esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL); - wifi_config_t wifi_config = {.sta = {.bssid_set = false}}; - // Copy the ssid in the config - strncpy((char*)wifi_config.sta.ssid, wifiSsid, sizeof(wifiSsid)); - // Copy the wifiPassword in the config - strncpy((char*)wifi_config.sta.password, wifiPassword, sizeof(wifiPassword)); + 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); @@ -75,16 +68,33 @@ bool StartWifi(const char* wifiSsid, const char* wifiPassword) { for (int i = 0; i < 20; i++) { // 20 iterations, each 500ms if (wifi_connected) { success = true; + std::cout << " Connected.\n"; break; } - printf("."); // Log a dot + std::cout << "."; + fflush(stdout); // Ensure output is printed immediately vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(500)); // Wait 500ms } - if (!wifi_connected) + 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 \ No newline at end of file +#endif diff --git a/EspIdf/EspIdfUtils.h b/EspIdf/EspIdfUtils.h index c57f8f4..afe4d02 100644 --- a/EspIdf/EspIdfUtils.h +++ b/EspIdf/EspIdfUtils.h @@ -1,7 +1,6 @@ #pragma once #if defined(IDF_VER) -bool StartWifi(const char *wifiSsid, const char *wifiPassword, - bool hotspotFallback = false); +bool StartWifi(const char *wifiSsid, const char *wifiPassword); #endif \ No newline at end of file From 8130a057eabe3c560a84c4664abf2389c90f58f4 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Apr 2025 11:41:33 +0200 Subject: [PATCH 10/28] Sending messages works --- EspIdf/EspIdfParticipant.cpp | 98 +++++++++++++++++++++++++----------- EspIdf/EspIdfParticipant.h | 4 +- LocalParticipant.cpp | 2 +- 3 files changed, 71 insertions(+), 33 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 9f8004c..2e59974 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -6,12 +6,27 @@ namespace RoboidControl { namespace EspIdf { +void set_socket_blocking(int sock) { + int flags = fcntl(sock, F_GETFL, 0); + if (flags != -1) { + fcntl( + sock, F_SETFL, + flags & ~O_NONBLOCK); // Clear O_NONBLOCK flag to set to blocking mode + } +} + +void set_socket_non_blocking(int sock) { + int flags = fcntl(sock, F_GETFL, 0); + if (flags != -1) { + fcntl(sock, F_SETFL, + flags | O_NONBLOCK); // Set socket to non-blocking mode + } +} + void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(IDF_VER) - this->remoteIpAddress = remoteIpAddress; - this->remotePort = remotePort; GetBroadcastAddress(); wifi_ap_record_t ap_info; @@ -21,11 +36,6 @@ void LocalParticipant::Setup(int localPort, return; } - struct sockaddr_in server_addr; //, client_addr; - // socklen_t addr_len = sizeof(client_addr); - // char recv_buffer[1024]; - // int sockfd; - // Create a UDP socket sockfd = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { @@ -34,31 +44,35 @@ void LocalParticipant::Setup(int localPort, return; } - // Set socket to non-blocking mode - int flags = fcntl(sockfd, F_GETFL, 0); - if (flags < 0) { - std::cout << "fcntl failed"; - close(sockfd); - return; - } - fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); + // // Set socket to non-blocking mode + // int flags = fcntl(sockfd, F_GETFL, 0); + // if (flags < 0) { + // std::cout << "fcntl failed"; + // close(sockfd); + // return; + // } + // fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); + // set_socket_non_blocking(sockfd); // Set up the server address structure - memset(&server_addr, 0, sizeof(server_addr)); - server_addr.sin_family = AF_INET; - server_addr.sin_port = htons(localPort); - server_addr.sin_addr.s_addr = + 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(sockfd, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) { + if (bind(sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) { std::cout << "Unable to bind UDP socket: errno " << errno << "\n"; close(sockfd); vTaskDelete(NULL); return; } - std::cout << "Wifi sync started to port " << this->remotePort << "\n"; + std::cout << "Wifi sync started local " << this->port << ", remote " + << this->remoteSite->ipAddress << ":" << this->remoteSite->port + << "\n"; #endif } @@ -72,15 +86,22 @@ void LocalParticipant::GetBroadcastAddress() { return; } - ip_addr_t broadcast_addr; + ip_addr_t broadcast_addr = {}; broadcast_addr.u_addr.ip4.addr = (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr; - std::cout << "Broadcast address: " << broadcastIpAddress << "\n"; + this->broadcastIpAddress = new char[16]; // IPv4 address can have a max of 15 + // characters + null terminator + snprintf(this->broadcastIpAddress, 16, IPSTR, + IP2STR(&broadcast_addr.u_addr.ip4)); + std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n"; } void LocalParticipant::Receive() { #if defined(IDF_VER) +return; // disable receiving for now + set_socket_non_blocking(sockfd); + struct sockaddr_in client_addr; socklen_t addr_len = sizeof(client_addr); int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, @@ -107,22 +128,39 @@ void LocalParticipant::Receive() { packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, (struct sockaddr*)&client_addr, &addr_len); } + set_socket_blocking(sockfd); + #endif } bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) - // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" - // << remoteParticipant->port << "\n"; + std::cout << "Sending to " << remoteParticipant->ipAddress << ":" + << remoteParticipant->port << "\n"; struct sockaddr_in dest_addr; + memset(dest_addr.sin_zero, 0, sizeof(dest_addr.sin_zero)); dest_addr.sin_family = AF_INET; dest_addr.sin_port = htons(remoteParticipant->port); inet_pton(AF_INET, remoteParticipant->ipAddress, &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 << "Send error\n"; + + int err = 0; + // int n = 0; + // do { + // if (n > 0) { + // std::cout << "Retry sending\n"; + // vTaskDelay(pdMS_TO_TICKS(10)); // Wait 10ms + // } + // n++; + // err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, + // sizeof(dest_addr)); + + // } while (errno == EAGAIN && n < 10); + + err = sendto(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; @@ -136,7 +174,7 @@ bool LocalParticipant::Publish(IMessage* msg) { struct sockaddr_in dest_addr; dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(this->remotePort); + 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)); diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index 2d90d2e..b76dafe 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -13,8 +13,8 @@ class LocalParticipant : public RoboidControl::LocalParticipant { bool Publish(IMessage* msg); protected: - const char* remoteIpAddress = nullptr; - unsigned short remotePort = 0; + // const char* remoteIpAddress = nullptr; + // unsigned short remotePort = 0; char* broadcastIpAddress = nullptr; int sockfd; diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index 5ebf3a6..d898009 100644 --- a/LocalParticipant.cpp +++ b/LocalParticipant.cpp @@ -56,7 +56,7 @@ void LocalParticipant::begin() { if (this->isIsolated) return; - SetupUDP(this->port, this->ipAddress, this->port); + SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port); } void LocalParticipant::SetupUDP(int localPort, From afc48a1438f7719acf47ac191757749eba4b7171 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Apr 2025 12:36:23 +0200 Subject: [PATCH 11/28] Receiving works --- EspIdf/EspIdfParticipant.cpp | 116 ++++++++++++----------------------- EspIdf/EspIdfParticipant.h | 4 ++ LocalParticipant.cpp | 14 ++--- 3 files changed, 51 insertions(+), 83 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 2e59974..f61307d 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -1,28 +1,10 @@ #include "EspIdfParticipant.h" #include "esp_wifi.h" -#include "lwip/sockets.h" namespace RoboidControl { namespace EspIdf { -void set_socket_blocking(int sock) { - int flags = fcntl(sock, F_GETFL, 0); - if (flags != -1) { - fcntl( - sock, F_SETFL, - flags & ~O_NONBLOCK); // Clear O_NONBLOCK flag to set to blocking mode - } -} - -void set_socket_non_blocking(int sock) { - int flags = fcntl(sock, F_GETFL, 0); - if (flags != -1) { - fcntl(sock, F_SETFL, - flags | O_NONBLOCK); // Set socket to non-blocking mode - } -} - void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) { @@ -44,16 +26,6 @@ void LocalParticipant::Setup(int localPort, return; } - // // Set socket to non-blocking mode - // int flags = fcntl(sockfd, F_GETFL, 0); - // if (flags < 0) { - // std::cout << "fcntl failed"; - // close(sockfd); - // return; - // } - // fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); - // set_socket_non_blocking(sockfd); - // Set up the server address structure struct sockaddr_in local_addr; memset(&local_addr, 0, sizeof(local_addr)); @@ -62,6 +34,7 @@ void LocalParticipant::Setup(int localPort, 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(sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) { std::cout << "Unable to bind UDP socket: errno " << errno << "\n"; @@ -70,6 +43,13 @@ void LocalParticipant::Setup(int localPort, return; } + // struct sockaddr_in dest_addr; + memset(dest_addr.sin_zero, 0, sizeof(dest_addr.sin_zero)); + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(this->remoteSite->port); + inet_pton(AF_INET, this->remoteSite->ipAddress, &dest_addr.sin_addr.s_addr); + + std::cout << "Wifi sync started local " << this->port << ", remote " << this->remoteSite->ipAddress << ":" << this->remoteSite->port << "\n"; @@ -99,36 +79,39 @@ void LocalParticipant::GetBroadcastAddress() { void LocalParticipant::Receive() { #if defined(IDF_VER) -return; // disable receiving for now - set_socket_non_blocking(sockfd); + struct pollfd fds; + fds.fd = sockfd; + fds.events = POLLIN; // We're looking for data available to read - struct sockaddr_in client_addr; - socklen_t addr_len = sizeof(client_addr); - int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, - (struct sockaddr*)&client_addr, &addr_len); - while (packetSize > 0) { - char sender_ipAddress[16]; - inet_ntoa_r(client_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); - unsigned int sender_port = client_addr.sin_port; - - // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, - // sender_port); if (remoteParticipant == nullptr) { - // remoteParticipant = this->AddParticipant(sender_ipAddress, - // sender_port); - // // std::cout << "New sender " << sender_ipAddress << ":" << sender_port - // // << "\n"; - // // std::cout << "New remote participant " << - // remoteParticipant->ipAddress - // // << ":" << remoteParticipant->port << " " - // // << (int)remoteParticipant->networkId << "\n"; - // } - - // ReceiveData(packetSize, remoteParticipant); - ReceiveData(packetSize, sender_ipAddress, sender_port); - packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, - (struct sockaddr*)&client_addr, &addr_len); + // 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; + } + socklen_t addr_len = sizeof(this->src_addr); + + while (ret > 0 && fds.revents & POLLIN) { + int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, + (struct sockaddr*)&this->src_addr, &addr_len); + if (packetSize < 0) { + std::cout << "recvfrom() error\n"; + return; + } + + char sender_ipAddress[16]; + inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); + unsigned int sender_port = ntohs(this->src_addr.sin_port); + std::cout << "Received from " << ntohs(this->src_addr.sin_port) << "\n"; + + ReceiveData(packetSize, sender_ipAddress, sender_port); + + int ret = poll(&fds, 1, 0); + if (ret == -1) { + std::cout << "poll() error\n"; + return; + } } - set_socket_blocking(sockfd); #endif } @@ -138,26 +121,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { std::cout << "Sending to " << remoteParticipant->ipAddress << ":" << remoteParticipant->port << "\n"; - struct sockaddr_in dest_addr; - memset(dest_addr.sin_zero, 0, sizeof(dest_addr.sin_zero)); - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(remoteParticipant->port); - inet_pton(AF_INET, remoteParticipant->ipAddress, &dest_addr.sin_addr.s_addr); - - int err = 0; - // int n = 0; - // do { - // if (n > 0) { - // std::cout << "Retry sending\n"; - // vTaskDelay(pdMS_TO_TICKS(10)); // Wait 10ms - // } - // n++; - // err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, - // sizeof(dest_addr)); - - // } while (errno == EAGAIN && n < 10); - - err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, + int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr)); if (errno != 0) std::cout << "Send error " << err << " or " << errno << "\n"; diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index b76dafe..9a1c549 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -2,6 +2,8 @@ #include "../LocalParticipant.h" +#include "lwip/sockets.h" + namespace RoboidControl { namespace EspIdf { @@ -18,6 +20,8 @@ class LocalParticipant : public RoboidControl::LocalParticipant { char* broadcastIpAddress = nullptr; int sockfd; + struct sockaddr_in dest_addr; + struct sockaddr_in src_addr; void GetBroadcastAddress(); }; diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index d898009..1d94e55 100644 --- a/LocalParticipant.cpp +++ b/LocalParticipant.cpp @@ -260,11 +260,11 @@ void LocalParticipant::ReceiveData(unsigned char packetSize, this->GetParticipant(senderIpAddress, senderPort); if (remoteParticipant == nullptr) { remoteParticipant = this->AddParticipant(senderIpAddress, senderPort); - // std::cout << "New sender " << sender_ipAddress << ":" << sender_port - // << "\n"; - // std::cout << "New remote participant " << remoteParticipant->ipAddress - // << ":" << remoteParticipant->port << " " - // << (int)remoteParticipant->networkId << "\n"; + std::cout << "New sender " << senderIpAddress << ":" << senderPort + << "\n"; + std::cout << "New remote participant " << remoteParticipant->ipAddress + << ":" << remoteParticipant->port << " " + << (int)remoteParticipant->networkId << "\n"; } ReceiveData(packetSize, remoteParticipant); @@ -316,8 +316,8 @@ void LocalParticipant::ReceiveData(unsigned char bufferSize, void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {} void LocalParticipant::Process(Participant* sender, SiteMsg* msg) { - // std::cout << this->name << ": process NetworkId [" << (int)this->networkId - // << "/" << (int)msg->networkId << "]\n"; + std::cout << this->name << ": process NetworkId [" << (int)this->networkId + << "/" << (int)msg->networkId << "]\n"; if (this->networkId != msg->networkId) { this->networkId = msg->networkId; // std::cout << this->things.size() << " things\n"; From 7b17d8459a46e94e8ad0fa8b3704c9ed0d318457 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Apr 2025 12:54:40 +0200 Subject: [PATCH 12/28] Basic things seems to work, now start doing the tracker stuff --- EspIdf/EspIdfParticipant.cpp | 1 - LocalParticipant.cpp | 23 +++++++++++++---------- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index f61307d..8e4dd66 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -102,7 +102,6 @@ void LocalParticipant::Receive() { char sender_ipAddress[16]; inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); unsigned int sender_port = ntohs(this->src_addr.sin_port); - std::cout << "Received from " << ntohs(this->src_addr.sin_port) << "\n"; ReceiveData(packetSize, sender_ipAddress, sender_port); diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index 1d94e55..42e11f9 100644 --- a/LocalParticipant.cpp +++ b/LocalParticipant.cpp @@ -25,8 +25,6 @@ namespace RoboidControl { -// LocalParticipant::LocalParticipant() {} - LocalParticipant::LocalParticipant(int port) { this->ipAddress = "0.0.0.0"; this->port = port; @@ -260,8 +258,8 @@ void LocalParticipant::ReceiveData(unsigned char packetSize, this->GetParticipant(senderIpAddress, senderPort); if (remoteParticipant == nullptr) { remoteParticipant = this->AddParticipant(senderIpAddress, senderPort); - std::cout << "New sender " << senderIpAddress << ":" << senderPort - << "\n"; + // std::cout << "New sender " << senderIpAddress << ":" << senderPort + // << "\n"; std::cout << "New remote participant " << remoteParticipant->ipAddress << ":" << remoteParticipant->port << " " << (int)remoteParticipant->networkId << "\n"; @@ -273,7 +271,7 @@ void LocalParticipant::ReceiveData(unsigned char packetSize, void LocalParticipant::ReceiveData(unsigned char bufferSize, Participant* remoteParticipant) { unsigned char msgId = this->buffer[0]; - // std::cout << "receive msg " << (int)msgId << "\n"; + //std::cout << "receive msg " << (int)msgId << "\n"; switch (msgId) { case ParticipantMsg::id: { ParticipantMsg* msg = new ParticipantMsg(this->buffer); @@ -328,9 +326,14 @@ void LocalParticipant::Process(Participant* sender, SiteMsg* msg) { void LocalParticipant::Process(Participant* sender, InvestigateMsg* msg) {} -void LocalParticipant::Process(Participant* sender, ThingMsg* msg) {} +void LocalParticipant::Process(Participant* sender, ThingMsg* msg) { + std::cout << this->name << ": process Thing [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n"; +} void LocalParticipant::Process(Participant* sender, NameMsg* msg) { + std::cout << this->name << ": process Name [" << (int)this->networkId + << "/" << (int)msg->networkId << "]\n"; + Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) { int nameLength = msg->nameLength; @@ -353,12 +356,12 @@ void LocalParticipant::Process(Participant* sender, NameMsg* msg) { } } -void LocalParticipant::Process(Participant* sender, PoseMsg* msg) {} +void LocalParticipant::Process(Participant* sender, PoseMsg* msg) { + std::cout << this->name << ": process Pose [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n"; +} void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) { - // std::cout << this->name << ": process Binary [" << (int)this->networkId << - // "/" - // << (int)msg->networkId << "]\n"; + std::cout << this->name << ": process Binary [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n"; Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) thing->ProcessBinary(msg->bytes); From b32ac689666a74223a82a41c0d5c0e5de7e8d62f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 3 Apr 2025 17:57:08 +0200 Subject: [PATCH 13/28] Simulated Accelerometer can be received --- LocalParticipant.cpp | 33 ++++++++++++++++++++++++--------- LocalParticipant.h | 35 ++++++++++++++++++++++++++++++++++- SiteServer.cpp | 25 +++++++++++++------------ SiteServer.h | 32 ++++++++++++++++---------------- Things/TemperatureSensor.cpp | 4 ---- 5 files changed, 87 insertions(+), 42 deletions(-) diff --git a/LocalParticipant.cpp b/LocalParticipant.cpp index 42e11f9..ba12dec 100644 --- a/LocalParticipant.cpp +++ b/LocalParticipant.cpp @@ -271,7 +271,7 @@ void LocalParticipant::ReceiveData(unsigned char packetSize, void LocalParticipant::ReceiveData(unsigned char bufferSize, Participant* remoteParticipant) { unsigned char msgId = this->buffer[0]; - //std::cout << "receive msg " << (int)msgId << "\n"; + // std::cout << "receive msg " << (int)msgId << "\n"; switch (msgId) { case ParticipantMsg::id: { ParticipantMsg* msg = new ParticipantMsg(this->buffer); @@ -314,8 +314,8 @@ void LocalParticipant::ReceiveData(unsigned char bufferSize, void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {} void LocalParticipant::Process(Participant* sender, SiteMsg* msg) { - std::cout << this->name << ": process NetworkId [" << (int)this->networkId - << "/" << (int)msg->networkId << "]\n"; + std::cout << this->name << ": process Site Id " << (int)this->networkId + << "->" << (int)msg->networkId << "\n"; if (this->networkId != msg->networkId) { this->networkId = msg->networkId; // std::cout << this->things.size() << " things\n"; @@ -327,12 +327,24 @@ void LocalParticipant::Process(Participant* sender, SiteMsg* msg) { void LocalParticipant::Process(Participant* sender, InvestigateMsg* msg) {} void LocalParticipant::Process(Participant* sender, ThingMsg* msg) { - std::cout << this->name << ": process Thing [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n"; + std::cout << this->name << ": process Thing [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]\n"; +#if !defined(NO_STD) + 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 } void LocalParticipant::Process(Participant* sender, NameMsg* msg) { - std::cout << this->name << ": process Name [" << (int)this->networkId - << "/" << (int)msg->networkId << "]\n"; + std::cout << this->name << ": process Name [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]\n"; Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) { @@ -351,17 +363,20 @@ void LocalParticipant::Process(Participant* sender, NameMsg* msg) { #endif thingName[nameLength] = '\0'; thing->name = thingName; - // std::cout << "thing name = " << thing->name << " length = " << nameLength + // std::cout << "thing name = " << thing->name << " length = " << + // nameLength // << "\n"; } } void LocalParticipant::Process(Participant* sender, PoseMsg* msg) { - std::cout << this->name << ": process Pose [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n"; + std::cout << this->name << ": process Pose [" << (int)this->networkId << "/" + << (int)msg->networkId << "]\n"; } void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) { - std::cout << this->name << ": process Binary [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n"; + std::cout << this->name << ": process Binary [" << (int)this->networkId << "/" + << (int)msg->networkId << "]\n"; Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) thing->ProcessBinary(msg->bytes); diff --git a/LocalParticipant.h b/LocalParticipant.h index 76742a7..260f794 100644 --- a/LocalParticipant.h +++ b/LocalParticipant.h @@ -11,7 +11,9 @@ #include "Participant.h" #if !defined(NO_STD) +#include #include +// #include #endif #if defined(_WIN32) || defined(_WIN64) @@ -51,7 +53,9 @@ class LocalParticipant : public Participant { /// @brief Create a participant which will try to connect to a site. /// @param ipAddress The IP address of the site /// @param port The port used by the site - LocalParticipant(const char* ipAddress, int port = 7681, int localPort = 7681); + LocalParticipant(const char* ipAddress, + int port = 7681, + int localPort = 7681); // Note to self: one cannot specify the port used by the local participant // now!! @@ -136,6 +140,35 @@ class LocalParticipant : public Participant { virtual void Process(Participant* sender, NameMsg* 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/SiteServer.cpp index ae82d84..4531b5b 100644 --- a/SiteServer.cpp +++ b/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 } @@ -45,19 +45,20 @@ void SiteServer::Process(Participant* sender, SiteMsg* msg) {} void SiteServer::Process(Participant* sender, ThingMsg* msg) { Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing == nullptr) { -#if defined(NO_STD) +// #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 +// #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/SiteServer.h b/SiteServer.h index 4b1f9bb..84a0fe0 100644 --- a/SiteServer.h +++ b/SiteServer.h @@ -17,16 +17,16 @@ class SiteServer : public LocalParticipant { // 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 +// #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; @@ -35,12 +35,12 @@ class SiteServer : public LocalParticipant { 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 +// #if !defined(NO_STD) +// using ThingConstructor = std::function; +// std::unordered_map thingMsgProcessors; +// #endif }; } // namespace RoboidControl diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index 2667d73..f31044a 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -4,10 +4,6 @@ namespace RoboidControl { -// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {} - -// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {} - TemperatureSensor::TemperatureSensor(Participant* participant, unsigned char networkId, unsigned char thingId) From 6353af4a29f4735b94e2086ef4cda0988ba52e5c Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 7 Apr 2025 08:35:16 +0200 Subject: [PATCH 14/28] Refactoring --- LocalParticipant.cpp => Participants/ParticipantUDP.cpp | 0 LocalParticipant.h => Participants/ParticipantUDP.h | 0 SiteServer.cpp => Participants/SiteServer.cpp | 0 SiteServer.h => Participants/SiteServer.h | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename LocalParticipant.cpp => Participants/ParticipantUDP.cpp (100%) rename LocalParticipant.h => Participants/ParticipantUDP.h (100%) rename SiteServer.cpp => Participants/SiteServer.cpp (100%) rename SiteServer.h => Participants/SiteServer.h (100%) diff --git a/LocalParticipant.cpp b/Participants/ParticipantUDP.cpp similarity index 100% rename from LocalParticipant.cpp rename to Participants/ParticipantUDP.cpp diff --git a/LocalParticipant.h b/Participants/ParticipantUDP.h similarity index 100% rename from LocalParticipant.h rename to Participants/ParticipantUDP.h diff --git a/SiteServer.cpp b/Participants/SiteServer.cpp similarity index 100% rename from SiteServer.cpp rename to Participants/SiteServer.cpp diff --git a/SiteServer.h b/Participants/SiteServer.h similarity index 100% rename from SiteServer.h rename to Participants/SiteServer.h From b594bd59f4bc7952ae178aa939e8830ec21a57fc Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 8 Apr 2025 09:49:52 +0200 Subject: [PATCH 15/28] Refactoring --- Arduino/ArduinoParticipant.cpp | 10 +-- Arduino/ArduinoParticipant.h | 4 +- CMakeLists.txt | 1 + EspIdf/EspIdfParticipant.cpp | 67 +++++++------- EspIdf/EspIdfParticipant.h | 9 +- Messages/Messages.cpp | 6 +- Messages/Messages.h | 6 +- Messages/ParticipantMsg.cpp | 2 +- Participant.cpp | 2 + Participant.h | 2 + Participants/IsolatedParticipant.cpp | 14 +++ Participants/IsolatedParticipant.h | 13 +++ Participants/ParticipantUDP.cpp | 129 ++++++++++++--------------- Participants/ParticipantUDP.h | 60 ++++++------- Participants/SiteServer.h | 4 +- Posix/PosixParticipant.cpp | 8 +- Posix/PosixParticipant.h | 4 +- Thing.cpp | 7 +- Thing.h | 2 +- Windows/WindowsParticipant.cpp | 10 +-- Windows/WindowsParticipant.h | 4 +- test/participant_test.cc | 8 +- test/thing_test.cc | 2 +- 23 files changed, 197 insertions(+), 177 deletions(-) create mode 100644 Participants/IsolatedParticipant.cpp create mode 100644 Participants/IsolatedParticipant.h 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 18722d8..09c1b91 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ file(GLOB srcs Windows/*.cpp EspIdf/*.cpp LinearAlgebra/*.cpp + Participants/*.cpp ) if(ESP_PLATFORM) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 8e4dd66..4831d7d 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -5,7 +5,7 @@ namespace RoboidControl { namespace EspIdf { -void LocalParticipant::Setup(int localPort, +void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(IDF_VER) @@ -43,41 +43,41 @@ void LocalParticipant::Setup(int localPort, return; } - // struct sockaddr_in dest_addr; - memset(dest_addr.sin_zero, 0, sizeof(dest_addr.sin_zero)); - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(this->remoteSite->port); - inet_pton(AF_INET, this->remoteSite->ipAddress, &dest_addr.sin_addr.s_addr); + // 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 +#endif // IDF_VER } -void LocalParticipant::GetBroadcastAddress() { - esp_netif_ip_info_t ip_info; - esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); +void ParticipantUDP::GetBroadcastAddress() { + // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION... - // 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; - } + // esp_netif_ip_info_t ip_info; + // esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); - ip_addr_t broadcast_addr = {}; - broadcast_addr.u_addr.ip4.addr = - (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr; + // // 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; + // } - this->broadcastIpAddress = new char[16]; // IPv4 address can have a max of 15 - // characters + null terminator - snprintf(this->broadcastIpAddress, 16, IPSTR, - IP2STR(&broadcast_addr.u_addr.ip4)); - std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n"; + // 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"; } -void LocalParticipant::Receive() { +void ParticipantUDP::Receive() { #if defined(IDF_VER) struct pollfd fds; fds.fd = sockfd; @@ -90,20 +90,21 @@ void LocalParticipant::Receive() { return; } socklen_t addr_len = sizeof(this->src_addr); + char sender_ipAddress[INET_ADDRSTRLEN]; while (ret > 0 && fds.revents & POLLIN) { - int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0, + int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0, (struct sockaddr*)&this->src_addr, &addr_len); if (packetSize < 0) { std::cout << "recvfrom() error\n"; return; } - char sender_ipAddress[16]; - inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); - unsigned int sender_port = ntohs(this->src_addr.sin_port); + std::cout << "receiving " << packetSize << " bytes\n"; + // inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); + // unsigned int sender_port = ntohs(this->src_addr.sin_port); - ReceiveData(packetSize, sender_ipAddress, sender_port); + // ReceiveData(packetSize, sender_ipAddress, sender_port); int ret = poll(&fds, 1, 0); if (ret == -1) { @@ -112,15 +113,15 @@ void LocalParticipant::Receive() { } } -#endif +#endif // IDF_VER } -bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) std::cout << "Sending to " << remoteParticipant->ipAddress << ":" << remoteParticipant->port << "\n"; - int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, + 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"; @@ -129,7 +130,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { return true; } -bool LocalParticipant::Publish(IMessage* msg) { +bool ParticipantUDP::Publish(IMessage* msg) { #if defined(IDF_VER) int bufferSize = msg->Serialize((char*)this->buffer); if (bufferSize <= 0) diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index 9a1c549..fea444f 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -1,13 +1,13 @@ #pragma once -#include "../LocalParticipant.h" +#include "Participants/ParticipantUDP.h" #include "lwip/sockets.h" namespace RoboidControl { namespace EspIdf { -class LocalParticipant : public RoboidControl::LocalParticipant { +class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); @@ -15,9 +15,8 @@ class LocalParticipant : public RoboidControl::LocalParticipant { bool Publish(IMessage* msg); protected: - // const char* remoteIpAddress = nullptr; - // unsigned short remotePort = 0; - char* broadcastIpAddress = nullptr; + char broadcastIpAddress[INET_ADDRSTRLEN]; // IPv4 address can have a max of 15 characters + // + null terminator int sockfd; struct sockaddr_in dest_addr; diff --git a/Messages/Messages.cpp b/Messages/Messages.cpp index 422c687..e21662f 100644 --- a/Messages/Messages.cpp +++ b/Messages/Messages.cpp @@ -18,15 +18,15 @@ unsigned char IMessage::Serialize(char* buffer) { return 0; } -// bool IMessage::SendMsg(LocalParticipant *client, IMessage msg) { +// bool IMessage::SendMsg(ParticipantUDP *client, IMessage msg) { // // return SendMsg(client, client.buffer, );nameLength // return client->SendBuffer(msg.Serialize(client->buffer)); // } -// bool IMessage::Publish(LocalParticipant *participant) { +// bool IMessage::Publish(ParticipantUDP *participant) { // return participant->PublishBuffer(Serialize(participant->buffer)); // } -// bool IMessage::SendTo(LocalParticipant *participant) { +// bool IMessage::SendTo(ParticipantUDP *participant) { // return participant->SendBuffer(Serialize(participant->buffer)); // } 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/ParticipantMsg.cpp b/Messages/ParticipantMsg.cpp index e989f39..16c3b72 100644 --- a/Messages/ParticipantMsg.cpp +++ b/Messages/ParticipantMsg.cpp @@ -19,7 +19,7 @@ unsigned char ParticipantMsg::Serialize(char* buffer) { 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/Participant.cpp b/Participant.cpp index c690c75..42a0d94 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -27,6 +27,8 @@ Participant::~Participant() { delete[] this->ipAddress; } +void Participant::Update(unsigned long currentTimeMs) {} + Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { for (Thing* thing : this->things) { // if (thing->networkId == networkId && thing->id == thingId) diff --git a/Participant.h b/Participant.h index 94c5861..c03fdcf 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; 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 index ba12dec..a443002 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -1,4 +1,4 @@ -#include "LocalParticipant.h" +#include "ParticipantUDP.h" #include "Thing.h" @@ -25,14 +25,14 @@ namespace RoboidControl { -LocalParticipant::LocalParticipant(int port) { +ParticipantUDP::ParticipantUDP(int port) { this->ipAddress = "0.0.0.0"; this->port = port; if (this->port == 0) this->isIsolated = true; } -LocalParticipant::LocalParticipant(const char* ipAddress, +ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) : Participant("127.0.0.1", localPort) { @@ -42,45 +42,45 @@ LocalParticipant::LocalParticipant(const char* ipAddress, this->remoteSite = new Participant(ipAddress, port); } -static LocalParticipant* isolatedParticipant = nullptr; +static ParticipantUDP* isolatedParticipant = nullptr; -LocalParticipant* LocalParticipant::Isolated() { +ParticipantUDP* ParticipantUDP::Isolated() { if (isolatedParticipant == nullptr) - isolatedParticipant = new LocalParticipant(0); + isolatedParticipant = new ParticipantUDP(0); return isolatedParticipant; } -void LocalParticipant::begin() { +void ParticipantUDP::begin() { if (this->isIsolated) return; SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port); } -void LocalParticipant::SetupUDP(int localPort, +void ParticipantUDP::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(_WIN32) || defined(_WIN64) - Windows::LocalParticipant* thisWindows = - static_cast(this); + Windows::ParticipantUDP* thisWindows = + static_cast(this); thisWindows->Setup(localPort, remoteIpAddress, remotePort); #elif defined(__unix__) || defined(__APPLE__) - Posix::LocalParticipant* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = + static_cast(this); thisPosix->Setup(localPort, remoteIpAddress, remotePort); #elif defined(ARDUINO) - Arduino::LocalParticipant* thisArduino = - static_cast(this); + Arduino::ParticipantUDP* thisArduino = + static_cast(this); thisArduino->Setup(localPort, remoteIpAddress, remotePort); #elif defined(IDF_VER) - EspIdf::LocalParticipant* thisEspIdf = - static_cast(this); + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); thisEspIdf->Setup(localPort, remoteIpAddress, remotePort); #endif this->connected = true; } -void LocalParticipant::Update(unsigned long currentTimeMs) { +void ParticipantUDP::Update(unsigned long currentTimeMs) { if (currentTimeMs == 0) currentTimeMs = Thing::GetTimeMs(); @@ -114,27 +114,27 @@ void LocalParticipant::Update(unsigned long currentTimeMs) { } } -void LocalParticipant::ReceiveUDP() { +void ParticipantUDP::ReceiveUDP() { #if defined(_WIN32) || defined(_WIN64) - Windows::LocalParticipant* thisWindows = - static_cast(this); + Windows::ParticipantUDP* thisWindows = + static_cast(this); thisWindows->Receive(); #elif defined(__unix__) || defined(__APPLE__) - Posix::LocalParticipant* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = + static_cast(this); thisPosix->Receive(); #elif defined(ARDUINO) - Arduino::LocalParticipant* thisArduino = - static_cast(this); + Arduino::ParticipantUDP* thisArduino = + static_cast(this); thisArduino->Receive(); #elif defined(IDF_VER) - EspIdf::LocalParticipant* thisEspIdf = - static_cast(this); + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); thisEspIdf->Receive(); #endif } -Participant* LocalParticipant::GetParticipant(const char* ipAddress, int port) { +Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) { for (Participant* sender : this->senders) { if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port) return sender; @@ -142,7 +142,7 @@ Participant* LocalParticipant::GetParticipant(const char* ipAddress, int port) { return nullptr; } -Participant* LocalParticipant::AddParticipant(const char* ipAddress, int port) { +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) @@ -157,7 +157,7 @@ Participant* LocalParticipant::AddParticipant(const char* ipAddress, int port) { #pragma region Send -void LocalParticipant::SendThingInfo(Participant* remoteParticipant, +void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, Thing* thing) { // std::cout << "Send thing info " << (int)thing->id << " \n"; ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); @@ -177,33 +177,33 @@ void LocalParticipant::SendThingInfo(Participant* remoteParticipant, delete customMsg; } -bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) { +bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) return true; #if defined(_WIN32) || defined(_WIN64) - Windows::LocalParticipant* thisWindows = - static_cast(this); + Windows::ParticipantUDP* thisWindows = + static_cast(this); return thisWindows->Send(remoteParticipant, bufferSize); #elif defined(__unix__) || defined(__APPLE__) - Posix::LocalParticipant* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = + static_cast(this); return thisPosix->Send(remoteParticipant, bufferSize); #elif defined(ARDUINO) - Arduino::LocalParticipant* thisArduino = - static_cast(this); + Arduino::ParticipantUDP* thisArduino = + static_cast(this); return thisArduino->Send(remoteParticipant, bufferSize); #elif defined(IDF_VER) - EspIdf::LocalParticipant* thisEspIdf = - static_cast(this); + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); return thisEspIdf->Send(remoteParticipant, bufferSize); #else return false; #endif } -void LocalParticipant::PublishThingInfo(Thing* thing) { +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... @@ -224,22 +224,22 @@ void LocalParticipant::PublishThingInfo(Thing* thing) { delete customMsg; } -bool LocalParticipant::Publish(IMessage* msg) { +bool ParticipantUDP::Publish(IMessage* msg) { #if defined(_WIN32) || defined(_WIN64) - Windows::LocalParticipant* thisWindows = - static_cast(this); + Windows::ParticipantUDP* thisWindows = + static_cast(this); return thisWindows->Publish(msg); #elif defined(__unix__) || defined(__APPLE__) - Posix::LocalParticipant* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = + static_cast(this); return thisPosix->Publish(msg); #elif defined(ARDUINO) - Arduino::LocalParticipant* thisArduino = - static_cast(this); + Arduino::ParticipantUDP* thisArduino = + static_cast(this); return thisArduino->Publish(msg); #elif defined(IDF_VER) - EspIdf::LocalParticipant* thisEspIdf = - static_cast(this); + EspIdf::ParticipantUDP* thisEspIdf = + static_cast(this); return thisEspIdf->Publish(msg); #else return false; @@ -251,7 +251,7 @@ bool LocalParticipant::Publish(IMessage* msg) { #pragma region Receive -void LocalParticipant::ReceiveData(unsigned char packetSize, +void ParticipantUDP::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) { Participant* remoteParticipant = @@ -268,7 +268,7 @@ void LocalParticipant::ReceiveData(unsigned char packetSize, ReceiveData(packetSize, remoteParticipant); } -void LocalParticipant::ReceiveData(unsigned char bufferSize, +void ParticipantUDP::ReceiveData(unsigned char bufferSize, Participant* remoteParticipant) { unsigned char msgId = this->buffer[0]; // std::cout << "receive msg " << (int)msgId << "\n"; @@ -311,9 +311,9 @@ void LocalParticipant::ReceiveData(unsigned char bufferSize, }; } -void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {} +void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {} -void LocalParticipant::Process(Participant* sender, SiteMsg* msg) { +void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { std::cout << this->name << ": process Site Id " << (int)this->networkId << "->" << (int)msg->networkId << "\n"; if (this->networkId != msg->networkId) { @@ -324,25 +324,14 @@ void LocalParticipant::Process(Participant* sender, SiteMsg* msg) { } } -void LocalParticipant::Process(Participant* sender, InvestigateMsg* msg) {} +void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {} -void LocalParticipant::Process(Participant* sender, ThingMsg* msg) { +void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) { std::cout << this->name << ": process Thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n"; -#if !defined(NO_STD) - 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 } -void LocalParticipant::Process(Participant* sender, NameMsg* msg) { +void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { std::cout << this->name << ": process Name [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n"; @@ -369,14 +358,14 @@ void LocalParticipant::Process(Participant* sender, NameMsg* msg) { } } -void LocalParticipant::Process(Participant* sender, PoseMsg* msg) { +void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { std::cout << this->name << ": process Pose [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n"; } -void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) { - std::cout << this->name << ": process Binary [" << (int)this->networkId << "/" - << (int)msg->networkId << "]\n"; +void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { + std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]\n"; Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) thing->ProcessBinary(msg->bytes); diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 260f794..4d82fb9 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -23,8 +23,6 @@ #include #include #include -#elif defined(ARDUINO) -// #include #endif namespace RoboidControl { @@ -40,20 +38,20 @@ constexpr int MAX_SENDER_COUNT = 256; /// 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::LocalParticipant::Isolated(). +/// RoboidControl::IsolatedParticipant::Isolated(). /// @sa RoboidControl::Thing::Thing() -class LocalParticipant : public Participant { +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. - LocalParticipant(int port = 7681); + 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 - LocalParticipant(const char* ipAddress, + ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); // Note to self: one cannot specify the port used by the local participant @@ -62,7 +60,7 @@ class LocalParticipant : public Participant { /// @brief Isolated participant is used when the application is run without /// networking /// @return A participant without networking support - static LocalParticipant* Isolated(); + static ParticipantUDP* Isolated(); /// @brief True if the participant is running isolated. /// Isolated participants do not communicate with other participants @@ -73,7 +71,7 @@ class LocalParticipant : public Participant { long publishInterval = 3000; // 3 seconds /// @brief The name of the participant - const char* name = "LocalParticipant"; + const char* name = "ParticipantUDP"; // int localPort = 0; @@ -101,7 +99,7 @@ class LocalParticipant : public Participant { void begin(); bool connected = false; - virtual void Update(unsigned long currentTimeMs = 0); + virtual void Update(unsigned long currentTimeMs = 0) override; void SendThingInfo(Participant* remoteParticipant, Thing* thing); void PublishThingInfo(Thing* thing); @@ -142,31 +140,31 @@ class LocalParticipant : public Participant { virtual void Process(Participant* sender, BinaryMsg* msg); #if !defined(NO_STD) - public: - using ThingConstructor = std::function; +// 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 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); - }; - }; +// 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; +// protected: +// std::unordered_map thingMsgProcessors; #endif }; diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index 84a0fe0..0fe2974 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -1,6 +1,6 @@ #pragma once -#include "LocalParticipant.h" +#include "ParticipantUDP.h" #if !defined(NO_STD) #include @@ -11,7 +11,7 @@ namespace RoboidControl { /// @brief A participant is device which can communicate with other participants -class SiteServer : public LocalParticipant { +class SiteServer : public ParticipantUDP { public: SiteServer(int port = 7681); 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/Thing.cpp b/Thing.cpp index 64d0be9..b57be7f 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -1,6 +1,7 @@ #include "Thing.h" -#include "LocalParticipant.h" +#include "Participant.h" +#include "Participants/IsolatedParticipant.h" #include @@ -21,7 +22,7 @@ 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) {} @@ -201,7 +202,7 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) { } void Thing::UpdateThings(unsigned long currentTimeMs) { - LocalParticipant::Isolated()->Update(currentTimeMs); + IsolatedParticipant::Isolated()->Update(currentTimeMs); } void Thing::GenerateBinary(char* buffer, unsigned char* ix) { diff --git a/Thing.h b/Thing.h index d7086e8..d640d8b 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 diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index 9b5a8b3..a321b2d 100644 --- a/Windows/WindowsParticipant.cpp +++ b/Windows/WindowsParticipant.cpp @@ -16,7 +16,7 @@ namespace RoboidControl { namespace Windows { -void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) { +void Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(_WIN32) || defined(_WIN64) // Create a UDP socket @@ -95,7 +95,7 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem #endif } -void LocalParticipant::Receive() { +void Receive() { #if defined(_WIN32) || defined(_WIN64) // char ip_str[INET_ADDRSTRLEN]; // inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN); @@ -124,7 +124,7 @@ void LocalParticipant::Receive() { 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 << ":" @@ -141,7 +141,7 @@ void LocalParticipant::Receive() { #endif } -bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { +bool 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); @@ -166,7 +166,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) { return true; } -bool LocalParticipant::Publish(IMessage* msg) { +bool Publish(IMessage* msg) { #if defined(_WIN32) || defined(_WIN64) int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) 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..d82c9e7 100644 --- a/test/participant_test.cc +++ b/test/participant_test.cc @@ -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..015c43c 100644 --- a/test/thing_test.cc +++ b/test/thing_test.cc @@ -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(); From 12d91378e5fe126b85141123aefb27a4c33f96e2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 8 Apr 2025 10:43:20 +0200 Subject: [PATCH 16/28] Fix refactoring issues --- EspIdf/EspIdfParticipant.cpp | 2 + EspIdf/EspIdfParticipant.h | 7 +++- Windows/WindowsParticipant.cpp | 73 +++++----------------------------- test/participant_test.cc | 2 +- test/thing_test.cc | 2 +- 5 files changed, 19 insertions(+), 67 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 4831d7d..5dd9ca0 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -1,6 +1,8 @@ #include "EspIdfParticipant.h" +#if defined(IDF_VER) #include "esp_wifi.h" +#endif namespace RoboidControl { namespace EspIdf { diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index fea444f..ecdad07 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -2,7 +2,9 @@ #include "Participants/ParticipantUDP.h" +#if defined(IDF_VER) #include "lwip/sockets.h" +#endif namespace RoboidControl { namespace EspIdf { @@ -15,12 +17,13 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { bool Publish(IMessage* msg); protected: - char broadcastIpAddress[INET_ADDRSTRLEN]; // IPv4 address can have a max of 15 characters - // + null terminator +#if defined(IDF_VER) + char broadcastIpAddress[INET_ADDRSTRLEN]; int sockfd; struct sockaddr_in dest_addr; struct sockaddr_in src_addr; +#endif void GetBroadcastAddress(); }; diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index a321b2d..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 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 Setup(int localPort, const char* remoteIpAddress, int remotePort) { } // 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 Setup(int localPort, const char* remoteIpAddress, int remotePort) { 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 Setup(int localPort, const char* remoteIpAddress, int remotePort) { 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 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,21 +72,13 @@ void 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); @@ -138,16 +99,16 @@ void Receive() { // ReceiveData(packetSize, remoteParticipant); } -#endif +#endif // _WIN32 || _WIN64 } -bool 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 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 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 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 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/test/participant_test.cc b/test/participant_test.cc index d82c9e7..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; diff --git a/test/thing_test.cc b/test/thing_test.cc index 015c43c..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; From a1fc21dfe1fa021d705f65ef2253d6e19a54be7e Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 8 Apr 2025 12:48:52 +0200 Subject: [PATCH 17/28] Receiving no longer crashes --- EspIdf/EspIdfParticipant.cpp | 38 ++++++++++++++++++++------------- EspIdf/EspIdfParticipant.h | 2 +- Participants/ParticipantUDP.cpp | 1 + 3 files changed, 25 insertions(+), 16 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 4831d7d..c8188a5 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -9,6 +9,7 @@ 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; @@ -19,8 +20,8 @@ void ParticipantUDP::Setup(int localPort, } // Create a UDP socket - sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (sockfd < 0) { + 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; @@ -36,7 +37,7 @@ void ParticipantUDP::Setup(int localPort, // Bind the socket to the address and port - if (bind(sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) { + 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); @@ -79,39 +80,46 @@ void ParticipantUDP::GetBroadcastAddress() { void ParticipantUDP::Receive() { #if defined(IDF_VER) - struct pollfd fds; - fds.fd = sockfd; - fds.events = POLLIN; // We're looking for data available to read + 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); + int ret = poll(fds, 1, 0); if (ret == -1) { std::cout << "poll() error\n"; return; } - socklen_t addr_len = sizeof(this->src_addr); + + //char buffer[1024]; + struct sockaddr_in source_addr; + + socklen_t addr_len = sizeof(source_addr); char sender_ipAddress[INET_ADDRSTRLEN]; - while (ret > 0 && fds.revents & POLLIN) { + while (ret > 0 && fds[0].revents & POLLIN) { int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0, - (struct sockaddr*)&this->src_addr, &addr_len); + (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\n"; - // inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); - // unsigned int sender_port = ntohs(this->src_addr.sin_port); + 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); + ReceiveData(packetSize, sender_ipAddress, sender_port); - int ret = poll(&fds, 1, 0); + ret = poll(fds, 1, 0); if (ret == -1) { std::cout << "poll() error\n"; return; } } + //std::cout << "no more messages\n"; #endif // IDF_VER } diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index fea444f..1e80f0e 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -20,7 +20,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { int sockfd; struct sockaddr_in dest_addr; - struct sockaddr_in src_addr; + //struct sockaddr_in src_addr; void GetBroadcastAddress(); }; diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index a443002..08270cf 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -98,6 +98,7 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { this->nextPublishMe = currentTimeMs + this->publishInterval; } + this->ReceiveUDP(); } From aede0e5cd3c76c4d34b0a1230c74b0a26ae4c27e Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 8 Apr 2025 17:25:29 +0200 Subject: [PATCH 18/28] Processomg Acc data * propagating --- EspIdf/EspIdfParticipant.cpp | 2 +- LinearAlgebra/Matrix.cpp | 61 +++++++++++++++++++++++++-------- LinearAlgebra/Matrix.h | 29 +++++++++++----- Participants/ParticipantUDP.cpp | 6 ++-- 4 files changed, 71 insertions(+), 27 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index c8188a5..099ae5c 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -107,7 +107,7 @@ void ParticipantUDP::Receive() { break; } - std::cout << "receiving " << packetSize << " bytes, msgId " << (int)this->buffer[0] << "\n"; + // 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); diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index fca4360..c18ce3a 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,7 +1,9 @@ #include "Matrix.h" #include -#pragma region Matrix1 +namespace LinearAlgebra { + + #pragma region Matrix1 Matrix1::Matrix1(int size) : size(size) { if (this->size == 0) @@ -12,8 +14,7 @@ Matrix1::Matrix1(int size) : size(size) { } } -Matrix1::Matrix1(float* data, int size) - : data(data), size(size) { +Matrix1::Matrix1(float* data, int size) : data(data), size(size) { this->externalData = true; } @@ -28,12 +29,7 @@ Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) { } Quaternion LinearAlgebra::Matrix1::ToQuaternion() { - return Quaternion( - this->data[0], - this->data[1], - this->data[2], - this->data[3] - ); + return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]); } // Matrix1 @@ -64,6 +60,13 @@ Matrix2::~Matrix2() { delete[] data; } +Matrix2 Matrix2::Clone() const { + Matrix2 r = Matrix2(this->nRows, this->nCols); + for (int ix = 0; ix < this->nValues; ix++) + r.data[ix] = this->data[ix]; + return r; +} + // Move constructor Matrix2::Matrix2(Matrix2&& other) noexcept : nRows(other.nRows), @@ -121,6 +124,16 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) { 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++) @@ -149,22 +162,38 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { 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"; + // std::cout << " 0"; int BIndex = j; for (int k = 0; k < ACols; ++k) { - std::cout << " + " << this->data[ARowOffset + k] << " * " - << B.data[BIndex]; + // 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"; + // std::cout << " = " << sum << " ix: " << BColOffset + j << "\n"; } } return r; } -void LinearAlgebra::Matrix2::SetSlice(int rowStart, +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, @@ -181,7 +210,7 @@ void LinearAlgebra::Matrix2::SetSlice(int rowStart, /// @return 4x4 Omega matrix Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) { Matrix2 r = Matrix2::Zero(4, 4); - r.SetSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v)); + r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v)); // set last row to -v int ix = 3 * 4; @@ -200,6 +229,8 @@ Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) { // Matrix2 #pragma endregion +} // namespace LinearAlgbra + template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { if (rows <= 0 || cols <= 0) { diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index a269bee..65d55d0 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -36,6 +36,8 @@ class Matrix2 { ~Matrix2(); + Matrix2 Clone() const; + static Matrix2 Zero(int nRows, int nCols); static Matrix2 Identity(int size); @@ -44,6 +46,8 @@ class Matrix2 { static Matrix2 SkewMatrix(const Vector3& v); + Matrix2 Transpose() const; + Matrix2 operator-() const; /// @brief Add a matrix to this matrix @@ -74,14 +78,23 @@ class Matrix2 { } return r; } - // friend Matrix2 operator*(float f, const Matrix2& v) { - // Matrix2 r = Matrix2(v.nRows, v.nCols); - // for (int ix = 0; ix < r.nValues; ix++) - // r.data[ix] = f * v.data[ix]; - // return r; - // } - void SetSlice(int rowStart, + 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, @@ -208,6 +221,6 @@ class MatrixOf { }; } // namespace LinearAlgebra -//using namespace LinearAlgebra; +// using namespace LinearAlgebra; #endif \ No newline at end of file diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 08270cf..622574e 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -374,9 +374,9 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { thing = this->Get(msg->networkId, msg->thingId); if (thing != nullptr) thing->ProcessBinary(msg->bytes); - // else - // std::cout << "custom msg for unknown thing [" << (int)msg->networkId - // << "/" << (int)msg->thingId << "]\n"; + else + std::cout << "custom msg for unknown thing [" << (int)msg->networkId + << "/" << (int)msg->thingId << "]\n"; } } From 2062e4a71c7b06f045cbfb104def1b2b5b3b03a4 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 9 Apr 2025 11:07:25 +0200 Subject: [PATCH 19/28] Pose is synchronized to de sim.env. --- EspIdf/EspIdfParticipant.cpp | 4 +-- Messages/PoseMsg.cpp | 2 -- Participant.cpp | 7 +++- Participants/ParticipantUDP.cpp | 62 ++++++++++++++++----------------- Thing.cpp | 7 +++- 5 files changed, 44 insertions(+), 38 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 099ae5c..85c627a 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -126,8 +126,8 @@ void ParticipantUDP::Receive() { bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) - std::cout << "Sending to " << remoteParticipant->ipAddress << ":" - << remoteParticipant->port << "\n"; + // std::cout << "Sending to " << remoteParticipant->ipAddress << ":" + // << remoteParticipant->port << "\n"; int err = sendto(this->sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr)); diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index 9bafa6f..4bbb25a 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(); diff --git a/Participant.cpp b/Participant.cpp index 42a0d94..f79e707 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -27,7 +27,12 @@ Participant::~Participant() { delete[] this->ipAddress; } -void Participant::Update(unsigned long currentTimeMs) {} +void Participant::Update(unsigned long currentTimeMs) { + for (Thing* thing : this->things) { + if (thing != nullptr) + thing->Update(currentTimeMs, true); + } +} Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { for (Thing* thing : this->things) { diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 622574e..6cd6517 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -32,9 +32,7 @@ ParticipantUDP::ParticipantUDP(int port) { this->isIsolated = true; } -ParticipantUDP::ParticipantUDP(const char* ipAddress, - int port, - int localPort) +ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) : Participant("127.0.0.1", localPort) { if (this->port == 0) this->isIsolated = true; @@ -58,15 +56,14 @@ void ParticipantUDP::begin() { } void ParticipantUDP::SetupUDP(int localPort, - const char* remoteIpAddress, - int remotePort) { + 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); + Posix::ParticipantUDP* thisPosix = static_cast(this); thisPosix->Setup(localPort, remoteIpAddress, remotePort); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -90,6 +87,7 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { ParticipantMsg* msg = new ParticipantMsg(this->networkId); + std::cout << "Send Participant\n"; if (this->remoteSite == nullptr) this->Publish(msg); else @@ -98,20 +96,22 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { this->nextPublishMe = currentTimeMs + this->publishInterval; } - + this->ReceiveUDP(); } for (Thing* thing : this->things) { - if (thing != nullptr) { - thing->Update(currentTimeMs); - if (this->isIsolated == false) { - PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - for (Participant* sender : this->senders) - this->Send(sender, poseMsg); - delete poseMsg; - } + 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); } } @@ -121,8 +121,7 @@ void ParticipantUDP::ReceiveUDP() { static_cast(this); thisWindows->Receive(); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); thisPosix->Receive(); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -159,8 +158,8 @@ Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) { #pragma region Send void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, - Thing* thing) { - // std::cout << "Send thing info " << (int)thing->id << " \n"; + Thing* thing) { + std::cout << "Send thing info [" << (int)thing->id << "] \n"; ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); this->Send(remoteParticipant, thingMsg); delete thingMsg; @@ -188,8 +187,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { static_cast(this); return thisWindows->Send(remoteParticipant, bufferSize); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); return thisPosix->Send(remoteParticipant, bufferSize); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -231,8 +229,7 @@ bool ParticipantUDP::Publish(IMessage* msg) { static_cast(this); return thisWindows->Publish(msg); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); return thisPosix->Publish(msg); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -253,14 +250,12 @@ bool ParticipantUDP::Publish(IMessage* msg) { #pragma region Receive void ParticipantUDP::ReceiveData(unsigned char packetSize, - char* senderIpAddress, - unsigned int senderPort) { + char* senderIpAddress, + unsigned int senderPort) { Participant* remoteParticipant = this->GetParticipant(senderIpAddress, senderPort); if (remoteParticipant == nullptr) { remoteParticipant = this->AddParticipant(senderIpAddress, senderPort); - // std::cout << "New sender " << senderIpAddress << ":" << senderPort - // << "\n"; std::cout << "New remote participant " << remoteParticipant->ipAddress << ":" << remoteParticipant->port << " " << (int)remoteParticipant->networkId << "\n"; @@ -270,7 +265,7 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize, } void ParticipantUDP::ReceiveData(unsigned char bufferSize, - Participant* remoteParticipant) { + Participant* remoteParticipant) { unsigned char msgId = this->buffer[0]; // std::cout << "receive msg " << (int)msgId << "\n"; switch (msgId) { @@ -312,11 +307,14 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, }; } -void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {} +void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { + std::cout << this->name << ": Process Participant " << (int)msg->networkId + << "\n"; +} void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { - std::cout << this->name << ": process Site Id " << (int)this->networkId - << "->" << (int)msg->networkId << "\n"; + // std::cout << this->name << ": process Site Id " << (int)this->networkId + // << "->" << (int)msg->networkId << "\n"; if (this->networkId != msg->networkId) { this->networkId = msg->networkId; // std::cout << this->things.size() << " things\n"; diff --git a/Thing.cpp b/Thing.cpp index b57be7f..73fb6fd 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -2,6 +2,7 @@ #include "Participant.h" #include "Participants/IsolatedParticipant.h" +#include "Messages/PoseMsg.h" #include @@ -190,7 +191,11 @@ 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]; From 5fddebd1bc9e0a82579abc9cd5d7183727e355fa Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 9 Apr 2025 12:31:46 +0200 Subject: [PATCH 20/28] Performance improvements --- LinearAlgebra/Matrix.cpp | 44 ++++++++++++++++++++++++++-------------- LinearAlgebra/Matrix.h | 1 + 2 files changed, 30 insertions(+), 15 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index c18ce3a..fae5c6a 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -3,7 +3,7 @@ namespace LinearAlgebra { - #pragma region Matrix1 +#pragma region Matrix1 Matrix1::Matrix1(int size) : size(size) { if (this->size == 0) @@ -97,6 +97,11 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) { 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); } @@ -129,7 +134,8 @@ Matrix2 Matrix2::Transpose() const { 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]; + r.data[colIx * this->nCols + rowIx] = + this->data[rowIx * this->nCols + colIx]; } return r; } @@ -177,10 +183,7 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { return r; } -Matrix2 Matrix2::Slice(int rowStart, - int rowStop, - int colStart, - int colStop) { +Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) { Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart); int resultRowIx = 0; @@ -194,14 +197,25 @@ Matrix2 Matrix2::Slice(int rowStart, } 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 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)]; + } } } @@ -229,7 +243,7 @@ Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) { // Matrix2 #pragma endregion -} // namespace LinearAlgbra +} // namespace LinearAlgebra template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index 65d55d0..fc40055 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -39,6 +39,7 @@ class Matrix2 { Matrix2 Clone() const; static Matrix2 Zero(int nRows, int nCols); + void Clear(); static Matrix2 Identity(int size); From e0303ff36983e8eacf506faa22e528be6155ceef Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 9 Apr 2025 16:07:49 +0200 Subject: [PATCH 21/28] Optimizations --- LinearAlgebra/Matrix.cpp | 42 +++++++++++++++++++++++++++++++++++----- LinearAlgebra/Matrix.h | 3 +++ 2 files changed, 40 insertions(+), 5 deletions(-) diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index fae5c6a..b725f55 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -42,9 +42,9 @@ Matrix2::Matrix2() {} Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { this->nValues = nRows * nCols; if (this->nValues == 0) - data = nullptr; + this->data = nullptr; else { - this->data = new float[nValues](); + this->data = new float[this->nValues]; this->externalData = false; } } @@ -55,15 +55,41 @@ Matrix2::Matrix2(float* data, int nRows, int 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 (data != nullptr && !this->externalData) + if (!this->externalData) delete[] data; } Matrix2 Matrix2::Clone() const { Matrix2 r = Matrix2(this->nRows, this->nCols); - for (int ix = 0; ix < this->nValues; ix++) - r.data[ix] = this->data[ix]; + std::copy(this->data, this->data + this->nValues, r.data); return r; } @@ -154,6 +180,12 @@ Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const { 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); diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index fc40055..ef72922 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -33,6 +33,8 @@ class Matrix2 { Matrix2(); Matrix2(int nRows, int nCols); Matrix2(float* data, int nRows, int nCols); + Matrix2(const Matrix2& m); + Matrix2& operator=(const Matrix2& other); ~Matrix2(); @@ -55,6 +57,7 @@ class Matrix2 { /// @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) { From 28e42df771fffc0c91e732feadb594afba615924 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 9 Apr 2025 17:45:55 +0200 Subject: [PATCH 22/28] Added gyroscope --- Participants/ParticipantUDP.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 6cd6517..b9a367c 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -267,7 +267,7 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize, void ParticipantUDP::ReceiveData(unsigned char bufferSize, Participant* remoteParticipant) { unsigned char msgId = this->buffer[0]; - // std::cout << "receive msg " << (int)msgId << "\n"; + std::cout << "receive msg " << (int)msgId << "\n"; switch (msgId) { case ParticipantMsg::id: { ParticipantMsg* msg = new ParticipantMsg(this->buffer); From 92be21a9c1284c2b1d72457cd38e5a6c5b7db479 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 10 Apr 2025 17:35:06 +0200 Subject: [PATCH 23/28] Communication improvements, adding gyro msg --- EspIdf/EspIdfParticipant.cpp | 4 +- Messages/BinaryMsg.cpp | 42 ++++++++++------- Messages/BinaryMsg.h | 5 +- Messages/Messages.cpp | 17 ------- Messages/ModelUrlMsg.h | 2 +- Messages/NameMsg.cpp | 1 + Participant.cpp | 31 ++++++------ Participant.h | 2 +- Participants/ParticipantUDP.cpp | 83 ++++++++++++++++++++------------- Participants/ParticipantUDP.h | 1 + Participants/SiteServer.cpp | 33 ++++++------- Thing.cpp | 52 +++++++++++---------- Thing.h | 17 ++++--- Things/TemperatureSensor.cpp | 7 +-- Things/TemperatureSensor.h | 4 +- Things/TouchSensor.cpp | 4 +- Things/TouchSensor.h | 2 +- 17 files changed, 161 insertions(+), 146 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 85c627a..099ae5c 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -126,8 +126,8 @@ void ParticipantUDP::Receive() { bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) - // std::cout << "Sending to " << remoteParticipant->ipAddress << ":" - // << remoteParticipant->port << "\n"; + std::cout << "Sending to " << remoteParticipant->ipAddress << ":" + << remoteParticipant->port << "\n"; int err = sendto(this->sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr)); diff --git a/Messages/BinaryMsg.cpp b/Messages/BinaryMsg.cpp index 7ab7f5d..7e8f6d8 100644 --- a/Messages/BinaryMsg.cpp +++ b/Messages/BinaryMsg.cpp @@ -2,33 +2,41 @@ 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; + 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/Messages.cpp b/Messages/Messages.cpp index e21662f..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(ParticipantUDP *client, IMessage msg) { -// // return SendMsg(client, client.buffer, );nameLength -// return client->SendBuffer(msg.Serialize(client->buffer)); -// } - -// bool IMessage::Publish(ParticipantUDP *participant) { -// return participant->PublishBuffer(Serialize(participant->buffer)); -// } -// bool IMessage::SendTo(ParticipantUDP *participant) { -// return participant->SendBuffer(Serialize(participant->buffer)); -// } - // IMessage #pragma endregion diff --git a/Messages/ModelUrlMsg.h b/Messages/ModelUrlMsg.h index 27e5e25..3c35e68 100644 --- a/Messages/ModelUrlMsg.h +++ b/Messages/ModelUrlMsg.h @@ -8,7 +8,7 @@ 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; diff --git a/Messages/NameMsg.cpp b/Messages/NameMsg.cpp index 31b5d0e..36bd474 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++) diff --git a/Participant.cpp b/Participant.cpp index f79e707..80b484f 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -28,21 +28,19 @@ Participant::~Participant() { } void Participant::Update(unsigned long currentTimeMs) { - for (Thing* thing : this->things) { - if (thing != nullptr) - thing->Update(currentTimeMs, true); + for (Thing* thing : this->things) { + if (thing != nullptr) + thing->Update(currentTimeMs, true); } } -Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { +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; } @@ -56,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 c03fdcf..5fdffc4 100644 --- a/Participant.h +++ b/Participant.h @@ -50,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/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index b9a367c..d2f22aa 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -252,59 +252,75 @@ bool ParticipantUDP::Publish(IMessage* msg) { void ParticipantUDP::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) { - Participant* remoteParticipant = + Participant* sender = this->GetParticipant(senderIpAddress, senderPort); - if (remoteParticipant == nullptr) { - remoteParticipant = this->AddParticipant(senderIpAddress, senderPort); - std::cout << "New remote participant " << remoteParticipant->ipAddress - << ":" << remoteParticipant->port << " " - << (int)remoteParticipant->networkId << "\n"; + if (sender == nullptr) { + sender = this->AddParticipant(senderIpAddress, senderPort); + std::cout << "New remote participant " << sender->ipAddress + << ":" << sender->port << std::endl; } - ReceiveData(packetSize, remoteParticipant); + ReceiveData(packetSize, sender); } void ParticipantUDP::ReceiveData(unsigned char bufferSize, - Participant* remoteParticipant) { + 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); - Process(remoteParticipant, msg); + bufferSize -= msg->length; + Process(sender, msg); delete msg; } break; case SiteMsg::id: { SiteMsg* msg = new SiteMsg(this->buffer); - Process(remoteParticipant, msg); + bufferSize -= msg->length; + Process(sender, msg); delete msg; } break; case InvestigateMsg::id: { InvestigateMsg* msg = new InvestigateMsg(this->buffer); - Process(remoteParticipant, msg); + Process(sender, msg); delete msg; } break; case ThingMsg::id: { ThingMsg* msg = new ThingMsg(this->buffer); - Process(remoteParticipant, msg); + bufferSize -= msg->length; + Process(sender, msg); delete msg; } break; case NameMsg::id: { NameMsg* msg = new NameMsg(this->buffer); - Process(remoteParticipant, msg); + 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); - Process(remoteParticipant, msg); + bufferSize -= msg->length; + Process(sender, msg); delete msg; } break; case BinaryMsg::id: { BinaryMsg* msg = new BinaryMsg(this->buffer); - Process(remoteParticipant, msg); + 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) { @@ -313,8 +329,8 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { } void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { - // std::cout << this->name << ": process Site Id " << (int)this->networkId - // << "->" << (int)msg->networkId << "\n"; + std::cout << this->name << ": process Site Id " << (int)this->networkId + << "->" << (int)msg->networkId << "\n"; if (this->networkId != msg->networkId) { this->networkId = msg->networkId; // std::cout << this->things.size() << " things\n"; @@ -332,9 +348,9 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) { void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { std::cout << this->name << ": process Name [" << (int)msg->networkId << "/" - << (int)msg->thingId << "]\n"; + << (int)msg->thingId << "] "; - Thing* thing = sender->Get(msg->networkId, msg->thingId); + Thing* thing = sender->Get(msg->thingId); if (thing != nullptr) { int nameLength = msg->nameLength; int stringLen = nameLength + 1; @@ -346,15 +362,19 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { // Use strncpy with bounds checking for other platforms (Arduino, POSIX, // ESP-IDF) strncpy(thingName, msg->name, - stringLen - 1); // Leave space for null terminator - thingName[stringLen - 1] = '\0'; // Ensure null termination + nameLength); // Leave space for null terminator #endif thingName[nameLength] = '\0'; thing->name = thingName; - // std::cout << "thing name = " << thing->name << " length = " << - // nameLength - // << "\n"; + + std::cout << thing->name; } + std::cout << std::endl; +} + +void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { + std::cout << this->name << ": process model url [" << (int)msg->networkId + << "/" << (int)msg->thingId << "]\n"; } void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { @@ -364,18 +384,15 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/" - << (int)msg->thingId << "]\n"; - Thing* thing = sender->Get(msg->networkId, msg->thingId); + << (int)msg->thingId << "] "; + Thing* thing = sender->Get(msg->thingId); if (thing != nullptr) - thing->ProcessBinary(msg->bytes); + thing->ProcessBinary(msg->data); else { - thing = this->Get(msg->networkId, msg->thingId); - if (thing != nullptr) - thing->ProcessBinary(msg->bytes); - else - std::cout << "custom msg for unknown thing [" << (int)msg->networkId - << "/" << (int)msg->thingId << "]\n"; + std::cout << " unknown thing [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]"; } + std::cout << std::endl; } // Receive diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 4d82fb9..ec93af9 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -136,6 +136,7 @@ class ParticipantUDP : public Participant { 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); diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 4531b5b..8bde494 100644 --- a/Participants/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,22 +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/Thing.cpp b/Thing.cpp index 73fb6fd..86e4008 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -1,8 +1,8 @@ #include "Thing.h" +#include "Messages/PoseMsg.h" #include "Participant.h" #include "Participants/IsolatedParticipant.h" -#include "Messages/PoseMsg.h" #include @@ -23,14 +23,15 @@ namespace RoboidControl { // return isolatedParticipant; // } -Thing::Thing(int thingType) : Thing(IsolatedParticipant::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; @@ -40,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); @@ -210,9 +211,10 @@ void Thing::UpdateThings(unsigned long 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 d640d8b..febe7a9 100644 --- a/Thing.h +++ b/Thing.h @@ -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 f31044a..45718a5 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -5,17 +5,18 @@ namespace RoboidControl { 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; From e92ecef46fc8dc492b79c5874b703d1d75dc3526 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 11 Apr 2025 08:35:58 +0200 Subject: [PATCH 24/28] moved from broadcasting to unicasting --- EspIdf/EspIdfParticipant.cpp | 32 +++++++++++++------------------- Participants/ParticipantUDP.cpp | 1 + 2 files changed, 14 insertions(+), 19 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 099ae5c..f479a18 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -60,22 +60,22 @@ std::cout << "Set up UDP\n"; void ParticipantUDP::GetBroadcastAddress() { // 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"); + 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; - // } + // 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; + 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"; + snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR, + IP2STR(&broadcast_addr.u_addr.ip4)); + std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n"; } void ParticipantUDP::Receive() { @@ -152,12 +152,6 @@ bool ParticipantUDP::Publish(IMessage* msg) { sizeof(dest_addr)); if (err != 0) std::cout << "Publish error\n"; - // udp.beginPacket(this->broadcastIpAddress, this->remotePort); - // udp.write((unsigned char*)buffer, bufferSize); - // udp.endPacket(); - - // std::cout << "Publish to " << this->broadcastIpAddress << ":" - // << this->remotePort << "\n"; #endif return true; }; diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index d2f22aa..a406449 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -28,6 +28,7 @@ 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; } From 122d087f81893d015c0e65c3b30343588eb18ee8 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 11 Apr 2025 11:06:21 +0200 Subject: [PATCH 25/28] Added sendmsg debug --- Messages/BinaryMsg.cpp | 4 ++++ Messages/ThingMsg.cpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/Messages/BinaryMsg.cpp b/Messages/BinaryMsg.cpp index 7e8f6d8..0b0b940 100644 --- a/Messages/BinaryMsg.cpp +++ b/Messages/BinaryMsg.cpp @@ -31,6 +31,10 @@ unsigned char BinaryMsg::Serialize(char* buffer) { if (this->dataLength <= 0) // in this case, no data is actually sent return 0; +#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; 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; From 1aed9856aadb97525ee2bcde2c6954f8b7e2d106 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 11 Apr 2025 11:15:20 +0200 Subject: [PATCH 26/28] Added sendmsg debug messages --- Messages/DestroyMsg.cpp | 4 ++++ Messages/InvestigateMsg.cpp | 4 ++++ Messages/ModelUrlMsg.cpp | 13 ++++++++++--- Messages/NameMsg.cpp | 5 +++++ Messages/ParticipantMsg.cpp | 8 +++++++- Messages/PoseMsg.cpp | 4 ++++ Messages/SiteMsg.cpp | 3 +++ Messages/TextMsg.cpp | 3 +++ 8 files changed, 40 insertions(+), 4 deletions(-) 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/ModelUrlMsg.cpp b/Messages/ModelUrlMsg.cpp index 5a61165..0753750 100644 --- a/Messages/ModelUrlMsg.cpp +++ b/Messages/ModelUrlMsg.cpp @@ -22,21 +22,23 @@ ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) { else this->urlLength = (unsigned char)strlen(thing->modelUrl); - //this->url = thing->modelUrl; // dangerous! + // 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 +56,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/NameMsg.cpp b/Messages/NameMsg.cpp index 36bd474..da91c79 100644 --- a/Messages/NameMsg.cpp +++ b/Messages/NameMsg.cpp @@ -42,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 16c3b72..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(ParticipantUDP *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 4bbb25a..c6d41aa 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -45,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; From 304856ea0ae91b58384a191f7b64bc7ed99a91d9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 11 Apr 2025 11:23:54 +0200 Subject: [PATCH 27/28] Improve ESP-IDF defines --- EspIdf/EspIdfParticipant.cpp | 38 +++++++++++++++++++++--------------- EspIdf/EspIdfParticipant.h | 9 ++++++--- 2 files changed, 28 insertions(+), 19 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index f479a18..b0f7974 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -1,15 +1,17 @@ #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) { + const char* remoteIpAddress, + int remotePort) { #if defined(IDF_VER) -std::cout << "Set up UDP\n"; + std::cout << "Set up UDP\n"; GetBroadcastAddress(); wifi_ap_record_t ap_info; @@ -35,9 +37,9 @@ std::cout << "Set up UDP\n"; 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) { + 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); @@ -45,19 +47,21 @@ std::cout << "Set up UDP\n"; } // Initialize the dest_addr structure - memset(&this->dest_addr, 0, sizeof(this->dest_addr)); // Clear the entire 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); - + 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 +#endif // IDF_VER } void ParticipantUDP::GetBroadcastAddress() { +#if defined(IDF_VER) // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION... esp_netif_ip_info_t ip_info; @@ -76,6 +80,7 @@ void ParticipantUDP::GetBroadcastAddress() { 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() { @@ -90,8 +95,8 @@ void ParticipantUDP::Receive() { std::cout << "poll() error\n"; return; } - - //char buffer[1024]; + + // char buffer[1024]; struct sockaddr_in source_addr; socklen_t addr_len = sizeof(source_addr); @@ -107,7 +112,8 @@ void ParticipantUDP::Receive() { break; } - // std::cout << "receiving " << packetSize << " bytes, msgId " << (int)this->buffer[0] << "\n"; + // 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); @@ -119,9 +125,9 @@ void ParticipantUDP::Receive() { return; } } - //std::cout << "no more messages\n"; + // std::cout << "no more messages\n"; -#endif // IDF_VER +#endif // IDF_VER } bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { @@ -129,8 +135,8 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { std::cout << "Sending to " << remoteParticipant->ipAddress << ":" << remoteParticipant->port << "\n"; - int err = sendto(this->sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, - sizeof(dest_addr)); + 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"; diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index 1e80f0e..cf1c6bf 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -2,7 +2,9 @@ #include "Participants/ParticipantUDP.h" +#if defined(IDF_VER) #include "lwip/sockets.h" +#endif namespace RoboidControl { namespace EspIdf { @@ -15,12 +17,13 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { bool Publish(IMessage* msg); protected: - char broadcastIpAddress[INET_ADDRSTRLEN]; // IPv4 address can have a max of 15 characters - // + null terminator +#if defined(IDF_VER) + char broadcastIpAddress[INET_ADDRSTRLEN]; int sockfd; struct sockaddr_in dest_addr; - //struct sockaddr_in src_addr; + // struct sockaddr_in src_addr; +#endif void GetBroadcastAddress(); }; From 32ca8f5f713867b58897f001b8f26538b04d6dcf Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 11 Apr 2025 11:55:23 +0200 Subject: [PATCH 28/28] Improve debug logging --- EspIdf/EspIdfParticipant.cpp | 4 +-- Messages/ModelUrlMsg.cpp | 12 ------- Messages/ModelUrlMsg.h | 2 +- Participant.cpp | 4 +-- Participants/ParticipantUDP.cpp | 57 ++++++++++++++++++++++----------- 5 files changed, 43 insertions(+), 36 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index b0f7974..22e8160 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -132,8 +132,8 @@ void ParticipantUDP::Receive() { bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) - std::cout << "Sending to " << remoteParticipant->ipAddress << ":" - << remoteParticipant->port << "\n"; + // std::cout << "Sending to " << remoteParticipant->ipAddress << ":" + // << remoteParticipant->port << "\n"; int err = sendto(this->sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr)); diff --git a/Messages/ModelUrlMsg.cpp b/Messages/ModelUrlMsg.cpp index 0753750..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,8 +12,6 @@ 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++) diff --git a/Messages/ModelUrlMsg.h b/Messages/ModelUrlMsg.h index 3c35e68..7c493eb 100644 --- a/Messages/ModelUrlMsg.h +++ b/Messages/ModelUrlMsg.h @@ -15,7 +15,7 @@ class ModelUrlMsg : public IMessage { /// @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/Participant.cpp b/Participant.cpp index 80b484f..65a1f51 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -39,8 +39,8 @@ Thing* Participant::Get(unsigned char thingId) { if (thing->id == thingId) return thing; } - std::cout << "Could not find thing " << this->ipAddress << ":" << this->port - << "[" << (int)thingId << "]\n"; + // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port + // << "[" << (int)thingId << "]\n"; return nullptr; } diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index a406449..f643587 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -88,7 +88,6 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { ParticipantMsg* msg = new ParticipantMsg(this->networkId); - std::cout << "Send Participant\n"; if (this->remoteSite == nullptr) this->Publish(msg); else @@ -160,7 +159,7 @@ Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) { void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, Thing* thing) { - std::cout << "Send thing info [" << (int)thing->id << "] \n"; + // std::cout << "Send thing info [" << (int)thing->id << "] \n"; ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); this->Send(remoteParticipant, thingMsg); delete thingMsg; @@ -253,12 +252,11 @@ bool ParticipantUDP::Publish(IMessage* msg) { void ParticipantUDP::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) { - Participant* sender = - this->GetParticipant(senderIpAddress, 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; + std::cout << "New remote participant " << sender->ipAddress << ":" + << sender->port << std::endl; } ReceiveData(packetSize, sender); @@ -267,7 +265,7 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize, void ParticipantUDP::ReceiveData(unsigned char bufferSize, Participant* sender) { unsigned char msgId = this->buffer[0]; - std::cout << "receive msg " << (int)msgId << "\n"; + // std::cout << "receive msg " << (int)msgId << "\n"; // std::cout << " buffer size = " <<(int) bufferSize << "\n"; switch (msgId) { case ParticipantMsg::id: { @@ -325,13 +323,18 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, } void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { - std::cout << this->name << ": Process Participant " << (int)msg->networkId +#if defined(DEBUG) + std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId << "\n"; +#endif } void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { - std::cout << this->name << ": process Site Id " << (int)this->networkId - << "->" << (int)msg->networkId << "\n"; +#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"; @@ -340,16 +343,25 @@ void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { } } -void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {} +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) { - std::cout << this->name << ": process Thing [" << (int)msg->networkId << "/" - << (int)msg->thingId << "]\n"; +#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) { - std::cout << this->name << ": process Name [" << (int)msg->networkId << "/" +#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) { @@ -374,18 +386,25 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { } void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { - std::cout << this->name << ": process model url [" << (int)msg->networkId +#if defined(DEBUG) + std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n"; +#endif } void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { - std::cout << this->name << ": process Pose [" << (int)this->networkId << "/" - << (int)msg->networkId << "]\n"; +#if defined(DEBUG) + std::cout << this->name << ": process PoseMsg [" << (int)this->networkId + << "/" << (int)msg->networkId << "]\n"; +#endif } void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { - std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/" - << (int)msg->thingId << "] "; +#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);