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

Merge remote-tracking branch 'origin_gitea/main' into Braccio

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-11 11:04:53 +02:00
commit 4ce7f5fb0c
21 changed files with 359 additions and 245 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
EspIdf/EspIdfParticipant.cpp
View File

@ -1,8 +1,6 @@
#include "EspIdfParticipant.h" #include "EspIdfParticipant.h"
#if defined(IDF_VER)
#include "esp_wifi.h" #include "esp_wifi.h"
#endif
namespace RoboidControl { namespace RoboidControl {
namespace EspIdf { namespace EspIdf {
@ -11,6 +9,7 @@ void ParticipantUDP::Setup(int localPort,
const char* remoteIpAddress, const char* remoteIpAddress,
int remotePort) { int remotePort) {
#if defined(IDF_VER) #if defined(IDF_VER)
std::cout << "Set up UDP\n";
GetBroadcastAddress(); GetBroadcastAddress();
wifi_ap_record_t ap_info; wifi_ap_record_t ap_info;
@ -21,8 +20,8 @@ void ParticipantUDP::Setup(int localPort,
} }
// Create a UDP socket // Create a UDP socket
sockfd = socket(AF_INET, SOCK_DGRAM, 0); this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (sockfd < 0) { if (this->sockfd < 0) {
std::cout << "Unable to create UDP socket: errno " << errno << "\n"; std::cout << "Unable to create UDP socket: errno " << errno << "\n";
vTaskDelete(NULL); vTaskDelete(NULL);
return; return;
@ -38,7 +37,7 @@ void ParticipantUDP::Setup(int localPort,
// Bind the socket to the address and port // 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"; std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
close(sockfd); close(sockfd);
vTaskDelete(NULL); vTaskDelete(NULL);
@ -61,59 +60,66 @@ void ParticipantUDP::Setup(int localPort,
void ParticipantUDP::GetBroadcastAddress() { void ParticipantUDP::GetBroadcastAddress() {
// SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION... // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION...
// esp_netif_ip_info_t ip_info; esp_netif_ip_info_t ip_info;
// esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
// // Get IP information (IP address, netmask, gateway) // Get IP information (IP address, netmask, gateway)
// if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) { if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
// std::cout << "Failed to get IP info\n"; std::cout << "Failed to get IP info\n";
// return; return;
// } }
// ip_addr_t broadcast_addr = {}; ip_addr_t broadcast_addr = {};
// broadcast_addr.u_addr.ip4.addr = broadcast_addr.u_addr.ip4.addr =
// (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr; (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr;
// snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR, snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR,
// IP2STR(&broadcast_addr.u_addr.ip4)); IP2STR(&broadcast_addr.u_addr.ip4));
// std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n"; std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n";
} }
void ParticipantUDP::Receive() { void ParticipantUDP::Receive() {
#if defined(IDF_VER) #if defined(IDF_VER)
struct pollfd fds; struct pollfd fds[1];
fds.fd = sockfd; fds[0].fd = sockfd;
fds.events = POLLIN; // We're looking for data available to read fds[0].events = POLLIN; // We're looking for data available to read
// Use poll() with a timeout of 0 to return immediately // 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) { if (ret == -1) {
std::cout << "poll() error\n"; std::cout << "poll() error\n";
return; 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]; 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, 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) { if (packetSize < 0) {
std::cout << "recvfrom() error\n"; std::cout << "recvfrom() error\n";
return; return;
} else if (packetSize == 0) {
break;
} }
std::cout << "receiving " << packetSize << " bytes\n"; // std::cout << "receiving " << packetSize << " bytes, msgId " << (int)this->buffer[0] << "\n";
// inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); inet_ntoa_r(source_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
// unsigned int sender_port = ntohs(this->src_addr.sin_port); 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) { if (ret == -1) {
std::cout << "poll() error\n"; std::cout << "poll() error\n";
return; return;
} }
} }
//std::cout << "no more messages\n";
#endif // IDF_VER #endif // IDF_VER
} }
@ -146,12 +152,6 @@ bool ParticipantUDP::Publish(IMessage* msg) {
sizeof(dest_addr)); sizeof(dest_addr));
if (err != 0) if (err != 0)
std::cout << "Publish error\n"; 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 #endif
return true; return true;
}; };

9
EspIdf/EspIdfParticipant.h
View File

@ -2,9 +2,7 @@
#include "Participants/ParticipantUDP.h" #include "Participants/ParticipantUDP.h"
#if defined(IDF_VER)
#include "lwip/sockets.h" #include "lwip/sockets.h"
#endif
namespace RoboidControl { namespace RoboidControl {
namespace EspIdf { namespace EspIdf {
@ -17,13 +15,12 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
bool Publish(IMessage* msg); bool Publish(IMessage* msg);
protected: protected:
#if defined(IDF_VER) char broadcastIpAddress[INET_ADDRSTRLEN]; // IPv4 address can have a max of 15 characters
char broadcastIpAddress[INET_ADDRSTRLEN]; // + null terminator
int sockfd; int sockfd;
struct sockaddr_in dest_addr; struct sockaddr_in dest_addr;
struct sockaddr_in src_addr; //struct sockaddr_in src_addr;
#endif
void GetBroadcastAddress(); void GetBroadcastAddress();
}; };

123
LinearAlgebra/Matrix.cpp
View File

@ -1,6 +1,8 @@
#include "Matrix.h" #include "Matrix.h"
#include <iostream> #include <iostream>
namespace LinearAlgebra {
#pragma region Matrix1 #pragma region Matrix1
Matrix1::Matrix1(int size) : size(size) { Matrix1::Matrix1(int size) : size(size) {
@ -12,8 +14,7 @@ Matrix1::Matrix1(int size) : size(size) {
} }
} }
Matrix1::Matrix1(float* data, int size) Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
: data(data), size(size) {
this->externalData = true; this->externalData = true;
} }
@ -28,12 +29,7 @@ Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
} }
Quaternion LinearAlgebra::Matrix1::ToQuaternion() { Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
return Quaternion( return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
this->data[0],
this->data[1],
this->data[2],
this->data[3]
);
} }
// Matrix1 // Matrix1
@ -46,9 +42,9 @@ Matrix2::Matrix2() {}
Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
this->nValues = nRows * nCols; this->nValues = nRows * nCols;
if (this->nValues == 0) if (this->nValues == 0)
data = nullptr; this->data = nullptr;
else { else {
this->data = new float[nValues](); this->data = new float[this->nValues];
this->externalData = false; this->externalData = false;
} }
} }
@ -59,11 +55,44 @@ Matrix2::Matrix2(float* data, int nRows, int nCols)
this->externalData = true; 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() { Matrix2::~Matrix2() {
if (data != nullptr && !this->externalData) if (!this->externalData)
delete[] data; delete[] data;
} }
Matrix2 Matrix2::Clone() const {
Matrix2 r = Matrix2(this->nRows, this->nCols);
std::copy(this->data, this->data + this->nValues, r.data);
return r;
}
// Move constructor // Move constructor
Matrix2::Matrix2(Matrix2&& other) noexcept Matrix2::Matrix2(Matrix2&& other) noexcept
: nRows(other.nRows), : nRows(other.nRows),
@ -94,6 +123,11 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) {
return r; return r;
} }
void Matrix2::Clear() {
for (int ix = 0; ix < this->nValues; ix++)
this->data[ix] = 0;
}
Matrix2 Matrix2::Identity(int size) { Matrix2 Matrix2::Identity(int size) {
return Diagonal(1, size); return Diagonal(1, size);
} }
@ -121,6 +155,17 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
return r; 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 LinearAlgebra::Matrix2::operator-() const {
Matrix2 r = Matrix2(this->nRows, this->nCols); Matrix2 r = Matrix2(this->nRows, this->nCols);
for (int ix = 0; ix < r.nValues; ix++) for (int ix = 0; ix < r.nValues; ix++)
@ -135,6 +180,12 @@ Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
return r; 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 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
Matrix2 r = Matrix2(this->nRows, B.nCols); Matrix2 r = Matrix2(this->nRows, B.nCols);
@ -149,30 +200,54 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
int BColOffset = i * BCols; // BColOffset is constant for each row of B int BColOffset = i * BCols; // BColOffset is constant for each row of B
for (int j = 0; j < BCols; ++j) { for (int j = 0; j < BCols; ++j) {
float sum = 0; float sum = 0;
std::cout << " 0"; // std::cout << " 0";
int BIndex = j; int BIndex = j;
for (int k = 0; k < ACols; ++k) { for (int k = 0; k < ACols; ++k) {
std::cout << " + " << this->data[ARowOffset + k] << " * " // std::cout << " + " << this->data[ARowOffset + k] << " * "
<< B.data[BIndex]; // << B.data[BIndex];
sum += this->data[ARowOffset + k] * B.data[BIndex]; sum += this->data[ARowOffset + k] * B.data[BIndex];
BIndex += BCols; BIndex += BCols;
} }
r.data[BColOffset + j] = sum; r.data[BColOffset + j] = sum;
std::cout << " = " << sum << " ix: " << BColOffset + j << "\n"; // std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
} }
} }
return r; return r;
} }
void LinearAlgebra::Matrix2::SetSlice(int rowStart, Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
int rowStop, Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
int colStart,
int colStop, int resultRowIx = 0;
const Matrix2& m) const { int resultColIx = 0;
for (int i = rowStart; i < rowStop; i++) { for (int i = rowStart; i < rowStop; i++) {
for (int j = colStart; j < colStop; j++) for (int j = colStart; j < colStop; j++)
this->data[i * this->nCols + j] = r.data[resultRowIx * r.nCols + resultColIx] =
m.data[(i - rowStart) * m.nCols + (j - colStart)]; this->data[i * this->nCols + j];
}
return r;
}
void Matrix2::UpdateSlice(int rowStart,
int rowStop,
int colStart,
int colStop,
const Matrix2& m) const {
// for (int i = rowStart; i < rowStop; i++) {
// for (int j = colStart; j < colStop; j++)
// this->data[i * this->nCols + j] =
// m.data[(i - rowStart) * m.nCols + (j - colStart)];
// }
int rRowDataIx = rowStart * this->nCols;
int mRowDataIx = 0;
for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
rRowDataIx = rowIx * this->nCols;
// rRowDataIx += this->nCols;
mRowDataIx += m.nCols;
for (int colIx = colStart; colIx < colStop; colIx++) {
this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
}
} }
} }
@ -181,7 +256,7 @@ void LinearAlgebra::Matrix2::SetSlice(int rowStart,
/// @return 4x4 Omega matrix /// @return 4x4 Omega matrix
Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) { Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
Matrix2 r = Matrix2::Zero(4, 4); 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 // set last row to -v
int ix = 3 * 4; int ix = 3 * 4;
@ -200,6 +275,8 @@ Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
// Matrix2 // Matrix2
#pragma endregion #pragma endregion
} // namespace LinearAlgebra
template <> template <>
MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) { MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
if (rows <= 0 || cols <= 0) { if (rows <= 0 || cols <= 0) {

33
LinearAlgebra/Matrix.h
View File

@ -33,10 +33,15 @@ class Matrix2 {
Matrix2(); Matrix2();
Matrix2(int nRows, int nCols); Matrix2(int nRows, int nCols);
Matrix2(float* data, int nRows, int nCols); Matrix2(float* data, int nRows, int nCols);
Matrix2(const Matrix2& m);
Matrix2& operator=(const Matrix2& other);
~Matrix2(); ~Matrix2();
Matrix2 Clone() const;
static Matrix2 Zero(int nRows, int nCols); static Matrix2 Zero(int nRows, int nCols);
void Clear();
static Matrix2 Identity(int size); static Matrix2 Identity(int size);
@ -44,12 +49,15 @@ class Matrix2 {
static Matrix2 SkewMatrix(const Vector3& v); static Matrix2 SkewMatrix(const Vector3& v);
Matrix2 Transpose() const;
Matrix2 operator-() const; Matrix2 operator-() const;
/// @brief Add a matrix to this matrix /// @brief Add a matrix to this matrix
/// @param m The matrix to add to this matrix /// @param m The matrix to add to this matrix
/// @return The result of the addition /// @return The result of the addition
Matrix2 operator+(const Matrix2& v) const; Matrix2 operator+(const Matrix2& v) const;
Matrix2 operator+=(const Matrix2& v);
Matrix2 operator*(const Matrix2& m) const; Matrix2 operator*(const Matrix2& m) const;
friend Matrix2 operator*(const Matrix2& m, float f) { friend Matrix2 operator*(const Matrix2& m, float f) {
@ -74,14 +82,23 @@ class Matrix2 {
} }
return r; 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 rowStop,
int colStart, int colStart,
int colStop, int colStop,
@ -208,6 +225,6 @@ class MatrixOf {
}; };
} // namespace LinearAlgebra } // namespace LinearAlgebra
//using namespace LinearAlgebra; // using namespace LinearAlgebra;
#endif #endif

42
Messages/BinaryMsg.cpp
View File

@ -2,33 +2,41 @@
namespace RoboidControl { 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) { BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
this->networkId = networkId; this->networkId = networkId;
this->thingId = thing->id; this->thingId = thing->id;
this->thing = thing; 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 BinaryMsg::Serialize(char* buffer) {
unsigned char ix = this->length; // unsigned char ix = this->length;
this->thing->GenerateBinary(buffer, &ix); // this->dataLength = this->thing->GenerateBinary(buffer, &ix);
if (ix <= this->length) // in this case, no data is actually sent if (this->dataLength <= 0) // in this case, no data is actually sent
return 0; return 0;
buffer[0] = this->id; unsigned char ix = 0;
buffer[1] = this->networkId; buffer[ix++] = this->id;
buffer[2] = this->thingId; buffer[ix++] = this->networkId;
return ix; buffer[ix++] = this->thingId;
buffer[ix++] = this->dataLength;
return this->length + this->dataLength;
} }
} // namespace RoboidControl } // namespace RoboidControl

5
Messages/BinaryMsg.h
View File

@ -10,7 +10,7 @@ class BinaryMsg : public IMessage {
/// @brief The message ID /// @brief The message ID
static const unsigned char id = 0xB1; static const unsigned char id = 0xB1;
/// @brief The length of the message without the binary data itslef /// @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 /// @brief The network ID of the thing
unsigned char networkId; unsigned char networkId;
@ -19,8 +19,9 @@ class BinaryMsg : public IMessage {
/// @brief The thing for which the binary data is communicated /// @brief The thing for which the binary data is communicated
Thing* thing; Thing* thing;
unsigned char dataLength;
/// @brief The binary data which is communicated /// @brief The binary data which is communicated
char* bytes = nullptr; char* data = nullptr;
/// @brief Create a new message for sending /// @brief Create a new message for sending
/// @param networkId The network ID of the thing /// @param networkId The network ID of the thing

17
Messages/Messages.cpp
View File

@ -1,7 +1,6 @@
#include "Messages.h" #include "Messages.h"
#include "LowLevelMessages.h" #include "LowLevelMessages.h"
//#include "Participant.h"
#include "string.h" #include "string.h"
namespace RoboidControl { namespace RoboidControl {
@ -10,26 +9,10 @@ namespace RoboidControl {
IMessage::IMessage() {} IMessage::IMessage() {}
// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
// IMessage::IMessage(char* buffer) {}
unsigned char IMessage::Serialize(char* buffer) { unsigned char IMessage::Serialize(char* buffer) {
return 0; 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 // IMessage
#pragma endregion #pragma endregion

2
Messages/ModelUrlMsg.h
View File

@ -8,7 +8,7 @@ class ModelUrlMsg : public IMessage {
/// @brief The message ID /// @brief The message ID
static const unsigned char id = 0x90; static const unsigned char id = 0x90;
/// @brief The length of the message without the URL string itself /// @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 /// @brief The network ID of the thing
unsigned char networkId; unsigned char networkId;

1
Messages/NameMsg.cpp
View File

@ -25,6 +25,7 @@ NameMsg::NameMsg(const char* buffer) {
this->networkId = buffer[ix++]; this->networkId = buffer[ix++];
this->thingId = buffer[ix++]; this->thingId = buffer[ix++];
this->nameLength = buffer[ix++]; this->nameLength = buffer[ix++];
// the name string in the buffer is not \0 terminated! // the name string in the buffer is not \0 terminated!
char* name = new char[this->nameLength + 1]; char* name = new char[this->nameLength + 1];
for (int i = 0; i < this->nameLength; i++) for (int i = 0; i < this->nameLength; i++)

2
Messages/PoseMsg.cpp
View File

@ -11,12 +11,10 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
if (thing->positionUpdated || force) { if (thing->positionUpdated || force) {
this->position = thing->GetPosition(); this->position = thing->GetPosition();
this->poseType |= Pose_Position; this->poseType |= Pose_Position;
thing->positionUpdated = false;
} }
if (thing->orientationUpdated || force) { if (thing->orientationUpdated || force) {
this->orientation = thing->GetOrientation(); this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation; this->poseType |= Pose_Orientation;
thing->orientationUpdated = false;
} }
if (thing->linearVelocityUpdated) { if (thing->linearVelocityUpdated) {
this->linearVelocity = thing->GetLinearVelocity(); this->linearVelocity = thing->GetLinearVelocity();

34
Participant.cpp
View File

@ -27,17 +27,20 @@ Participant::~Participant() {
delete[] this->ipAddress; delete[] this->ipAddress;
} }
void Participant::Update(unsigned long currentTimeMs) {} void Participant::Update(unsigned long currentTimeMs) {
for (Thing* thing : this->things) {
Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { if (thing != nullptr)
thing->Update(currentTimeMs, true);
}
}
Thing* Participant::Get(unsigned char thingId) {
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
// if (thing->networkId == networkId && thing->id == thingId)
if (thing->id == thingId) if (thing->id == thingId)
return thing; return thing;
} }
// std::cout << "Could not find thing " << this->ipAddress << ":" << std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
// this->port << "[" << (int)thingId << "]\n";
// << "[" << (int)networkId << "/" << (int)thingId << "]\n";
return nullptr; return nullptr;
} }
@ -51,25 +54,22 @@ void Participant::Add(Thing* thing, bool checkId) {
thing->id = (unsigned char)this->things.size() + 1; thing->id = (unsigned char)this->things.size() + 1;
this->things.push_back(thing); this->things.push_back(thing);
#endif #endif
// std::cout << "Add thing with generated ID " << this->ipAddress << ":" << // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
// this->port << "[" << (int)thing->networkId << "/" // << this->port << "[" << (int)thing->id << "]\n";
// << (int)thing->id << "]\n";
} else { } else {
Thing* foundThing = Get(thing->networkId, thing->id); Thing* foundThing = Get(thing->id);
if (foundThing == nullptr) { if (foundThing == nullptr) {
#if defined(NO_STD) #if defined(NO_STD)
this->things[this->thingCount++] = thing; this->things[this->thingCount++] = thing;
#else #else
this->things.push_back(thing); this->things.push_back(thing);
#endif #endif
// std::cout << "Add thing " << this->ipAddress << ":" << this->port << // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "["
// "[" << (int)thing->networkId << "/"
// << (int)thing->id << "]\n"; // << (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";
} }
} }

2
Participant.h
View File

@ -50,7 +50,7 @@ class Participant {
/// @param thingId The ID of the thing /// @param thingId The ID of the thing
/// @return The thing if found or nullptr when no thing has been found /// @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. /// @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. /// @brief Add a new thing for this participant.
/// @param thing The thing to add /// @param thing The thing to add
/// @param checkId Checks the thing ID of the thing. If it is 0, a new thing /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing

137
Participants/ParticipantUDP.cpp
View File

@ -28,13 +28,12 @@ namespace RoboidControl {
ParticipantUDP::ParticipantUDP(int port) { ParticipantUDP::ParticipantUDP(int port) {
this->ipAddress = "0.0.0.0"; this->ipAddress = "0.0.0.0";
this->port = port; this->port = port;
this->remoteSite = nullptr;
if (this->port == 0) if (this->port == 0)
this->isIsolated = true; this->isIsolated = true;
} }
ParticipantUDP::ParticipantUDP(const char* ipAddress, ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
int port,
int localPort)
: Participant("127.0.0.1", localPort) { : Participant("127.0.0.1", localPort) {
if (this->port == 0) if (this->port == 0)
this->isIsolated = true; this->isIsolated = true;
@ -58,15 +57,14 @@ void ParticipantUDP::begin() {
} }
void ParticipantUDP::SetupUDP(int localPort, void ParticipantUDP::SetupUDP(int localPort,
const char* remoteIpAddress, const char* remoteIpAddress,
int remotePort) { int remotePort) {
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows = Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this); static_cast<Windows::ParticipantUDP*>(this);
thisWindows->Setup(localPort, remoteIpAddress, remotePort); thisWindows->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
static_cast<Posix::ParticipantUDP*>(this);
thisPosix->Setup(localPort, remoteIpAddress, remotePort); thisPosix->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(ARDUINO) #elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino = Arduino::ParticipantUDP* thisArduino =
@ -90,6 +88,7 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
ParticipantMsg* msg = new ParticipantMsg(this->networkId); ParticipantMsg* msg = new ParticipantMsg(this->networkId);
std::cout << "Send Participant\n";
if (this->remoteSite == nullptr) if (this->remoteSite == nullptr)
this->Publish(msg); this->Publish(msg);
else else
@ -98,19 +97,22 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
this->nextPublishMe = currentTimeMs + this->publishInterval; this->nextPublishMe = currentTimeMs + this->publishInterval;
} }
this->ReceiveUDP(); this->ReceiveUDP();
} }
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
if (thing != nullptr) { if (thing == nullptr)
thing->Update(currentTimeMs); continue;
if (this->isIsolated == false) {
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); if (this->isIsolated == false) {
for (Participant* sender : this->senders) PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
this->Send(sender, poseMsg); this->Send(thing->owner, poseMsg);
delete poseMsg; BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
} this->Send(thing->owner, binaryMsg);
delete poseMsg;
} }
thing->Update(currentTimeMs, true);
} }
} }
@ -120,8 +122,7 @@ void ParticipantUDP::ReceiveUDP() {
static_cast<Windows::ParticipantUDP*>(this); static_cast<Windows::ParticipantUDP*>(this);
thisWindows->Receive(); thisWindows->Receive();
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
static_cast<Posix::ParticipantUDP*>(this);
thisPosix->Receive(); thisPosix->Receive();
#elif defined(ARDUINO) #elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino = Arduino::ParticipantUDP* thisArduino =
@ -158,8 +159,8 @@ Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
#pragma region Send #pragma region Send
void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
Thing* thing) { 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); ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
this->Send(remoteParticipant, thingMsg); this->Send(remoteParticipant, thingMsg);
delete thingMsg; delete thingMsg;
@ -187,8 +188,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
static_cast<Windows::ParticipantUDP*>(this); static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Send(remoteParticipant, bufferSize); return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Send(remoteParticipant, bufferSize); return thisPosix->Send(remoteParticipant, bufferSize);
#elif defined(ARDUINO) #elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino = Arduino::ParticipantUDP* thisArduino =
@ -230,8 +230,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
static_cast<Windows::ParticipantUDP*>(this); static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Publish(msg); return thisWindows->Publish(msg);
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Publish(msg); return thisPosix->Publish(msg);
#elif defined(ARDUINO) #elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino = Arduino::ParticipantUDP* thisArduino =
@ -252,66 +251,83 @@ bool ParticipantUDP::Publish(IMessage* msg) {
#pragma region Receive #pragma region Receive
void ParticipantUDP::ReceiveData(unsigned char packetSize, void ParticipantUDP::ReceiveData(unsigned char packetSize,
char* senderIpAddress, char* senderIpAddress,
unsigned int senderPort) { unsigned int senderPort) {
Participant* remoteParticipant = Participant* sender =
this->GetParticipant(senderIpAddress, senderPort); this->GetParticipant(senderIpAddress, senderPort);
if (remoteParticipant == nullptr) { if (sender == nullptr) {
remoteParticipant = this->AddParticipant(senderIpAddress, senderPort); sender = this->AddParticipant(senderIpAddress, senderPort);
// std::cout << "New sender " << senderIpAddress << ":" << senderPort std::cout << "New remote participant " << sender->ipAddress
// << "\n"; << ":" << sender->port << std::endl;
std::cout << "New remote participant " << remoteParticipant->ipAddress
<< ":" << remoteParticipant->port << " "
<< (int)remoteParticipant->networkId << "\n";
} }
ReceiveData(packetSize, remoteParticipant); ReceiveData(packetSize, sender);
} }
void ParticipantUDP::ReceiveData(unsigned char bufferSize, void ParticipantUDP::ReceiveData(unsigned char bufferSize,
Participant* remoteParticipant) { Participant* sender) {
unsigned char msgId = this->buffer[0]; 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) { switch (msgId) {
case ParticipantMsg::id: { case ParticipantMsg::id: {
ParticipantMsg* msg = new ParticipantMsg(this->buffer); ParticipantMsg* msg = new ParticipantMsg(this->buffer);
Process(remoteParticipant, msg); bufferSize -= msg->length;
Process(sender, msg);
delete msg; delete msg;
} break; } break;
case SiteMsg::id: { case SiteMsg::id: {
SiteMsg* msg = new SiteMsg(this->buffer); SiteMsg* msg = new SiteMsg(this->buffer);
Process(remoteParticipant, msg); bufferSize -= msg->length;
Process(sender, msg);
delete msg; delete msg;
} break; } break;
case InvestigateMsg::id: { case InvestigateMsg::id: {
InvestigateMsg* msg = new InvestigateMsg(this->buffer); InvestigateMsg* msg = new InvestigateMsg(this->buffer);
Process(remoteParticipant, msg); Process(sender, msg);
delete msg; delete msg;
} break; } break;
case ThingMsg::id: { case ThingMsg::id: {
ThingMsg* msg = new ThingMsg(this->buffer); ThingMsg* msg = new ThingMsg(this->buffer);
Process(remoteParticipant, msg); bufferSize -= msg->length;
Process(sender, msg);
delete msg; delete msg;
} break; } break;
case NameMsg::id: { case NameMsg::id: {
NameMsg* msg = new NameMsg(this->buffer); 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; delete msg;
} break; } break;
case PoseMsg::id: { case PoseMsg::id: {
PoseMsg* msg = new PoseMsg(this->buffer); PoseMsg* msg = new PoseMsg(this->buffer);
Process(remoteParticipant, msg); bufferSize -= msg->length;
Process(sender, msg);
delete msg; delete msg;
} break; } break;
case BinaryMsg::id: { case BinaryMsg::id: {
BinaryMsg* msg = new BinaryMsg(this->buffer); BinaryMsg* msg = new BinaryMsg(this->buffer);
Process(remoteParticipant, msg); bufferSize -= msg->length + msg->dataLength;
Process(sender, msg);
delete msg; delete msg;
} break; } 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) {} void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
std::cout << this->name << ": Process Participant " << (int)msg->networkId
<< "\n";
}
void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
std::cout << this->name << ": process Site Id " << (int)this->networkId std::cout << this->name << ": process Site Id " << (int)this->networkId
@ -333,9 +349,9 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
std::cout << this->name << ": process Name [" << (int)msg->networkId << "/" 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) { if (thing != nullptr) {
int nameLength = msg->nameLength; int nameLength = msg->nameLength;
int stringLen = nameLength + 1; int stringLen = nameLength + 1;
@ -347,15 +363,19 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
// Use strncpy with bounds checking for other platforms (Arduino, POSIX, // Use strncpy with bounds checking for other platforms (Arduino, POSIX,
// ESP-IDF) // ESP-IDF)
strncpy(thingName, msg->name, strncpy(thingName, msg->name,
stringLen - 1); // Leave space for null terminator nameLength); // Leave space for null terminator
thingName[stringLen - 1] = '\0'; // Ensure null termination
#endif #endif
thingName[nameLength] = '\0'; thingName[nameLength] = '\0';
thing->name = thingName; thing->name = thingName;
// std::cout << "thing name = " << thing->name << " length = " <<
// nameLength std::cout << thing->name;
// << "\n";
} }
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) { void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
@ -365,18 +385,15 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/" std::cout << this->name << ": process Binary [" << (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) if (thing != nullptr)
thing->ProcessBinary(msg->bytes); thing->ProcessBinary(msg->data);
else { else {
thing = this->Get(msg->networkId, msg->thingId); std::cout << " unknown thing [" << (int)msg->networkId << "/"
if (thing != nullptr) << (int)msg->thingId << "]";
thing->ProcessBinary(msg->bytes);
// else
// std::cout << "custom msg for unknown thing [" << (int)msg->networkId
// << "/" << (int)msg->thingId << "]\n";
} }
std::cout << std::endl;
} }
// Receive // Receive

1
Participants/ParticipantUDP.h
View File

@ -136,6 +136,7 @@ class ParticipantUDP : public Participant {
virtual void Process(Participant* sender, InvestigateMsg* msg); virtual void Process(Participant* sender, InvestigateMsg* msg);
virtual void Process(Participant* sender, ThingMsg* msg); virtual void Process(Participant* sender, ThingMsg* msg);
virtual void Process(Participant* sender, NameMsg* 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, PoseMsg* msg);
virtual void Process(Participant* sender, BinaryMsg* msg); virtual void Process(Participant* sender, BinaryMsg* msg);

33
Participants/SiteServer.cpp
View File

@ -25,7 +25,7 @@ SiteServer::SiteServer(int port) {
SetupUDP(port, ipAddress, 0); SetupUDP(port, ipAddress, 0);
#if !defined(NO_STD) #if !defined(NO_STD)
//Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor); // Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
#endif #endif
} }
@ -43,22 +43,23 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
void SiteServer::Process(Participant* sender, SiteMsg* msg) {} void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
void SiteServer::Process(Participant* sender, ThingMsg* 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 (thing == nullptr) {
// #if defined(NO_STD) // #if defined(NO_STD)
new Thing(sender, msg->networkId, msg->thingId, new Thing(sender, (Thing::Type)msg->thingType,
(Thing::Type)msg->thingType); msg->thingId);
// #else // #else
// auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType); // auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
// //Thing* newThing; // //Thing* newThing;
// if (thingMsgProcessor != thingMsgProcessors.end()) // found item // if (thingMsgProcessor != thingMsgProcessors.end()) // found item
// //newThing = // //newThing =
// thingMsgProcessor->second(sender, msg->networkId, msg->thingId); // thingMsgProcessor->second(sender, msg->networkId,
// else // msg->thingId);
// //newThing = // else
// new Thing(sender, msg->networkId, msg->thingId, // //newThing =
// (Thing::Type)msg->thingType); // new Thing(sender, msg->networkId, msg->thingId,
// #endif // (Thing::Type)msg->thingType);
// #endif
} }
} }

57
Thing.cpp
View File

@ -1,5 +1,6 @@
#include "Thing.h" #include "Thing.h"
#include "Messages/PoseMsg.h"
#include "Participant.h" #include "Participant.h"
#include "Participants/IsolatedParticipant.h" #include "Participants/IsolatedParticipant.h"
@ -22,14 +23,15 @@ namespace RoboidControl {
// return isolatedParticipant; // 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::Thing(Participant* owner, Type thingType, unsigned char thingId)
: Thing(owner, (unsigned char)thingType) {} : 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->owner = owner;
this->id = 0; this->id = thingId;
this->type = thingType; this->type = thingType;
this->networkId = 0; this->networkId = 0;
@ -39,27 +41,27 @@ Thing::Thing(Participant* owner, int thingType) {
this->linearVelocity = Spherical::zero; this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero; this->angularVelocity = Spherical::zero;
// std::cout << "add thing to participant\n"; // std::cout << "add thing [" << (int)this->id << "] to owner "
owner->Add(this); // << this->owner->ipAddress << ":" << this->owner->port << std::endl;
this->owner->Add(this, false);
} }
Thing::Thing(Participant* owner, // Thing::Thing(Participant* owner,
unsigned char networkId, // Type thingType,
unsigned char thingId, // int thingId) {
Type thingType) { // // no participant reference yet..
// no participant reference yet.. // this->owner = owner;
this->owner = owner; // this->networkId = networkId;
this->networkId = networkId; // this->id = thingId;
this->id = thingId; // this->type = (unsigned char)thingType;
this->type = (unsigned char)thingType;
this->linearVelocity = Spherical::zero; // this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero; // this->angularVelocity = Spherical::zero;
// std::cout << "Created thing " << (int)this->networkId << "/" << // // std::cout << "Created thing " << (int)this->networkId << "/" <<
// (int)this->id // // (int)this->id
// << "\n"; // // << "\n";
owner->Add(this, false); // owner->Add(this, false);
} // }
void Thing::Terminate() { void Thing::Terminate() {
// Thing::Remove(this); // Thing::Remove(this);
@ -190,7 +192,11 @@ void Thing::Update(bool recursive) {
} }
void Thing::Update(unsigned long currentTimeMs, 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) { if (recursive) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx]; Thing* child = this->children[childIx];
@ -205,9 +211,10 @@ void Thing::UpdateThings(unsigned long currentTimeMs) {
IsolatedParticipant::Isolated()->Update(currentTimeMs); IsolatedParticipant::Isolated()->Update(currentTimeMs);
} }
void Thing::GenerateBinary(char* buffer, unsigned char* ix) { int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
(void)buffer; (void)buffer;
(void)ix; (void)ix;
return 0;
} }
void Thing::ProcessBinary(char* bytes) { void Thing::ProcessBinary(char* bytes) {
(void)bytes; (void)bytes;

17
Thing.h
View File

@ -43,19 +43,21 @@ class Thing {
Thing(int thingType = Type::Undetermined); Thing(int thingType = Type::Undetermined);
/// @brief Create a new thing of the given type /// @brief Create a new thing of the given type
/// @param thingType The predefined type of thing /// @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 /// @brief Create a new thing of the give type
/// @param thingType The custom type of the thing /// @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 /// @brief Create a new thing for the given participant
/// @param participant The participant for which this thing is created /// @param participant The participant for which this thing is created
/// @param networkId The network ID of the thing /// @param networkId The network ID of the thing
/// @param thingId The ID of the thing /// @param thingId The ID of the thing
/// @param thingType The type of thing /// @param thingType The type of thing
Thing(Participant* participant, // Thing(Participant* participant,
unsigned char networkId, // unsigned char networkId,
unsigned char thingId, // unsigned char thingId,
Type thingType = Type::Undetermined); // Type thingType = Type::Undetermined);
/// @brief The participant managing this thing /// @brief The participant managing this thing
Participant* owner; Participant* owner;
@ -192,7 +194,8 @@ class Thing {
/// @brief Function used to generate binary data for this thing /// @brief Function used to generate binary data for this thing
/// @param buffer The byte array for thw binary data /// @param buffer The byte array for thw binary data
/// @param ix The starting position for writing the 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 // /// @brief FUnction used to process binary data received for this thing
/// @param bytes The binary data /// @param bytes The binary data
virtual void ProcessBinary(char* bytes); virtual void ProcessBinary(char* bytes);

7
Things/TemperatureSensor.cpp
View File

@ -5,17 +5,18 @@
namespace RoboidControl { namespace RoboidControl {
TemperatureSensor::TemperatureSensor(Participant* participant, TemperatureSensor::TemperatureSensor(Participant* participant,
unsigned char networkId,
unsigned char thingId) unsigned char thingId)
: Thing(participant, networkId, thingId, Type::TemperatureSensor) {} : Thing(participant, Type::TemperatureSensor, thingId) {}
void TemperatureSensor::SetTemperature(float temp) { void TemperatureSensor::SetTemperature(float temp) {
this->temperature = 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"; // std::cout << "Send temperature: " << this->temperature << "\n";
LowLevelMessages::SendFloat16(buffer, ix, this->temperature); LowLevelMessages::SendFloat16(buffer, ix, this->temperature);
return *ix - startIx;
} }
void TemperatureSensor::ProcessBinary(char* bytes) { void TemperatureSensor::ProcessBinary(char* bytes) {

4
Things/TemperatureSensor.h
View File

@ -15,7 +15,7 @@ class TemperatureSensor : public Thing {
/// @brief Create a temperature sensor with the given ID /// @brief Create a temperature sensor with the given ID
/// @param networkId The network ID of the sensor /// @param networkId The network ID of the sensor
/// @param thingId The ID of the thing /// @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 /// @brief Manually override the measured temperature
/// @param temperature The new temperature /// @param temperature The new temperature
@ -24,7 +24,7 @@ class TemperatureSensor : public Thing {
/// @brief Function to create a binary message with the temperature /// @brief Function to create a binary message with the temperature
/// @param buffer The byte array for thw binary data /// @param buffer The byte array for thw binary data
/// @param ix The starting position for writing the 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 /// @brief Function to extract the temperature received in the binary message
/// @param bytes The binary data /// @param bytes The binary data
virtual void ProcessBinary(char* bytes) override; virtual void ProcessBinary(char* bytes) override;

4
Things/TouchSensor.cpp
View File

@ -10,7 +10,9 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
this->SetParent(parent); 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) { void TouchSensor::ProcessBinary(char* bytes) {
// if (bytes[0] == 1) // if (bytes[0] == 1)

2
Things/TouchSensor.h
View File

@ -25,7 +25,7 @@ class TouchSensor : public Thing {
/// @brief Function to create a binary message with the temperature /// @brief Function to create a binary message with the temperature
/// @param buffer The byte array for thw binary data /// @param buffer The byte array for thw binary data
/// @param ix The starting position for writing the 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 /// @brief Function to extract the temperature received in the binary message
/// @param bytes The binary data /// @param bytes The binary data
virtual void ProcessBinary(char* bytes) override; virtual void ProcessBinary(char* bytes) override;