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

Improved new thing processing

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-26 16:33:33 +02:00
parent 0ad9d3e9ec
commit 1add0647e1
14 changed files with 80 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Arduino/ArduinoParticipant.cpp
View File

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

2
Arduino/ArduinoParticipant.h
View File

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

2
EspIdf/EspIdfParticipant.cpp
View File

@ -130,7 +130,7 @@ void ParticipantUDP::Receive() {
#endif // IDF_VER
}
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) {
#if defined(IDF_VER)
// std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
// << remoteParticipant->port << "\n";

2
EspIdf/EspIdfParticipant.h
View File

@ -13,7 +13,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
public:
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive();
bool Send(Participant* remoteParticipant, int bufferSize);
bool SendTo(Participant* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
protected:

18
Participant.cpp
View File

@ -49,10 +49,10 @@ Participant::~Participant() {
delete[] this->ipAddress;
}
void Participant::Update() {
void Participant::Update(bool recurse) {
for (Thing* thing : this->things) {
if (thing != nullptr)
thing->Update(true);
thing->Update();
}
}
@ -67,26 +67,26 @@ bool Participant::Send(IMessage* msg) {
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Send(this, bufferSize);
return thisWindows->SendTo(this, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Send(this, bufferSize);
return thisPosix->SendTo(this, bufferSize);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->Send(this, bufferSize);
return thisArduino->SendTo(this, bufferSize);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->Send(remoteParticipant, bufferSize);
return thisEspIdf->SendTo(this, bufferSize);
#else
return false;
#endif
}
Thing* Participant::Get(unsigned char thingId) {
Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
for (Thing* thing : this->things) {
if (thing->id == thingId)
if (thing->owner->networkId == networkId && thing->id == thingId)
return thing;
}
std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
@ -115,7 +115,7 @@ void Participant::Add(Thing* thing, bool checkId) {
// std::cout << "Add thing with generated ID " << this->ipAddress << ":"
// << this->port << "[" << (int)thing->id << "]\n";
} else {
Thing* foundThing = Get(thing->id);
Thing* foundThing = Get(thing->owner->networkId, thing->id);
if (foundThing == nullptr) {
#if defined(NO_STD)
this->things[this->thingCount++] = thing;

5
Participant.h
View File

@ -112,9 +112,10 @@ class Participant {
std::list<Thing*> things;
#endif
/// @brief Find a thing managed by this participant
/// @param networkId The network ID of the thing
/// @param thingId The ID of the thing
/// @return The thing if found, nullptr when no thing has been found
Thing* Get(unsigned char thingId);
Thing* Get(unsigned char networkId, unsigned char thingId);
/// @brief Add a new thing for this participant.
/// @param thing The thing to add
/// @param checkId If true, the thing.id is regenerated if it is zero
@ -129,7 +130,7 @@ class Participant {
public:
/// @brief Update all things for this participant
virtual void Update();
virtual void Update(bool recurse = true);
#pragma endregion Update

53
Participants/ParticipantUDP.cpp
View File

@ -8,6 +8,10 @@
#include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.h"
#include "Things/DistanceSensor.h"
#include "Things/TouchSensor.h"
#include "Things/DifferentialDrive.h"
#include <string.h>
namespace RoboidControl {
@ -83,7 +87,7 @@ void ParticipantUDP::SetupUDP(int localPort,
// 1. receive external messages
// 2. update the state
// 3. send out the updated messages
void ParticipantUDP::Update() {
void ParticipantUDP::Update(bool recurse) {
unsigned long currentTimeMs = Thing::GetTimeMs();
if (this->isIsolated == false) {
@ -182,7 +186,7 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
bool ParticipantUDP::Send(IMessage* msg) {
if (this->remoteSite != nullptr)
return this->remoteSite->Send(msg);
return true;
}
@ -379,6 +383,43 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
<< "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
<< (int)msg->parentId << "\n";
#endif
Participant* owner = Participant::registry.Get(msg->networkId);
if (owner == nullptr) {
owner = new Participant();
owner->networkId = msg->networkId;
Participant::registry.Add(owner);
}
Thing* thing = owner->Get(msg->networkId, msg->thingId);
if (thing == nullptr) {
bool isRemote = (sender->networkId != owner->networkId);
thing = ProcessNewThing(owner, msg, isRemote);
thing->id = msg->thingId;
thing->type = msg->thingType;
thing->isRemote = isRemote;
}
if (msg->parentId != 0) {
thing->SetParent(owner->Get(msg->networkId, msg->parentId));
if (thing->GetParent() == nullptr)
std::cout << "Could not find parent" << std::endl;
} else
thing->SetParent(nullptr);
}
Thing* ParticipantUDP::ProcessNewThing(Participant* owner,
ThingMsg* msg,
bool isRemote) {
switch (msg->thingType) {
case Thing::Type::DistanceSensor:
return new DistanceSensor(owner->root);
case Thing::Type::TouchSensor:
return new TouchSensor(owner->root);
case Thing::Type::DifferentialDrive:
return new DifferentialDrive(owner->root);
default:
return new Thing(owner->root);
}
}
void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
@ -387,7 +428,7 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
<< (int)msg->thingId << "] ";
#endif
Thing* thing = sender->Get(msg->thingId);
Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing != nullptr) {
int nameLength = msg->nameLength;
int stringLen = nameLength + 1;
@ -429,7 +470,7 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
if (owner == nullptr)
return;
Thing* thing = owner->Get(msg->thingId);
Thing* thing = owner->Get(msg->networkId, msg->thingId);
if (thing == nullptr)
return;
@ -451,7 +492,7 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
Participant* owner = Participant::registry.Get(msg->networkId);
if (owner != nullptr) {
Thing* thing = owner->Get(msg->thingId);
Thing* thing = owner->Get(msg->networkId, msg->thingId);
if (thing != nullptr)
thing->ProcessBinary(msg->data);
#if !defined(NO_STD)
@ -478,7 +519,7 @@ void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) {
<< (int)msg->thingId << "]\n";
#endif
Thing* thing = sender->Get(msg->thingId);
Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing != nullptr)
this->Remove(thing);
}

5
Participants/ParticipantUDP.h
View File

@ -96,7 +96,7 @@ public:
#pragma region Update
public:
virtual void Update() override;
virtual void Update(bool recurse = true) override;
protected:
unsigned long nextPublishMe = 0;
@ -133,7 +133,10 @@ protected:
virtual void Process(Participant* sender, ParticipantMsg* msg);
virtual void Process(Participant* sender, NetworkIdMsg* msg);
virtual void Process(Participant* sender, InvestigateMsg* msg);
virtual void Process(Participant* sender, ThingMsg* msg);
virtual Thing* ProcessNewThing(Participant* sender, ThingMsg* msg, bool isRemote);
virtual void Process(Participant* sender, NameMsg* msg);
virtual void Process(Participant* sender, ModelUrlMsg* msg);
virtual void Process(Participant* sender, PoseMsg* msg);

8
Participants/SiteServer.cpp
View File

@ -70,16 +70,16 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(Participant* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->thingId);
if (thing == nullptr)
Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing == nullptr) {
// new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
// Thing::Reconstruct(sender, msg->thingType, msg->thingId);
//thing = new Thing(msg->thingType, sender->root);
;
}
thing->id = msg->thingId;
if (msg->parentId != 0) {
thing->SetParent(Get(msg->parentId));
thing->SetParent(Get(msg->networkId, msg->parentId));
if (thing->IsRoot())
// if (thing->GetParent() != nullptr)
#if defined(NO_STD)

2
Posix/PosixParticipant.cpp
View File

@ -90,7 +90,7 @@ void ParticipantUDP::Receive() {
#endif
}
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) {
#if defined(__unix__) || defined(__APPLE__)
// std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
// << "\n";

2
Posix/PosixParticipant.h
View File

@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
public:
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive();
bool Send(Participant* remoteParticipant, int bufferSize);
bool SendTo(Participant* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
protected:

5
Thing.h
View File

@ -88,6 +88,11 @@ class Thing {
/// This can be either a Thing::Type of a byte value for custom types
unsigned char type = Type::Undetermined;
/// @brief Is this a remote thing?
/// A remote thing is owned by other participant
/// and is not simulated by the local participant
bool isRemote = false;
/// @brief The participant owning this thing
Participant* owner = nullptr;

2
Windows/WindowsParticipant.cpp
View File

@ -102,7 +102,7 @@ void ParticipantUDP::Receive() {
#endif // _WIN32 || _WIN64
}
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) {
#if defined(_WIN32) || defined(_WIN64)
char ip_str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);

2
Windows/WindowsParticipant.h
View File

@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
public:
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive();
bool Send(Participant* remoteParticipant, int bufferSize);
bool SendTo(Participant* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
protected: