RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Merge branch '0.4_dev' into MQTT

Browse Source
This commit is contained in:
Pascal Serrarens 2025-07-07 12:29:46 +02:00
commit c3f78079d3
27 changed files with 1128 additions and 733 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Arduino/ArduinoParticipant.cpp
View File

@ -88,7 +88,7 @@ void ParticipantUDP::Receive() {
#endif
}
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) {
#if defined(ARDUINO) && defined(HAS_WIFI)
// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
// << remoteParticipant->port << "\n";

2
Arduino/ArduinoParticipant.h
View File

@ -9,7 +9,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
public:
void Setup();
void Receive();
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
protected:

2
EspIdf/EspIdfParticipant.cpp
View File

@ -181,7 +181,7 @@ bool ParticipantUDP::Send(IMessage* msg) {
return this->SendTo(this->remoteSite, bufferSize);
}
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant,
int bufferSize) {
#if defined(IDF_VER)
uint16_t port = ntohs(dest_addr.sin_port);

2
EspIdf/EspIdfParticipant.h
View File

@ -23,7 +23,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive();
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
// bool SendTest();

332
LinearAlgebra/Matrix.cpp
View File

@ -3,12 +3,19 @@
#include <iostream>
#endif
// #include "esp_log.h"
// #include "esp_system.h"
// #include "freertos/FreeRTOS.h"
// #include "freertos/task.h"
namespace LinearAlgebra {
#pragma region Matrix1
Matrix1::Matrix1() {}
Matrix1::Matrix1(int size) : size(size) {
if (this->size == 0)
if (this->size <= 0)
data = nullptr;
else {
this->data = new float[size]();
@ -16,10 +23,63 @@ Matrix1::Matrix1(int size) : size(size) {
}
}
Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
Matrix1::Matrix1(int size, float* data) : data(data), size(size) {
this->externalData = true;
}
Matrix1::Matrix1(const Matrix1& m) : size(m.size) {
if (this->size <= 0)
this->data = nullptr;
else {
this->data = new float[this->size];
for (int ix = 0; ix < this->size; ++ix)
this->data[ix] = m.data[ix];
}
}
Matrix1::~Matrix1() {
delete[] data;
}
unsigned int Matrix1::Size() const {
return this->size;
}
Matrix1 Matrix1::FromVector3(Vector3 v) {
Matrix1 r(3);
r.data[0] = v.x;
r.data[1] = v.y;
r.data[2] = v.z;
return r;
}
Matrix1 Matrix1::Zero(unsigned int size) {
Matrix1 r(size);
r.Fill(0);
return r;
}
void Matrix1::Fill(float value) {
for (int ix = 0; ix < this->size; ix++)
this->data[ix] = value;
}
void Matrix1::Fill(float value, unsigned int start, unsigned int stop) {
if (start > stop || start > this->size)
return;
if (stop > this->size)
stop = this->size;
for (unsigned int ix = start; ix < stop; ix++)
this->data[ix] = value;
}
Vector3 Matrix1::ToVector3() {
return Vector3(this->data[0], this->data[1], this->data[2]);
}
Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
Matrix1 r = Matrix1(4);
float* data = r.data;
@ -34,6 +94,40 @@ Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
}
Matrix1 Matrix1::Slice(int start, int stop) {
if (start < 0 || stop >= this->size)
std::cerr << "Slice index out of range." << std::endl;
Matrix1 result(stop - start);
int resultIx = 0;
for (int ix = start; ix < stop; ix++)
result.data[resultIx++] = this->data[ix];
return result;
}
void Matrix1::UpdateSlice(int start, int stop, const Matrix1& source) const {
if (start > stop || start > this->size || start > source.size)
return;
if (stop > this->size)
stop = this->size;
if (stop > source.size)
stop = source.size;
unsigned int sourceIx = 0;
for (int ix = start; ix < stop; ix++, sourceIx++) {
this->data[ix] = source.data[sourceIx];
}
}
const float& Matrix1::operator()(int ix) const {
return data[ix];
}
float& Matrix1::operator()(int ix) {
return data[ix];
}
// Matrix1
#pragma endregion
@ -43,15 +137,19 @@ Matrix2::Matrix2() {}
Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
this->nValues = nRows * nCols;
if (this->nValues == 0)
// printf("Allocate %d\n", this->nValues);
if (this->nValues <= 0) {
this->data = nullptr;
else {
this->externalData = false;
return;
}
this->data = new float[this->nValues];
this->externalData = false;
}
}
Matrix2::Matrix2(float* data, int nRows, int nCols)
Matrix2::Matrix2(int nRows, int nCols, float* data, float autoDestruct)
: nRows(nRows), nCols(nCols), data(data) {
this->nValues = nRows * nCols;
this->externalData = true;
@ -88,39 +186,43 @@ Matrix2& Matrix2::operator=(const Matrix2& m) {
}
Matrix2::~Matrix2() {
if (!this->externalData)
if (!this->externalData) {
// printf("Deallocate %d\n", this->nValues);
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;
unsigned int Matrix2::NRows() {
return this->nRows;
}
unsigned int Matrix2::NCols() {
return this->nCols;
}
// 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;
}
// Matrix2& Matrix2::operator=(Matrix2&& other) noexcept {
// if (this != &other) {
// delete[] data; // Clean up current data
// nRows = other.nRows;
// nCols = other.nCols;
// nValues = other.nValues;
// data = other.data;
// other.data = nullptr; // Avoid double deletion
// }
// return *this;
// }
Matrix2 Matrix2::Zero(int nRows, int nCols) {
Matrix2 r = Matrix2(nRows, nCols);
@ -129,9 +231,16 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) {
return r;
}
void Matrix2::Clear() {
const float& Matrix2::operator()(int row, int col) const {
return data[row * this->nCols + col];
}
float& Matrix2::operator()(int row, int col) {
return data[row * this->nCols + col];
}
void Matrix2::Fill(float value) {
for (int ix = 0; ix < this->nValues; ix++)
this->data[ix] = 0;
this->data[ix] = value;
}
Matrix2 Matrix2::Identity(int size) {
@ -186,13 +295,30 @@ Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
return r;
}
Matrix2 Matrix2::operator+=(const Matrix2& v) {
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 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;
}
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(rowIx) += m.data[mRowIx + colIx] * v(rowIx);
}
return r;
}
Matrix2 Matrix2::operator*(const Matrix2& B) const {
Matrix2 r = Matrix2(this->nRows, B.nCols);
int ACols = this->nCols;
@ -221,6 +347,43 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
return r;
}
Matrix2 Matrix2::GetRows(int from, int to) {
if (from < 0 || to >= this->nRows)
std::cerr << "Slice index out of range." << std::endl;
Matrix2 result = Matrix2(to - from, this->nCols);
int resultRowIx = 0;
for (int rowIx = from; rowIx < to; rowIx++) {
for (int colIx = 0; colIx < this->nCols; colIx++)
result.data[resultRowIx * result.nCols + colIx] =
this->data[rowIx * this->nCols + colIx];
resultRowIx++;
}
return result;
}
Vector3 Matrix2::GetRow3(int rowIx) {
int cellIx = rowIx * this->nCols;
Vector3 row(this->data[cellIx + 0], this->data[cellIx + 1],
this->data[cellIx + 2]);
return row;
}
void Matrix2::SetRow(int rowIx, Matrix1 source) {
int cellIx = rowIx * this->nCols;
for (int ix = 0; ix < source.Size(); ix++)
this->data[cellIx + ix] = source(ix);
}
void Matrix2::SetRow3(int rowIx, Vector3 v) {
int cellIx = rowIx * this->nCols;
this->data[cellIx + 0] = v.x;
this->data[cellIx + 1] = v.y;
this->data[cellIx + 2] = v.z;
}
Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
@ -234,22 +397,18 @@ Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
return r;
}
void Matrix2::UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const {
UpdateSlice(rowStart, rowStop, 0, this->nCols, m);
}
void Matrix2::UpdateSlice(int rowStart,
int rowStop,
int colStart,
int colStop,
const Matrix2& m) const {
// for (int i = rowStart; i < rowStop; i++) {
// for (int j = colStart; j < colStop; j++)
// this->data[i * this->nCols + j] =
// m.data[(i - rowStart) * m.nCols + (j - colStart)];
// }
int rRowDataIx = rowStart * this->nCols;
int mRowDataIx = 0;
for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
rRowDataIx = rowIx * this->nCols;
// rRowDataIx += this->nCols;
int rRowDataIx = rowIx * this->nCols;
mRowDataIx += m.nCols;
for (int colIx = colStart; colIx < colStop; colIx++) {
this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
@ -257,6 +416,91 @@ void Matrix2::UpdateSlice(int rowStart,
}
}
Matrix2 Matrix2::Inverse() {
int n = this->nRows;
// // Create an identity matrix of the same size as the original matrix
Matrix2 augmentedMatrix(n, 2 * n);
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
augmentedMatrix.data[i * this->nCols + j] =
this->data[i * this->nCols + j];
augmentedMatrix.data[i * this->nCols + (j + n)] =
(i == j) ? 1 : 0; // identity matrix;
}
}
// Perform Gaussian elimination
for (int i = 0; i < n; i++) {
// Find the pivot row
float pivot = augmentedMatrix.data[i * augmentedMatrix.nCols + i];
if (abs(pivot) < 1e-10) // Check for singular matrix
std::cerr << "Matrix is singular and cannot be inverted." << std::endl;
// Normalize the pivot row
for (int j = 0; j < 2 * n; j++)
augmentedMatrix.data[i * augmentedMatrix.nCols + j] /= pivot;
// Eliminate the column below the pivot
for (int j = i + 1; j < n; j++) {
float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
for (int k = 0; k < 2 * n; k++)
augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
}
}
// Back substitution
for (int i = n - 1; i >= 0; i--) {
// Eliminate the column above the pivot
for (int j = i - 1; j >= 0; j--) {
float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
for (int k = 0; k < 2 * n; k++)
augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
}
}
// Extract the inverse matrix from the augmented matrix
Matrix2 inverse(n, n);
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++)
inverse.data[i * inverse.nCols + j] =
augmentedMatrix.data[i * augmentedMatrix.nCols + j + n];
}
return inverse;
}
Matrix2 Matrix2::DeleteRows(int rowStart, int rowStop) {
Matrix2 r = Matrix2(this->nRows - (rowStop - rowStart), this->nCols);
int resultRowIx = 0;
for (int i = 0; i < this->nRows; i++) {
if (i >= rowStart && i < rowStop)
continue;
for (int j = 0; j < this->nCols; j++)
r.data[resultRowIx * r.nCols + j] = this->data[i * this->nCols + j];
resultRowIx++;
}
return r;
}
Matrix2 Matrix2::DeleteColumns(int colStart, int colStop) {
Matrix2 r = Matrix2(this->nRows, this->nCols - (colStop - colStart));
for (int i = 0; i < this->nRows; i++) {
int resultColIx = 0;
for (int j = 0; j < this->nCols; j++) {
if (j >= colStart && j < colStop)
continue;
r.data[i * r.nCols + resultColIx++] = this->data[i * this->nCols + j];
}
}
return r;
}
/// @brief Compute the Omega matrix of a 3D vector
/// @param v The vector
/// @return 4x4 Omega matrix

98
LinearAlgebra/Matrix.h
View File

@ -9,15 +9,36 @@ namespace LinearAlgebra {
/// @brief A 1-dimensional matrix or vector of arbitrary size
class Matrix1 {
public:
float* data = nullptr;
int size = 0;
Matrix1();
Matrix1(int size);
Matrix1(float* data, int size);
Matrix1(int size, float* data);
Matrix1(const Matrix1& m); // Make a clone
~Matrix1();
unsigned int Size() const;
static Matrix1 Zero(unsigned int size);
void Fill(float value);
void Fill(float value, unsigned int start, unsigned int stop);
static Matrix1 FromVector3(Vector3 v);
Vector3 ToVector3();
static Matrix1 FromQuaternion(Quaternion q);
Quaternion ToQuaternion();
const float& operator()(int ix) const;
float& operator()(int ix);
Matrix1 Slice(int start, int stop);
void UpdateSlice(int start, int stop, const Matrix1& m) const;
protected:
float* data = nullptr;
int size = 0;
private:
bool externalData = true;
};
@ -25,31 +46,47 @@ class Matrix1 {
/// @brief A 2-dimensional matrix of arbitrary size
class Matrix2 {
public:
int nRows = 0;
int nCols = 0;
int nValues = 0;
float* data = nullptr;
Matrix2();
Matrix2(int nRows, int nCols);
Matrix2(float* data, int nRows, int nCols);
Matrix2(int nRows, int nCols, float* data, float autoDestruct = false);
Matrix2(const Matrix2& m);
Matrix2& operator=(const Matrix2& other);
~Matrix2();
Matrix2 Clone() const;
unsigned int NRows();
unsigned int NCols();
static Matrix2 Zero(int nRows, int nCols);
void Clear();
void Fill(float value);
static Matrix2 Identity(int size);
static Matrix2 Diagonal(float f, int size);
static Matrix2 SkewMatrix(const Vector3& v);
Matrix2 Transpose() const;
Matrix2 Inverse();
const float& operator()(int row, int col) const;
float& operator()(int row, int col);
Matrix2 GetRows(int from, int to);
Vector3 GetRow3(int rowIx);
void SetRow(int rowIx, Matrix1 source);
void SetRow3(int rowIx, Vector3 v);
Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
void UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const;
void UpdateSlice(int rowStart,
int rowStop,
int colStart,
int colStop,
const Matrix2& m) const;
Matrix2 DeleteRows(int rowStart, int rowStop);
Matrix2 DeleteColumns(int colStart, int colStop);
Matrix2 operator-() const;
@ -57,7 +94,9 @@ 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& v);
Matrix2 operator-(const Matrix2& v) const;
Matrix2 operator*(const Matrix2& m) const;
friend Matrix2 operator*(const Matrix2& m, float f) {
@ -73,15 +112,7 @@ class Matrix2 {
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 Matrix1 operator*(const Matrix2& m, const Matrix1& v);
friend Matrix2 operator/(const Matrix2& m, float f) {
Matrix2 r = Matrix2(m.nRows, m.nCols);
@ -96,20 +127,19 @@ class Matrix2 {
return r;
}
Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
void UpdateSlice(int rowStart,
int rowStop,
int colStart,
int colStop,
const Matrix2& m) const;
// private:
// move constructor and move assignment operator
Matrix2(Matrix2&& other) noexcept;
Matrix2& operator=(Matrix2&& other) noexcept;
// Matrix2(Matrix2&& other) noexcept;
// Matrix2& operator=(Matrix2&& other) noexcept;
static Matrix2 Omega(const Vector3& v);
public:
int nRows = 0;
int nCols = 0;
protected:
int nValues = 0;
float* data = nullptr;
private:
bool externalData = true;
};

10
LinearAlgebra/Quaternion.cpp
View File

@ -99,14 +99,12 @@ 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;
float* data = new float[9]; // 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);
@ -117,6 +115,8 @@ Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
data[2 * 3 + 1] = 2 * (y * z + w * x);
data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
Matrix2 r = Matrix2(3, 3, data, true);
return r;
}
@ -168,6 +168,10 @@ Quaternion Quaternion::Inverse(Quaternion r) {
return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n);
}
Quaternion Quaternion::Reflect(Quaternion q) {
return Quaternion(-q.x, -q.y, -q.z, q.w);
}
Quaternion Quaternion::LookRotation(const Vector3& forward) {
Vector3 up = Vector3(0, 1, 0);
return LookRotation(forward, up);

2
LinearAlgebra/Quaternion.h
View File

@ -126,6 +126,8 @@ struct Quaternion : Quat {
/// needed</param> <returns>The inverted quaternion</returns>
static Quaternion Inverse(Quaternion quaternion);
static Quaternion Reflect(Quaternion q);
/// <summary>
/// A rotation which looks in the given direction
/// </summary>

10
LinearAlgebra/Vector2.cpp
View File

@ -180,3 +180,13 @@ Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) {
Vector2 v = v1 + (v2 - v1) * f;
return v;
}
#pragma region Vector2Of
template <typename T>
Vector2Of<T>::Vector2Of() {
this->horizontal = 0;
this->vertical = 0;
}
#pragma endregion Vector2Of

11
LinearAlgebra/Vector2.h
View File

@ -201,6 +201,17 @@ struct Vector2 : Vec2 {
static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f);
};
template <typename T>
struct Vector2Of {
public:
Vector2Of();
T horizontal = 0;
T vertical = 0;
};
using Vector2Int = Vector2Of<int>;
} // namespace LinearAlgebra
using namespace LinearAlgebra;

340
LinearAlgebra/test/Matrix_test.cc
View File

@ -1,215 +1,215 @@
#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
// #if GTEST
// #include <gtest/gtest.h>
// #include <math.h>
// #include <limits>
#include "Matrix.h"
// #include "Matrix.h"
TEST(Matrix2, Zero) {
// Test case 1: 2x2 zero matrix
Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
EXPECT_TRUE(zeroMatrix.nRows == 2);
EXPECT_TRUE(zeroMatrix.nCols == 2);
for (int i = 0; i < zeroMatrix.nValues; ++i) {
EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
}
std::cout << "Test case 1 passed: 2x2 zero matrix\n";
// TEST(Matrix2, Zero) {
// // Test case 1: 2x2 zero matrix
// Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
// EXPECT_TRUE(zeroMatrix.nRows == 2);
// EXPECT_TRUE(zeroMatrix.nCols == 2);
// for (int i = 0; i < zeroMatrix.nValues; ++i) {
// EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
// }
// std::cout << "Test case 1 passed: 2x2 zero matrix\n";
// Test case 2: 3x3 zero matrix
zeroMatrix = Matrix2::Zero(3, 3);
EXPECT_TRUE(zeroMatrix.nRows == 3);
EXPECT_TRUE(zeroMatrix.nCols == 3);
for (int i = 0; i < zeroMatrix.nValues; ++i) {
EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
}
std::cout << "Test case 2 passed: 3x3 zero matrix\n";
// // Test case 2: 3x3 zero matrix
// zeroMatrix = Matrix2::Zero(3, 3);
// EXPECT_TRUE(zeroMatrix.nRows == 3);
// EXPECT_TRUE(zeroMatrix.nCols == 3);
// for (int i = 0; i < zeroMatrix.nValues; ++i) {
// EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
// }
// std::cout << "Test case 2 passed: 3x3 zero matrix\n";
// Test case 3: 1x1 zero matrix
zeroMatrix = Matrix2::Zero(1, 1);
EXPECT_TRUE(zeroMatrix.nRows == 1);
EXPECT_TRUE(zeroMatrix.nCols == 1);
EXPECT_TRUE(zeroMatrix.data[0] == 0.0f);
std::cout << "Test case 3 passed: 1x1 zero matrix\n";
// // Test case 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 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);
// 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;
// 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";
// 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);
// // 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;
// 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";
// 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;
// // 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";
// 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;
// // 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";
// for (int i = 0; i < 4; ++i)
// EXPECT_TRUE(result4.data[i] == A.data[i]);
// std::cout << "Test 4 passed: Multiplication with identity matrix.\n";
}
// }
TEST(MatrixSingle, Init) {
// zero
MatrixOf<float> m0 = MatrixOf<float>(0, 0);
// TEST(MatrixSingle, Init) {
// // zero
// MatrixOf<float> m0 = MatrixOf<float>(0, 0);
// one
float data1[] = {1.0F};
MatrixOf<float> m1 = MatrixOf<float>(1, 1, data1);
// // one
// float data1[] = {1.0F};
// MatrixOf<float> m1 = MatrixOf<float>(1, 1, data1);
// two
float data2[] = {1.0F, 2.0F, 3.0F, 4.0F};
MatrixOf<float> m2 = MatrixOf<float>(2, 2, data2);
// // two
// float data2[] = {1.0F, 2.0F, 3.0F, 4.0F};
// MatrixOf<float> m2 = MatrixOf<float>(2, 2, data2);
// negative
// MatrixOf<float> m_1 = MatrixOf<float>(-1, -1);
// parameters are unsigned
}
// // negative
// // MatrixOf<float> m_1 = MatrixOf<float>(-1, -1);
// // parameters are unsigned
// }
TEST(MatrixSingle, Transpose) {
float data1[] = {1.0F};
MatrixOf<float> m = MatrixOf<float>(1, 1, data1);
// TEST(MatrixSingle, Transpose) {
// float data1[] = {1.0F};
// MatrixOf<float> m = MatrixOf<float>(1, 1, data1);
MatrixOf<float> r = MatrixOf<float>(1, 1);
m.Transpose(&r);
// MatrixOf<float> r = MatrixOf<float>(1, 1);
// m.Transpose(&r);
// 2 x 2
// // 2 x 2
float data3[] = {1.0F, 2.0F, 3.0F, 4.0F};
MatrixOf<float> m22 = MatrixOf<float>(2, 2, data3);
EXPECT_EQ(m22.RowCount(), 2);
EXPECT_EQ(m22.ColCount(), 2);
// float data3[] = {1.0F, 2.0F, 3.0F, 4.0F};
// MatrixOf<float> m22 = MatrixOf<float>(2, 2, data3);
// EXPECT_EQ(m22.RowCount(), 2);
// EXPECT_EQ(m22.ColCount(), 2);
float data4[] = {0.0F, 0.0F, 0.0F, 0.0F};
MatrixOf<float> r22 = MatrixOf<float>(2, 2, data4);
EXPECT_EQ(r22.RowCount(), 2);
EXPECT_EQ(r22.ColCount(), 2);
// float data4[] = {0.0F, 0.0F, 0.0F, 0.0F};
// MatrixOf<float> r22 = MatrixOf<float>(2, 2, data4);
// EXPECT_EQ(r22.RowCount(), 2);
// EXPECT_EQ(r22.ColCount(), 2);
m22.Transpose(&r22);
EXPECT_EQ(r22.RowCount(), 2);
EXPECT_EQ(r22.ColCount(), 2);
EXPECT_FLOAT_EQ(r22.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(r22.Get(0, 1), 3.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 0), 2.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 1), 4.0F);
// m22.Transpose(&r22);
// EXPECT_EQ(r22.RowCount(), 2);
// EXPECT_EQ(r22.ColCount(), 2);
// EXPECT_FLOAT_EQ(r22.Get(0, 0), 1.0F);
// EXPECT_FLOAT_EQ(r22.Get(0, 1), 3.0F);
// EXPECT_FLOAT_EQ(r22.Get(1, 0), 2.0F);
// EXPECT_FLOAT_EQ(r22.Get(1, 1), 4.0F);
// 1 x 2
float data12[] = {1.0F, 2.0F};
MatrixOf<float> m12 = MatrixOf<float>(1, 2, data12);
EXPECT_EQ(m12.RowCount(), 1);
EXPECT_EQ(m12.ColCount(), 2);
// // 1 x 2
// float data12[] = {1.0F, 2.0F};
// MatrixOf<float> m12 = MatrixOf<float>(1, 2, data12);
// EXPECT_EQ(m12.RowCount(), 1);
// EXPECT_EQ(m12.ColCount(), 2);
float data21[] = {0.0F, 0.0F};
MatrixOf<float> r21 = MatrixOf<float>(2, 1, data21);
EXPECT_EQ(r21.RowCount(), 2);
EXPECT_EQ(r21.ColCount(), 1);
// float data21[] = {0.0F, 0.0F};
// MatrixOf<float> r21 = MatrixOf<float>(2, 1, data21);
// EXPECT_EQ(r21.RowCount(), 2);
// EXPECT_EQ(r21.ColCount(), 1);
m12.Transpose(&r21);
EXPECT_EQ(r21.RowCount(), 2);
EXPECT_EQ(r21.ColCount(), 1);
EXPECT_FLOAT_EQ(r21.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(r21.Get(1, 0), 2.0F);
// m12.Transpose(&r21);
// EXPECT_EQ(r21.RowCount(), 2);
// EXPECT_EQ(r21.ColCount(), 1);
// EXPECT_FLOAT_EQ(r21.Get(0, 0), 1.0F);
// EXPECT_FLOAT_EQ(r21.Get(1, 0), 2.0F);
// changing dimensions, same size is okay
MatrixOf<float> r12 = MatrixOf<float>(1, 2, data21);
EXPECT_EQ(r12.RowCount(), 1);
EXPECT_EQ(r12.ColCount(), 2);
// // changing dimensions, same size is okay
// MatrixOf<float> r12 = MatrixOf<float>(1, 2, data21);
// EXPECT_EQ(r12.RowCount(), 1);
// EXPECT_EQ(r12.ColCount(), 2);
m12.Transpose(&r12);
EXPECT_EQ(r12.RowCount(), 2);
EXPECT_EQ(r12.ColCount(), 1);
EXPECT_FLOAT_EQ(r12.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(r12.Get(0, 1), 2.0F);
}
// m12.Transpose(&r12);
// EXPECT_EQ(r12.RowCount(), 2);
// EXPECT_EQ(r12.ColCount(), 1);
// EXPECT_FLOAT_EQ(r12.Get(0, 0), 1.0F);
// EXPECT_FLOAT_EQ(r12.Get(0, 1), 2.0F);
// }
TEST(MatrixSingle, Multiply) {
float m12data[] = {1.0F, 2.0F};
MatrixOf<float> m12 = MatrixOf<float>(1, 2, m12data);
// TEST(MatrixSingle, Multiply) {
// float m12data[] = {1.0F, 2.0F};
// MatrixOf<float> m12 = MatrixOf<float>(1, 2, m12data);
EXPECT_EQ(m12.RowCount(), 1);
EXPECT_EQ(m12.ColCount(), 2);
EXPECT_FLOAT_EQ(m12.Get(0, 0), 1.0F);
EXPECT_FLOAT_EQ(m12.Get(0, 1), 2.0F);
// EXPECT_EQ(m12.RowCount(), 1);
// EXPECT_EQ(m12.ColCount(), 2);
// EXPECT_FLOAT_EQ(m12.Get(0, 0), 1.0F);
// EXPECT_FLOAT_EQ(m12.Get(0, 1), 2.0F);
float m21data[] = {3.0F, 4.0F};
MatrixOf<float> m21 = MatrixOf<float>(2, 1, m21data);
// float m21data[] = {3.0F, 4.0F};
// MatrixOf<float> m21 = MatrixOf<float>(2, 1, m21data);
EXPECT_EQ(m21.RowCount(), 2);
EXPECT_EQ(m21.ColCount(), 1);
EXPECT_FLOAT_EQ(m21.Get(0, 0), 3.0F);
EXPECT_FLOAT_EQ(m21.Get(1, 0), 4.0F);
// EXPECT_EQ(m21.RowCount(), 2);
// EXPECT_EQ(m21.ColCount(), 1);
// EXPECT_FLOAT_EQ(m21.Get(0, 0), 3.0F);
// EXPECT_FLOAT_EQ(m21.Get(1, 0), 4.0F);
float r11data[] = {0.0F};
MatrixOf<float> r11 = MatrixOf<float>(1, 1, r11data);
// float r11data[] = {0.0F};
// MatrixOf<float> r11 = MatrixOf<float>(1, 1, r11data);
EXPECT_EQ(r11.RowCount(), 1);
EXPECT_EQ(r11.ColCount(), 1);
// EXPECT_EQ(r11.RowCount(), 1);
// EXPECT_EQ(r11.ColCount(), 1);
MatrixOf<float>::Multiply(&m12, &m21, &r11);
EXPECT_EQ(r11.RowCount(), 1);
EXPECT_EQ(r11.ColCount(), 1);
EXPECT_FLOAT_EQ(r11.Get(0, 0), 11.0F);
// MatrixOf<float>::Multiply(&m12, &m21, &r11);
// EXPECT_EQ(r11.RowCount(), 1);
// EXPECT_EQ(r11.ColCount(), 1);
// EXPECT_FLOAT_EQ(r11.Get(0, 0), 11.0F);
float r22data[] = {0.0F, 0.0F, 0.0F, 0.0F};
MatrixOf<float> r22 = MatrixOf<float>(2, 2, r22data);
// float r22data[] = {0.0F, 0.0F, 0.0F, 0.0F};
// MatrixOf<float> r22 = MatrixOf<float>(2, 2, r22data);
MatrixOf<float>::Multiply(&m21, &m12, &r22);
EXPECT_EQ(r22.RowCount(), 2);
EXPECT_EQ(r22.ColCount(), 2);
EXPECT_FLOAT_EQ(r22.Get(0, 0), 3.0F);
EXPECT_FLOAT_EQ(r22.Get(0, 1), 4.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 0), 6.0F);
EXPECT_FLOAT_EQ(r22.Get(1, 1), 8.0F);
}
// MatrixOf<float>::Multiply(&m21, &m12, &r22);
// EXPECT_EQ(r22.RowCount(), 2);
// EXPECT_EQ(r22.ColCount(), 2);
// EXPECT_FLOAT_EQ(r22.Get(0, 0), 3.0F);
// EXPECT_FLOAT_EQ(r22.Get(0, 1), 4.0F);
// EXPECT_FLOAT_EQ(r22.Get(1, 0), 6.0F);
// EXPECT_FLOAT_EQ(r22.Get(1, 1), 8.0F);
// }
TEST(MatrixSingle, Multiply_Vector3) {
Vector3 v = Vector3(1.0, 2.0, 3.0);
Vector3 r = Vector3::zero;
// TEST(MatrixSingle, Multiply_Vector3) {
// Vector3 v = Vector3(1.0, 2.0, 3.0);
// Vector3 r = Vector3::zero;
// float m13data[] = {3.0, 4.0, 5.0};
// MatrixOf<float> m13 = MatrixOf<float>(1, 3, m13data);
// Vector3 r = MatrixOf<float>::Multiply(&m13, v);
// // float m13data[] = {3.0, 4.0, 5.0};
// // MatrixOf<float> m13 = MatrixOf<float>(1, 3, m13data);
// // Vector3 r = MatrixOf<float>::Multiply(&m13, v);
float m33data[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
MatrixOf<float> m33 = MatrixOf<float>(3, 3, m33data);
r = MatrixOf<float>::Multiply(&m33, v);
EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(1.0f, 2.0f, 3.0f)), 0);
}
// float m33data[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
// MatrixOf<float> m33 = MatrixOf<float>(3, 3, m33data);
// r = MatrixOf<float>::Multiply(&m33, v);
// EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(1.0f, 2.0f, 3.0f)), 0);
// }
#endif
// #endif

158
Participant.cpp
View File

@ -19,34 +19,9 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
Participant::Participant() {
Thing::CreateRoot(this);
//this->Add(this->root);
}
/*
Participant::Participant(const char* ipAddress, int port) {
Thing::CreateRoot(this);
//this->Add(this->root);
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)
strncpy_s(addressString, stringLength, ipAddress,
addressLength); // Leave space for null terminator
#else
strncpy(addressString, ipAddress, addressLength);
#endif
addressString[addressLength] = '\0';
this->ipAddress = addressString;
this->port = port;
}
*/
Participant::~Participant() {
// registry.Remove(this);
// delete[] this->ipAddress;
}
Participant::~Participant() {}
void Participant::Update(bool recurse) {
for (Thing* thing : this->things) {
@ -57,47 +32,19 @@ void Participant::Update(bool recurse) {
bool Participant::Send(IMessage* msg) {
std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff)
<< " to base Participant without communcation support " << std::endl;
<< " to base Participant without communcation support "
<< std::endl;
return true;
}
/*
bool Participant::Send(IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
return true;
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
// << " to " << this->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->SendTo(this, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->SendTo(this, bufferSize);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->SendTo(this, bufferSize);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->SendTo(this, bufferSize);
#else
return false;
#endif
}
*/
Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
for (Thing* thing : this->things) {
if (thing->owner->networkId == networkId && thing->id == thingId)
return thing;
}
std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port
<< "[" << (int)thingId << "]\n";
std::cout << "Could not find thing " << "[" << (int)networkId << ": "
<< (int)thingId << "]\n";
return nullptr;
}
@ -120,8 +67,7 @@ void Participant::Add(Thing* thing, bool checkId) {
this->things.push_back(thing);
#endif
std::cout << "Add thing with generated ID "
//<< this->ipAddress << ":" << this->port
<< "[" << (int)thing->id << "]\n";
<< "[" << (int)networkId << ": " << (int)thing->id << "]\n";
} else {
Thing* foundThing = Get(thing->owner->networkId, thing->id);
if (foundThing == nullptr) {
@ -130,13 +76,11 @@ void Participant::Add(Thing* thing, bool checkId) {
#else
this->things.push_back(thing);
#endif
std::cout << "Add thing " << //this->ipAddress << ":" << this->port <<
"["
<< (int)thing->id << "]\n";
std::cout << "Add thing [" << (int)networkId << ": " << (int)thing->id
<< "]\n";
} else {
std::cout << "Did not add, existing thing "
//<< this->ipAddress << ":" << this->port
<< "[" << (int)thing->id << "]\n";
<< "[" << (int)networkId << ": " << (int)thing->id << "]\n";
}
}
}
@ -164,88 +108,4 @@ void Participant::Remove(Thing* thing) {
#pragma endregion
// #pragma region ParticipantRegistry
// /*
// Participant* ParticipantRegistry::Get(const char* ipAddress,
// unsigned int port) {
// #if !defined(NO_STD)
// for (Participant* participant : ParticipantRegistry::participants) {
// if (participant == nullptr)
// continue;
// if (strcmp(participant->ipAddress, ipAddress) == 0 &&
// participant->port == port) {
// // std::cout << "found participant " << participant->ipAddress << ":"
// // << (int)participant->port << std::endl;
// return participant;
// }
// }
// std::cout << "Could not find participant " << ipAddress << ":" << (int)port
// << std::endl;
// #endif
// return nullptr;
// }
// */
// Participant* ParticipantRegistry::Get(unsigned char participantId) {
// #if !defined(NO_STD)
// for (Participant* participant : ParticipantRegistry::participants) {
// if (participant == nullptr)
// continue;
// if (participant->networkId == participantId)
// return participant;
// }
// std::cout << "Could not find participant " << (int)participantId << std::endl;
// #endif
// return nullptr;
// }
// // Participant* ParticipantRegistry::Add(const char* ipAddress,
// // unsigned int port) {
// // Participant* participant = new Participant(ipAddress, port);
// // Add(participant);
// // return participant;
// // }
// void ParticipantRegistry::Add(Participant* participant) {
// Participant* foundParticipant =
// Get(participant->networkId);
// //Get(participant->ipAddress, participant->port);
// if (foundParticipant == nullptr) {
// #if defined(NO_STD)
// // this->things[this->thingCount++] = thing;
// #else
// ParticipantRegistry::participants.push_back(participant);
// #endif
// // std::cout << "Add participant " << participant->ipAddress << ":"
// // << participant->port << "[" << (int)participant->networkId
// // << "]\n";
// // std::cout << "participants " <<
// // ParticipantRegistry::participants.size()
// // << "\n";
// // } else {
// // std::cout << "Did not add, existing participant " <<
// // participant->ipAddress
// // << ":" << participant->port << "[" <<
// // (int)participant->networkId
// // << "]\n";
// }
// }
// void ParticipantRegistry::Remove(Participant* participant) {
// // participants.remove(participant);
// }
// #if defined(NO_STD)
// Participant** ParticipantRegistry::GetAll() const {
// return ParticipantRegistry::participants;
// }
// #else
// const std::list<Participant*>& ParticipantRegistry::GetAll() const {
// return ParticipantRegistry::participants;
// }
// #endif
// #pragma endregion ParticipantRegistry
} // namespace RoboidControl

55
Participant.h
View File

@ -7,55 +7,6 @@ namespace RoboidControl {
constexpr int MAX_THING_COUNT = 256;
/*
/// @brief class which manages all known participants
class ParticipantRegistry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
//Participant* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
Participant* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
//Participant* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(Participant* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(Participant* participant);
private:
#if defined(NO_STD)
public:
Participant** GetAll() const;
int count = 0;
private:
Participant** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<Participant*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<Participant*> participants;
#endif
};
*/
/// @brief A participant is a device which manages things.
/// It can communicate with other participant to synchronise the state of
/// things. This class is used to register the things the participant is
@ -137,12 +88,6 @@ class Participant {
#pragma endregion Send
// #pragma region Participant Registry
// public:
// static ParticipantRegistry registry;
// #pragma endregion Participant Registry
};
} // namespace RoboidControl

187
Participants/ParticipantMQTT.cpp
View File

@ -13,9 +13,9 @@
namespace RoboidControl {
MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port)
: RemoteParticipantUDP("127.0.0.1", port) {
: ParticipantSocket("127.0.0.1", port) {
this->name = "ParticipantUDP";
this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
this->remoteSite = new ParticipantSocket(ipAddress, port);
// Participant::registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
@ -33,11 +33,120 @@ void MQTTParticipantBase::SetupTCP() {
#define MQTT_CONNECT 0x10
#define MQTT_CONNECT_ACK 0x20
void MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
// MQTT CONNECT packet construction
std::vector<uint8_t> createConnectPacket(const std::string& clientId) {
std::vector<uint8_t> packet;
// Fixed header
packet.push_back(0x10); // CONNECT packet type
// Remaining length calculation
int remainingLength =
2 + 4 + // Protocol name length and name (MQTT)
1 + // Protocol level
1 + // Connect flags
2 + // Keep alive
2 + clientId.length(); // Client ID
// Encode remaining length
do {
uint8_t encodedByte = remainingLength % 128;
remainingLength /= 128;
if (remainingLength > 0) {
encodedByte |= 128;
}
packet.push_back(encodedByte);
} while (remainingLength > 0);
// Protocol name (MQTT)
packet.push_back(0x00);
packet.push_back(0x04);
packet.push_back('M');
packet.push_back('Q');
packet.push_back('T');
packet.push_back('T');
// Protocol level (v3.1.1)
packet.push_back(0x04);
// Connect flags
packet.push_back(0x02); // Clean session
// Keep alive (10 seconds)
packet.push_back(0x00);
packet.push_back(0x0A);
// Client ID
packet.push_back(0x00);
packet.push_back(static_cast<uint8_t>(clientId.length()));
packet.insert(packet.end(), clientId.begin(), clientId.end());
return packet;
}
void MQTTParticipantBase::send_mqtt_connect(const char* clientId) {
std::vector<uint8_t> packet;
// Fixed header
packet.push_back(0x10); // CONNECT packet type
// Calculate client ID length
size_t clientIdLength = strlen(clientId);
// Remaining length calculation
int remainingLength =
2 + 4 + // Protocol name length and name (MQTT)
1 + // Protocol level
1 + // Connect flags
2 + // Keep alive
2 + clientIdLength; // Client ID
// Encode remaining length
do {
uint8_t encodedByte = remainingLength % 128;
remainingLength /= 128;
if (remainingLength > 0) {
encodedByte |= 128;
}
packet.push_back(encodedByte);
} while (remainingLength > 0);
// Protocol name (MQTT)
packet.push_back(0x00);
packet.push_back(0x04);
packet.push_back('M');
packet.push_back('Q');
packet.push_back('T');
packet.push_back('T');
// Protocol level (v3.1.1)
packet.push_back(0x04);
// Connect flags
packet.push_back(0x02); // Clean session
// Keep alive (10 seconds)
packet.push_back(0x00);
packet.push_back(0x0A);
// Client ID
packet.push_back(0x00);
packet.push_back(static_cast<uint8_t>(clientIdLength));
packet.insert(packet.end(), clientId, clientId + clientIdLength);
for (int i = 0; i < packet.size(); i++)
this->buffer[i] = packet.data()[i];
int buffersize = packet.size();
SendTCP(buffersize);
//send(sock, (const char*)connectPacket.data(), connectPacket.size(), 0);
/*
MQTTConnectPacket packet;
packet.fixed_header = MQTT_CONNECT;
packet.remaining_length = // 14;
2 + 4 + 2 + strlen(client_id) +
2 + 4 + 2 + strlen(clientId) +
3; // Protocol name + protocol level + connect
// flags + keep alive + client ID
packet.protocol_name_length = 4; // "MQTT"
@ -60,10 +169,10 @@ void MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
this->buffer[index++] = packet.connect_flags;
this->buffer[index++] = packet.keep_alive & 0xFF; // LSB
this->buffer[index++] = (packet.keep_alive >> 8) & 0xFF; // MSB
size_t client_id_length = strlen(client_id);
size_t client_id_length = strlen(clientId);
this->buffer[index++] = (client_id_length >> 8) & 0xFF; // MSB
this->buffer[index++] = client_id_length & 0xFF; // LSB
memcpy(&this->buffer[index], client_id, client_id_length);
memcpy(&this->buffer[index], clientId, client_id_length);
index += client_id_length;
for (int ix = 0; ix < index; ix++)
@ -72,11 +181,70 @@ void MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
// Send the MQTT connect packet
SendTCP(index);
std::cout << "Send connect, client ID = " << client_id << std::endl;
*/
std::cout << "Send connect, client ID = " << clientId << std::endl;
}
uint16_t packetId;
// Generate packet ID (incremental)
uint16_t getNextPacketId() {
return ++packetId;
}
std::vector<uint8_t> createSubscribePacket(const char* topic, uint8_t qos) {
std::vector<uint8_t> packet;
// Fixed header
packet.push_back(0x82); // SUBSCRIBE packet type with flags
// Calculate packet length
size_t topicLength = strlen(topic);
int remainingLength =
2 + // Packet ID
2 + // Topic length
topicLength +
1; // QoS byte
// Encode remaining length
do {
uint8_t encodedByte = remainingLength % 128;
remainingLength /= 128;
if (remainingLength > 0) {
encodedByte |= 128;
}
packet.push_back(encodedByte);
} while (remainingLength > 0);
// Packet ID (2 bytes)
uint16_t packetIdentifier = getNextPacketId();
packet.push_back((packetIdentifier >> 8) & 0xFF); // High byte
packet.push_back(packetIdentifier & 0xFF); // Low byte
// Topic length (2 bytes)
packet.push_back((topicLength >> 8) & 0xFF);
packet.push_back(topicLength & 0xFF);
// Topic string
packet.insert(packet.end(), topic, topic + topicLength);
// QoS (1 byte)
packet.push_back(qos);
return packet;
}
void MQTTParticipantBase::sendSubscribe(const char* topic) {
std::vector<uint8_t> packet = createSubscribePacket(topic, 0);
for (int i = 0; i < packet.size(); i++)
this->buffer[i] = packet.data()[i];
int buffersize = packet.size();
SendTCP(buffersize);
/*
// Packet Identifier (2 bytes)
static unsigned short packetId =
1; // Increment this for each new subscription
@ -109,6 +277,7 @@ void MQTTParticipantBase::sendSubscribe(const char* topic) {
// Send the SUBSCRIBE packet
SendTCP(7 + topicLength);
*/
std::cout << "Send subscribe to topic: " << topic << std::endl;
}
@ -128,7 +297,7 @@ int MQTTParticipantBase::ReceiveTCP() {
void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) {
std::cout << " receivemsg\n";
// std::cout << "receive msg " << (int)msgId << "\n";
// std::cout << " buffer size = " <<(int) bufferSize << "\n";
std::cout << " buffer size = " <<(int) bufferSize << "\n";
};
// void ParticipantMQTT::receiveMessages(int sock) {

5
Participants/ParticipantMQTT.h
View File

@ -43,7 +43,7 @@ namespace RoboidControl {
/// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing()
class MQTTParticipantBase : public RemoteParticipantUDP {
class MQTTParticipantBase : public ParticipantSocket {
#pragma region Init
public:
@ -60,7 +60,7 @@ protected:
public:
/// @brief The remote site when this participant is connected to a site
RemoteParticipantUDP* remoteSite = nullptr;
ParticipantSocket* remoteSite = nullptr;
protected:
@ -128,3 +128,4 @@ protected:
} // namespace RoboidControl
#include "Posix/PosixMQTT.h"
#include "Windows/WindowsMQTT.h"

24
Participants/ParticipantSocket.cpp Normal file
View File

@ -0,0 +1,24 @@
#include "ParticipantSocket.h"
#include <string.h>
namespace RoboidControl {
ParticipantSocket::ParticipantSocket(const char* ipAddress, int port) {
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)
strncpy_s(addressString, stringLength, ipAddress,
addressLength); // Leave space for null terminator
#else
strncpy(addressString, ipAddress, addressLength);
#endif
addressString[addressLength] = '\0';
this->ipAddress = addressString;
this->port = port;
}
}

28
Participants/ParticipantSocket.h Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include "Participant.h"
namespace RoboidControl {
class ParticipantSocket : public Participant {
public:
/// @brief Create a new participant with the given communcation info
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
ParticipantSocket(const char* ipAddress, int port);
/// @brief The Ip Address of a participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
unsigned int port = 0;
bool Send(IMessage* msg) override;
};
}

232
Participants/ParticipantUDP.cpp
View File

@ -12,112 +12,11 @@
#include "Things/DistanceSensor.h"
#include "Things/TouchSensor.h"
#include <string.h>
// #include <string.h>
namespace RoboidControl {
#pragma region ParticipantRegistry
ParticipantRegistry ParticipantUDPGeneric::registry;
RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress,
unsigned int port) {
#if !defined(NO_STD)
for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
if (participant == nullptr)
continue;
if (strcmp(participant->ipAddress, ipAddress) == 0 &&
participant->port == port) {
// std::cout << "found participant " << participant->ipAddress << ":"
// << (int)participant->port << std::endl;
return participant;
}
}
std::cout << "Could not find participant " << ipAddress << ":" << (int)port
<< std::endl;
#endif
return nullptr;
}
RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) {
#if !defined(NO_STD)
for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
if (participant == nullptr)
continue;
if (participant->networkId == participantId)
return participant;
}
std::cout << "Could not find participant " << (int)participantId << std::endl;
#endif
return nullptr;
}
RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress,
unsigned int port) {
RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port);
Add(participant);
return participant;
}
void ParticipantRegistry::Add(RemoteParticipantUDP* participant) {
Participant* foundParticipant = Get(participant->networkId);
// Get(participant->ipAddress, participant->port);
if (foundParticipant == nullptr) {
#if defined(NO_STD)
// this->things[this->thingCount++] = thing;
#else
ParticipantRegistry::participants.push_back(participant);
#endif
// std::cout << "Add participant " << participant->ipAddress << ":"
// << participant->port << "[" << (int)participant->networkId
// << "]\n";
// std::cout << "participants " <<
// ParticipantRegistry::participants.size()
// << "\n";
// } else {
// std::cout << "Did not add, existing participant " <<
// participant->ipAddress
// << ":" << participant->port << "[" <<
// (int)participant->networkId
// << "]\n";
}
}
void ParticipantRegistry::Remove(RemoteParticipantUDP* participant) {
// participants.remove(participant);
}
#if defined(NO_STD)
RemoteParticipantUDP** ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#else
const std::list<RemoteParticipantUDP*>& ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#endif
#pragma endregion ParticipantRegistry
RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) {
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)
strncpy_s(addressString, stringLength, ipAddress,
addressLength); // Leave space for null terminator
#else
strncpy(addressString, ipAddress, addressLength);
#endif
addressString[addressLength] = '\0';
this->ipAddress = addressString;
this->port = port;
}
bool RemoteParticipantUDP::Send(IMessage* msg) {
bool ParticipantSocket::Send(IMessage* msg) {
// No message is actually sent, because this class has no networking
// implementation
return false;
@ -126,7 +25,7 @@ bool RemoteParticipantUDP::Send(IMessage* msg) {
#pragma region Init
ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
: RemoteParticipantUDP("127.0.0.1", port) {
: ParticipantSocket("127.0.0.1", port) {
this->name = "ParticipantUDP";
this->remoteSite = nullptr;
if (this->port == 0)
@ -145,12 +44,12 @@ ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
int port,
int localPort)
: RemoteParticipantUDP("127.0.0.1", localPort) {
: ParticipantSocket("127.0.0.1", localPort) {
this->name = "ParticipantUDP";
if (this->port == 0)
this->isIsolated = true;
else
this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
this->remoteSite = new ParticipantSocket(ipAddress, port);
registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
@ -196,7 +95,7 @@ void ParticipantUDPGeneric::Update(bool recurse) {
this->nextPublishMe = currentTimeMs + this->publishInterval;
}
//this->ReceiveUDP();
this->ReceiveUDP();
}
UpdateMyThings();
@ -311,8 +210,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
unsigned int senderPort) {
// std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
// << std::endl;
RemoteParticipantUDP* sender =
this->registry.Get(senderIpAddress, senderPort);
ParticipantSocket* sender = this->registry.Get(senderIpAddress, senderPort);
if (sender == nullptr) {
sender = this->registry.Add(senderIpAddress, senderPort);
#if !defined(NO_STD)
@ -325,7 +223,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
}
void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
RemoteParticipantUDP* sender) {
ParticipantSocket* sender) {
unsigned char msgId = this->buffer[0];
// std::cout << "receive msg " << (int)msgId << "\n";
// std::cout << " buffer size = " <<(int) bufferSize << "\n";
@ -398,7 +296,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
ParticipantMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
@ -406,7 +304,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
NetworkIdMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
@ -422,7 +320,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
}
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
InvestigateMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
@ -430,16 +328,15 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
ThingMsg* msg) {
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, ThingMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
<< (int)msg->parentId << "\n";
#endif
RemoteParticipantUDP* owner = registry.Get(msg->networkId);
ParticipantSocket* owner = registry.Get(msg->networkId);
if (owner == nullptr) {
owner = new RemoteParticipantUDP(sender->ipAddress, sender->port);
owner = new ParticipantSocket(sender->ipAddress, sender->port);
owner->networkId = msg->networkId;
registry.Add(owner);
}
@ -461,7 +358,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
thing->SetParent(nullptr);
}
Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
Thing* ParticipantUDPGeneric::ProcessNewThing(ParticipantSocket* owner,
ThingMsg* msg,
bool isRemote) {
switch (msg->thingType) {
@ -476,8 +373,7 @@ Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
}
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
NameMsg* msg) {
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, NameMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "] ";
@ -509,7 +405,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
ModelUrlMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
@ -517,8 +413,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
PoseMsg* msg) {
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, PoseMsg* msg) {
#if !defined(DEBUG) && !defined(NO_STD)
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
<< "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
@ -541,8 +436,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
thing->SetAngularVelocity(msg->angularVelocity);
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
BinaryMsg* msg) {
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, BinaryMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n";
@ -564,15 +458,14 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
}
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
TextMsg* msg) {
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, TextMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
<< (int)msg->text << "\n";
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
DestroyMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
@ -587,4 +480,87 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
// Receive
#pragma endregion
#pragma region Registry
ParticipantUDP::Registry ParticipantUDPGeneric::registry;
ParticipantSocket* ParticipantUDP::Registry::Get(const char* ipAddress, unsigned int port) {
#if !defined(NO_STD)
for (ParticipantSocket* participant : Registry::participants) {
if (participant == nullptr)
continue;
if (strcmp(participant->ipAddress, ipAddress) == 0 &&
participant->port == port) {
// std::cout << "found participant " << participant->ipAddress << ":"
// << (int)participant->port << std::endl;
return participant;
}
}
std::cout << "Could not find participant " << ipAddress << ":" << (int)port
<< std::endl;
#endif
return nullptr;
}
ParticipantSocket* ParticipantUDP::Registry::Get(unsigned char participantId) {
#if !defined(NO_STD)
for (ParticipantSocket* participant : Registry::participants) {
if (participant == nullptr)
continue;
if (participant->networkId == participantId)
return participant;
}
std::cout << "Could not find participant " << (int)participantId << std::endl;
#endif
return nullptr;
}
ParticipantSocket* ParticipantUDP::Registry::Add(const char* ipAddress, unsigned int port) {
ParticipantSocket* participant = new ParticipantSocket(ipAddress, port);
Add(participant);
return participant;
}
void ParticipantUDP::Registry::Add(ParticipantSocket* participant) {
Participant* foundParticipant = Get(participant->networkId);
// Get(participant->ipAddress, participant->port);
if (foundParticipant == nullptr) {
#if defined(NO_STD)
// this->things[this->thingCount++] = thing;
#else
Registry::participants.push_back(participant);
#endif
// std::cout << "Add participant " << participant->ipAddress << ":"
// << participant->port << "[" << (int)participant->networkId
// << "]\n";
// std::cout << "participants " <<
// Registry::participants.size()
// << "\n";
// } else {
// std::cout << "Did not add, existing participant " <<
// participant->ipAddress
// << ":" << participant->port << "[" <<
// (int)participant->networkId
// << "]\n";
}
}
void ParticipantUDP::Registry::Remove(ParticipantSocket* participant) {
// participants.remove(participant);
}
#if defined(NO_STD)
RemoteParticipantUDP** Registry::GetAll() const {
return Registry::participants;
}
#else
const std::list<ParticipantSocket*>& ParticipantUDP::Registry::GetAll() const {
return Registry::participants;
}
#endif
// Registry
#pragma endregion Registry
} // namespace RoboidControl

153
Participants/ParticipantUDP.h
View File

@ -10,7 +10,8 @@
#include "Messages/PoseMsg.h"
#include "Messages/TextMsg.h"
#include "Messages/ThingMsg.h"
#include "Participant.h"
#include "ParticipantSocket.h"
#if !defined(NO_STD)
#include <functional>
@ -31,74 +32,6 @@ namespace RoboidControl {
constexpr int MAX_SENDER_COUNT = 256;
class RemoteParticipantUDP : public Participant {
public:
/// @brief Create a new participant with the given communcation info
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
RemoteParticipantUDP(const char* ipAddress, int port);
/// @brief The Ip Address of a participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
unsigned int port = 0;
bool Send(IMessage* msg) override;
};
/// @brief class which manages all known participants
class ParticipantRegistry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
RemoteParticipantUDP* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
RemoteParticipantUDP* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(RemoteParticipantUDP* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(RemoteParticipantUDP* participant);
private:
#if defined(NO_STD)
public:
RemoteParticipantUDP** GetAll() const;
int count = 0;
private:
RemoteParticipantUDP** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<RemoteParticipantUDP*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<RemoteParticipantUDP*> participants;
#endif
};
/// @brief A participant using UDP communication
/// A local participant is the local device which can communicate with
/// other participants It manages all local things and communcation with other
@ -111,7 +44,7 @@ class ParticipantRegistry {
/// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing()
class ParticipantUDPGeneric : public RemoteParticipantUDP {
class ParticipantUDPGeneric : public ParticipantSocket {
#pragma region Init
public:
@ -139,7 +72,7 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
bool isIsolated = false;
/// @brief The remote site when this participant is connected to a site
RemoteParticipantUDP* remoteSite = nullptr;
ParticipantSocket* remoteSite = nullptr;
/// The interval in milliseconds for publishing (broadcasting) data on the
/// local network
@ -192,36 +125,86 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
void ReceiveData(unsigned char bufferSize,
char* senderIpAddress,
unsigned int senderPort);
void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant);
void ReceiveData(unsigned char bufferSize,
ParticipantSocket* remoteParticipant);
virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0;
virtual void SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) = 0;
virtual void ReceiveUDP() = 0;
virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg);
virtual void Process(ParticipantSocket* sender, ParticipantMsg* msg);
virtual void Process(ParticipantSocket* sender, NetworkIdMsg* msg);
virtual void Process(ParticipantSocket* sender, InvestigateMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg);
virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender,
virtual void Process(ParticipantSocket* sender, ThingMsg* msg);
virtual Thing* ProcessNewThing(ParticipantSocket* sender,
ThingMsg* msg,
bool isRemote);
virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg);
virtual void Process(ParticipantSocket* sender, NameMsg* msg);
virtual void Process(ParticipantSocket* sender, ModelUrlMsg* msg);
virtual void Process(ParticipantSocket* sender, PoseMsg* msg);
virtual void Process(ParticipantSocket* sender, BinaryMsg* msg);
virtual void Process(ParticipantSocket* sender, TextMsg* msg);
virtual void Process(ParticipantSocket* sender, DestroyMsg* msg);
#pragma endregion Receive
public:
static ParticipantRegistry registry;
/// @brief class which manages all known participants
class Registry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
ParticipantSocket* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
ParticipantSocket* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
ParticipantSocket* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(ParticipantSocket* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(ParticipantSocket* participant);
private:
#if defined(NO_STD)
public:
ParticipantSocket** GetAll() const;
int count = 0;
private:
ParticipantSocket** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<ParticipantSocket*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<ParticipantSocket*> participants;
#endif
};
static Registry registry;
};
} // namespace RoboidControl
#include "EspIdf/EspIdfParticipant.h"
#include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.h"

6
Participants/SiteServer.cpp
View File

@ -56,7 +56,7 @@ void SiteServer::UpdateMyThings() {
#pragma region Receive
void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
void SiteServer::Process(ParticipantSocket* sender, ParticipantMsg* msg) {
if (msg->networkId != sender->networkId) {
// std::cout << this->name << " received New Client -> " <<
// sender->ipAddress
@ -67,9 +67,9 @@ void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
}
}
void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(ParticipantSocket* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) {
void SiteServer::Process(ParticipantSocket* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing == nullptr) {
// new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);

6
Participants/SiteServer.h
View File

@ -33,9 +33,9 @@ class SiteServer : public ParticipantUDP {
protected:
unsigned long nextPublishMe = 0;
virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) override;
virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override;
virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override;
virtual void Process(ParticipantSocket* sender, ParticipantMsg* msg) override;
virtual void Process(ParticipantSocket* sender, NetworkIdMsg* msg) override;
virtual void Process(ParticipantSocket* sender, ThingMsg* msg) override;
#pragma endregion Receive

27
Posix/PosixParticipant.cpp
View File

@ -128,32 +128,7 @@ void ParticipantUDP::ReceiveUDP() {
#endif
}
// int ParticipantUDP::ReceiveTCP() {
// #if defined(__unix__) || defined(__APPLE__)
// int bytesReceived = recv(sock, buffer, sizeof(buffer) - 1, 0);
// if (bytesReceived > 0) {
// buffer[bytesReceived] = '\0'; // Null-terminate the received data
// std::cout << "Received: " << buffer << std::endl;
// return bytesReceived;
// } else if (bytesReceived == 0) {
// // Connection has been gracefully closed
// std::cout << "Connection closed by the server." << std::endl;
// return 0;
// } else {
// if (errno == EAGAIN || errno == EWOULDBLOCK) {
// // No data available to read, continue the loop
// // std::cout << "No data available" << std::endl;
// } else {
// std::cerr << "Error receiving data: " << strerror(errno) << std::endl;
// }
// return 0;
// }
// #endif // _WIN32 || _WIN64
// return 0;
// }
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
int bufferSize) {
bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) {
#if defined(__unix__) || defined(__APPLE__)
// std::cout << "Send to " << remoteParticipant->ipAddress << ":" <<
// ntohs(remoteParticipant->port)

2
Posix/PosixParticipant.h
View File

@ -29,7 +29,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
// void Receive();
int ReceiveTCP();
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg) override;
protected:

102
Windows/WindowsMQTT.cpp Normal file
View File

@ -0,0 +1,102 @@
#include "WindowsMQTT.h"
#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
#include <ws2tcpip.h>
#include <chrono>
#include <thread>
namespace RoboidControl {
MQTTParticipant::MQTTParticipant(const char* remoteIpAddress, int port)
: MQTTParticipantBase(remoteIpAddress, port) {
SetupTCP();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
send_mqtt_connect("RoboidControl");
sendSubscribe("domoticz/out");
}
void MQTTParticipant::SetupTCP() {
// Initialize Winsock
WSADATA wsaData;
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
std::cerr << "WSAStartup failed: " << std::endl;
return;
}
// Create a TCP socket
this->sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
if (sock == INVALID_SOCKET) {
std::cerr << "TCP Socket creation failed: " << WSAGetLastError()
<< std::endl;
WSACleanup();
return;
}
// Define the server address
memset(&this->remote_addr, 0, sizeof(this->remote_addr));
this->remote_addr.sin_family = AF_INET;
this->remote_addr.sin_port = htons((u_short)this->remoteSite->port);
if (inet_pton(AF_INET, this->remoteSite->ipAddress, &this->remote_addr.sin_addr) <= 0) {
std::cerr << "Invalid address" << std::endl;
closesocket(sock);
WSACleanup();
return;
}
// Connect to the server
if (connect(sock, (sockaddr*)&this->remote_addr, sizeof(this->remote_addr)) ==
SOCKET_ERROR) {
std::cerr << "Connection failed: " << WSAGetLastError() << std::endl;
closesocket(sock);
WSACleanup();
return;
}
// Set the socket to non-blocking mode
u_long mode = 1; // 1 to enable non-blocking socket
if (ioctlsocket(sock, FIONBIO, &mode) != NO_ERROR) {
std::cerr << "Failed to set non-blocking mode: " << WSAGetLastError()
<< std::endl;
closesocket(sock);
WSACleanup();
return;
}
}
int MQTTParticipant::ReceiveTCP() {
int bytesReceived = recv(sock, buffer, sizeof(buffer) - 1, 0);
if (bytesReceived > 0) {
buffer[bytesReceived] = '\0'; // Null-terminate the received data
std::cout << "Received: " << buffer << std::endl;
return bytesReceived;
} else if (bytesReceived == 0) {
// Connection has been gracefully closed
std::cout << "Connection closed by the server." << std::endl;
return 0;
} else {
int error = WSAGetLastError();
if (error == WSAEWOULDBLOCK) {
// No data available, continue with other tasks
//std::cout << "No data available, continuing..." << std::endl;
// You can add a sleep or other logic here to avoid busy waiting
} else {
std::cerr << "Receive failed: " << error << std::endl;
}
return 0;
}
return 0;
}
void MQTTParticipant::SendTCP(int bufferSize) {
#if defined(_WIN32) || defined(_WIN64)
send(sock, this->buffer, bufferSize, 0);
#endif
}
} // namespace RoboidControl
#endif

27
Windows/WindowsMQTT.h Normal file
View File

@ -0,0 +1,27 @@
#pragma once
#if defined(_WIN32) || defined(_WIN64)
#include "Participants/ParticipantMQTT.h"
namespace RoboidControl {
class MQTTParticipant : public MQTTParticipantBase {
public:
MQTTParticipant(const char* ipAddress, int port = 1883);
protected:
void SetupTCP() override;
void SendTCP(int bufferSize) override;
int ReceiveTCP() override;
sockaddr_in remote_addr;
public:
int sock;
};
} // namespace RoboidControl
#endif

11
Windows/WindowsParticipant.cpp
View File

@ -9,9 +9,12 @@
namespace RoboidControl {
ParticipantUDP::ParticipantUDP() {}
ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}
void ParticipantUDP::Setup(int localPort,
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
: ParticipantUDPGeneric(ipAddress, port, localPort) {}
void ParticipantUDP::SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) {
#if defined(_WIN32) || defined(_WIN64)
@ -128,7 +131,7 @@ void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) {
#endif // _WIN32 || _WIN64
}
void ParticipantUDP::Receive() {
void ParticipantUDP::ReceiveUDP() {
#if defined(_WIN32) || defined(_WIN64)
// char ip_str[INET_ADDRSTRLEN];
// inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@ -195,7 +198,7 @@ int ParticipantUDP::ReceiveTCP() {
return 0;
}
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
bool ParticipantUDP::SendTo(ParticipantSocket* 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);

9
Windows/WindowsParticipant.h
View File

@ -7,14 +7,15 @@ namespace RoboidControl {
class ParticipantUDP : public ParticipantUDPGeneric {
public:
ParticipantUDP();
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
ParticipantUDP(int port = 7681);
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
void SetupTCP(const char* remoteIpAddres, int remotePort);
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
bool SendTCP(int bufferSize);
bool Publish(IMessage* msg);
void Receive();
void ReceiveUDP();
int ReceiveTCP();
protected: