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

9
EspIdf/EspIdfParticipant.h
View File

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

119
LinearAlgebra/Matrix.cpp
View File

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

33
LinearAlgebra/Matrix.h
View File

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

42
Messages/BinaryMsg.cpp
View File

@ -2,33 +2,41 @@
namespace RoboidControl {
BinaryMsg::BinaryMsg(char* buffer) {
unsigned char ix = 1;
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->bytes = buffer + ix; // This is only valid because the code ensures the the msg
// lifetime is shorter than the buffer lifetime...
}
BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
this->networkId = networkId;
this->thingId = thing->id;
this->thing = thing;
unsigned char ix = BinaryMsg::length;
this->dataLength = this->thing->GenerateBinary(this->data, &ix);
}
BinaryMsg::~BinaryMsg() {}
BinaryMsg::BinaryMsg(char* buffer) {
unsigned char ix = 1;
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->dataLength = buffer[ix++];
char* data = new char[this->dataLength];
for (int i = 0; i < this->dataLength; i++)
data[i] = buffer[ix++];
this->data = data;
}
BinaryMsg::~BinaryMsg() {
delete[] this->data;
}
unsigned char BinaryMsg::Serialize(char* buffer) {
unsigned char ix = this->length;
this->thing->GenerateBinary(buffer, &ix);
if (ix <= this->length) // in this case, no data is actually sent
// unsigned char ix = this->length;
// this->dataLength = this->thing->GenerateBinary(buffer, &ix);
if (this->dataLength <= 0) // in this case, no data is actually sent
return 0;
buffer[0] = this->id;
buffer[1] = this->networkId;
buffer[2] = this->thingId;
return ix;
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
buffer[ix++] = this->dataLength;
return this->length + this->dataLength;
}
} // namespace RoboidControl

5
Messages/BinaryMsg.h
View File

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

17
Messages/Messages.cpp
View File

@ -1,7 +1,6 @@
#include "Messages.h"
#include "LowLevelMessages.h"
//#include "Participant.h"
#include "string.h"
namespace RoboidControl {
@ -10,26 +9,10 @@ namespace RoboidControl {
IMessage::IMessage() {}
// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
// IMessage::IMessage(char* buffer) {}
unsigned char IMessage::Serialize(char* buffer) {
return 0;
}
// bool IMessage::SendMsg(ParticipantUDP *client, IMessage msg) {
// // return SendMsg(client, client.buffer, );nameLength
// return client->SendBuffer(msg.Serialize(client->buffer));
// }
// bool IMessage::Publish(ParticipantUDP *participant) {
// return participant->PublishBuffer(Serialize(participant->buffer));
// }
// bool IMessage::SendTo(ParticipantUDP *participant) {
// return participant->SendBuffer(Serialize(participant->buffer));
// }
// IMessage
#pragma endregion

2
Messages/ModelUrlMsg.h
View File

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

1
Messages/NameMsg.cpp
View File

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

2
Messages/PoseMsg.cpp
View File

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

34
Participant.cpp
View File

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

2
Participant.h
View File

@ -50,7 +50,7 @@ class Participant {
/// @param thingId The ID of the thing
/// @return The thing if found or nullptr when no thing has been found
/// @note The use of the network ID is likely to disappear in future versions.
Thing* Get(unsigned char networkId, unsigned char thingId);
Thing* Get(unsigned char thingId);
/// @brief Add a new thing for this participant.
/// @param thing The thing to add
/// @param checkId Checks the thing ID of the thing. If it is 0, a new thing

121
Participants/ParticipantUDP.cpp
View File

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

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, ThingMsg* msg);
virtual void Process(Participant* sender, NameMsg* msg);
virtual void Process(Participant* sender, ModelUrlMsg* msg);
virtual void Process(Participant* sender, PoseMsg* msg);
virtual void Process(Participant* sender, BinaryMsg* msg);

33
Participants/SiteServer.cpp
View File

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

57
Thing.cpp
View File

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

17
Thing.h
View File

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

7
Things/TemperatureSensor.cpp
View File

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

4
Things/TemperatureSensor.h
View File

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

4
Things/TouchSensor.cpp
View File

@ -10,7 +10,9 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
this->SetParent(parent);
}
void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
return 0;
}
void TouchSensor::ProcessBinary(char* bytes) {
// if (bytes[0] == 1)

2
Things/TouchSensor.h
View File

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