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

Communication improvements, adding gyro msg

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-10 17:35:06 +02:00
parent 28e42df771
commit 92be21a9c1
17 changed files with 161 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EspIdf/EspIdfParticipant.cpp
View File

@ -126,8 +126,8 @@ void ParticipantUDP::Receive() {
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
#if defined(IDF_VER)
// std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
// << remoteParticipant->port << "\n";
std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
<< remoteParticipant->port << "\n";
int err = sendto(this->sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
sizeof(dest_addr));

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++)

29
Participant.cpp
View File

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

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

83
Participants/ParticipantUDP.cpp
View File

@ -252,59 +252,75 @@ bool ParticipantUDP::Publish(IMessage* msg) {
void ParticipantUDP::ReceiveData(unsigned char packetSize,
char* senderIpAddress,
unsigned int senderPort) {
Participant* remoteParticipant =
Participant* sender =
this->GetParticipant(senderIpAddress, senderPort);
if (remoteParticipant == nullptr) {
remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
std::cout << "New remote participant " << remoteParticipant->ipAddress
<< ":" << remoteParticipant->port << " "
<< (int)remoteParticipant->networkId << "\n";
if (sender == nullptr) {
sender = this->AddParticipant(senderIpAddress, senderPort);
std::cout << "New remote participant " << sender->ipAddress
<< ":" << sender->port << std::endl;
}
ReceiveData(packetSize, remoteParticipant);
ReceiveData(packetSize, sender);
}
void ParticipantUDP::ReceiveData(unsigned char bufferSize,
Participant* remoteParticipant) {
Participant* sender) {
unsigned char msgId = this->buffer[0];
std::cout << "receive msg " << (int)msgId << "\n";
// std::cout << " buffer size = " <<(int) bufferSize << "\n";
switch (msgId) {
case ParticipantMsg::id: {
ParticipantMsg* msg = new ParticipantMsg(this->buffer);
Process(remoteParticipant, msg);
bufferSize -= msg->length;
Process(sender, msg);
delete msg;
} break;
case SiteMsg::id: {
SiteMsg* msg = new SiteMsg(this->buffer);
Process(remoteParticipant, msg);
bufferSize -= msg->length;
Process(sender, msg);
delete msg;
} break;
case InvestigateMsg::id: {
InvestigateMsg* msg = new InvestigateMsg(this->buffer);
Process(remoteParticipant, msg);
Process(sender, msg);
delete msg;
} break;
case ThingMsg::id: {
ThingMsg* msg = new ThingMsg(this->buffer);
Process(remoteParticipant, msg);
bufferSize -= msg->length;
Process(sender, msg);
delete msg;
} break;
case NameMsg::id: {
NameMsg* msg = new NameMsg(this->buffer);
Process(remoteParticipant, msg);
bufferSize -= msg->length + msg->nameLength;
Process(sender, msg);
delete msg;
} break;
case ModelUrlMsg::id: {
ModelUrlMsg* msg = new ModelUrlMsg(this->buffer);
bufferSize -= msg->length + msg->urlLength;
Process(sender, msg);
delete msg;
} break;
case PoseMsg::id: {
PoseMsg* msg = new PoseMsg(this->buffer);
Process(remoteParticipant, msg);
bufferSize -= msg->length;
Process(sender, msg);
delete msg;
} break;
case BinaryMsg::id: {
BinaryMsg* msg = new BinaryMsg(this->buffer);
Process(remoteParticipant, msg);
bufferSize -= msg->length + msg->dataLength;
Process(sender, msg);
delete msg;
} break;
};
// Check if the buffer has been read completely
if (bufferSize > 0)
std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
}
void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
@ -313,8 +329,8 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
}
void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
// std::cout << this->name << ": process Site Id " << (int)this->networkId
// << "->" << (int)msg->networkId << "\n";
std::cout << this->name << ": process Site Id " << (int)this->networkId
<< "->" << (int)msg->networkId << "\n";
if (this->networkId != msg->networkId) {
this->networkId = msg->networkId;
// std::cout << this->things.size() << " things\n";
@ -332,9 +348,9 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
std::cout << this->name << ": process Name [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "]\n";
<< (int)msg->thingId << "] ";
Thing* thing = sender->Get(msg->networkId, msg->thingId);
Thing* thing = sender->Get(msg->thingId);
if (thing != nullptr) {
int nameLength = msg->nameLength;
int stringLen = nameLength + 1;
@ -346,15 +362,19 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
// Use strncpy with bounds checking for other platforms (Arduino, POSIX,
// ESP-IDF)
strncpy(thingName, msg->name,
stringLen - 1); // Leave space for null terminator
thingName[stringLen - 1] = '\0'; // Ensure null termination
nameLength); // Leave space for null terminator
#endif
thingName[nameLength] = '\0';
thing->name = thingName;
// std::cout << "thing name = " << thing->name << " length = " <<
// nameLength
// << "\n";
std::cout << thing->name;
}
std::cout << std::endl;
}
void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
std::cout << this->name << ": process model url [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n";
}
void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
@ -364,18 +384,15 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "]\n";
Thing* thing = sender->Get(msg->networkId, msg->thingId);
<< (int)msg->thingId << "] ";
Thing* thing = sender->Get(msg->thingId);
if (thing != nullptr)
thing->ProcessBinary(msg->bytes);
thing->ProcessBinary(msg->data);
else {
thing = this->Get(msg->networkId, msg->thingId);
if (thing != nullptr)
thing->ProcessBinary(msg->bytes);
else
std::cout << "custom msg for unknown thing [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n";
std::cout << " unknown thing [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "]";
}
std::cout << std::endl;
}
// Receive

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
}
}

52
Thing.cpp
View File

@ -1,8 +1,8 @@
#include "Thing.h"
#include "Messages/PoseMsg.h"
#include "Participant.h"
#include "Participants/IsolatedParticipant.h"
#include "Messages/PoseMsg.h"
#include <string.h>
@ -23,14 +23,15 @@ namespace RoboidControl {
// return isolatedParticipant;
// }
Thing::Thing(int thingType) : Thing(IsolatedParticipant::Isolated(), thingType) {}
Thing::Thing(int thingType)
: Thing(IsolatedParticipant::Isolated(), thingType) {}
Thing::Thing(Participant* owner, Type thingType)
: Thing(owner, (unsigned char)thingType) {}
Thing::Thing(Participant* owner, Type thingType, unsigned char thingId)
: Thing(owner, (unsigned char)thingType, thingId) {}
Thing::Thing(Participant* owner, int thingType) {
Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
this->owner = owner;
this->id = 0;
this->id = thingId;
this->type = thingType;
this->networkId = 0;
@ -40,27 +41,27 @@ Thing::Thing(Participant* owner, int thingType) {
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
// std::cout << "add thing to participant\n";
owner->Add(this);
// std::cout << "add thing [" << (int)this->id << "] to owner "
// << this->owner->ipAddress << ":" << this->owner->port << std::endl;
this->owner->Add(this, false);
}
Thing::Thing(Participant* owner,
unsigned char networkId,
unsigned char thingId,
Type thingType) {
// no participant reference yet..
this->owner = owner;
this->networkId = networkId;
this->id = thingId;
this->type = (unsigned char)thingType;
// Thing::Thing(Participant* owner,
// Type thingType,
// int thingId) {
// // no participant reference yet..
// this->owner = owner;
// this->networkId = networkId;
// this->id = thingId;
// this->type = (unsigned char)thingType;
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
// std::cout << "Created thing " << (int)this->networkId << "/" <<
// (int)this->id
// << "\n";
owner->Add(this, false);
}
// this->linearVelocity = Spherical::zero;
// this->angularVelocity = Spherical::zero;
// // std::cout << "Created thing " << (int)this->networkId << "/" <<
// // (int)this->id
// // << "\n";
// owner->Add(this, false);
// }
void Thing::Terminate() {
// Thing::Remove(this);
@ -210,9 +211,10 @@ void Thing::UpdateThings(unsigned long currentTimeMs) {
IsolatedParticipant::Isolated()->Update(currentTimeMs);
}
void Thing::GenerateBinary(char* buffer, unsigned char* ix) {
int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
(void)buffer;
(void)ix;
return 0;
}
void Thing::ProcessBinary(char* bytes) {
(void)bytes;

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;