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

Improved network sync

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-22 17:47:06 +02:00
parent 619695c90d
commit e56f25f9a1
13 changed files with 123 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Arduino/Things/UltrasonicSensor.cpp
View File

@ -51,10 +51,10 @@ float UltrasonicSensor::GetDistance() {
// if (distance > 30)
// distance = 0;
this->touchedSomething |= (this->distance <= this->touchDistance);
this->touchedSomething |= (this->distance > 0 && this->distance <= this->touchDistance);
// std::cout << "Ultrasonic " << this->distance << " " <<
// this->touchedSomething << "\n";
// std::cout << "Ultrasonic " << this->touchedSomething << " | " << (this->distance > 0) << " " <<
// (this->distance <= this->touchDistance) << "\n";
return distance;
}

2
Arduino/Things/UltrasonicSensor.h
View File

@ -9,7 +9,7 @@ namespace Arduino {
class UltrasonicSensor : public TouchSensor {
public:
// Inherit all constructors
using TouchSensor::TouchSensor;
//using TouchSensor::TouchSensor;
/// @brief Setup an ultrasonic sensor
/// @param participant The participant to use

13
Messages/DestroyMsg.cpp
View File

@ -7,15 +7,18 @@ DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
this->thingId = thing->id;
}
DestroyMsg::DestroyMsg(char* buffer) {}
DestroyMsg::DestroyMsg(char* buffer) {
this->networkId = buffer[1];
this->thingId = buffer[2];
}
DestroyMsg::~DestroyMsg() {}
unsigned char DestroyMsg::Serialize(char* buffer) {
#if defined(DEBUG)
std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId
<< "] " << std::endl;
#endif
//#if defined(DEBUG)
std::cout << "Send DestroyMsg [" << (int)this->networkId << "/"
<< (int)this->thingId << "] " << std::endl;
//#endif
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;

6
Messages/PoseMsg.cpp
View File

@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
this->thingId = thing->id;
this->poseType = 0;
if (thing->positionUpdated || force) {
if (thing->positionUpdated || (force && thing->GetLinearVelocity().distance == 0)) {
this->position = thing->GetPosition();
this->poseType |= Pose_Position;
}
if (thing->orientationUpdated || force) {
if (thing->orientationUpdated || (force && thing->GetAngularVelocity().distance == 0)) {
this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation;
}
@ -45,7 +45,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
if (this->poseType == 0)
return 0;
#if defined(DEBUG)
#if defined(DEBUG) && DEBUG > 1
std::cout << "Send PoseMsg [" << (int)this->networkId << "/"
<< (int)this->thingId << "] " << (int)this->poseType << std::endl;
#endif

8
Participant.cpp
View File

@ -69,7 +69,7 @@ Participant* Participant::AddParticipant(const char* ipAddress, unsigned int por
void Participant::AddParticipant(Participant* participant) {
Participant* foundParticipant =
Participant::GetParticipant(participant->networkId);
Participant::GetParticipant(participant->ipAddress, participant->port);
if (foundParticipant == nullptr) {
#if defined(NO_STD)
this->things[this->thingCount++] = thing;
@ -80,7 +80,7 @@ void Participant::AddParticipant(Participant* participant) {
<< participant->port << "[" << (int)participant->networkId
<< "]\n";
} else {
std::cout << "Did not add, existing thing " << participant->ipAddress << ":"
std::cout << "Did not add, existing participant " << participant->ipAddress << ":"
<< participant->port << "[" << (int)participant->networkId
<< "]\n";
}
@ -143,8 +143,8 @@ void Participant::Remove(Thing* thing) {
this->thingCount = lastThingIx;
#else
this->things.remove_if([thing](Thing* obj) { return obj == thing; });
std::cout << "Removing " << thing->networkId << "/" << thing->id
<< " list size = " << this->things.size() << "\n";
std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id
<< "] list size = " << this->things.size() << "\n";
#endif
}

2
Participant.h
View File

@ -35,7 +35,7 @@ class Participant {
virtual void Update(unsigned long currentTimeMs = 0);
protected:
public:
#if defined(NO_STD)
unsigned char thingCount = 0;
Thing* things[MAX_THING_COUNT];

65
Participants/ParticipantUDP.cpp
View File

@ -5,6 +5,7 @@
#include "Arduino/ArduinoParticipant.h"
#include "EspIdf/EspIdfParticipant.h"
#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
#include <ws2tcpip.h>
@ -102,18 +103,53 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
this->ReceiveUDP();
}
UpdateMyThings(currentTimeMs);
UpdateOtherThings(currentTimeMs);
}
void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) {
for (Thing* thing : this->things) {
if (thing == nullptr)
if (thing == nullptr || thing->GetParent() != nullptr)
continue;
thing->Update(currentTimeMs, true);
if (this->isIsolated == false && thing->owner != this) {
if (this->isIsolated || this->networkId == 0)
continue;
if (thing->isTerminated) {
DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
this->Send(this->remoteSite, destroyMsg);
delete destroyMsg;
this->Remove(thing);
} else {
// Send to remote site
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
this->Send(thing->owner, poseMsg);
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
this->Send(thing->owner, binaryMsg);
this->Send(this->remoteSite, poseMsg);
delete poseMsg;
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
this->Send(this->remoteSite, binaryMsg);
delete binaryMsg;
}
}
}
void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) {
for (Participant* participant : Participant::participants) {
if (participant == nullptr || participant == this)
continue;
participant->Update(currentTimeMs);
if (this->isIsolated)
continue;
for (Thing* thing : participant->things) {
PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
this->Send(participant, poseMsg);
delete poseMsg;
BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
this->Send(participant, binaryMsg);
delete binaryMsg;
}
}
}
@ -238,8 +274,8 @@ bool ParticipantUDP::Publish(IMessage* msg) {
void ParticipantUDP::ReceiveData(unsigned char packetSize,
char* senderIpAddress,
unsigned int senderPort) {
std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
<< std::endl;
// std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
// << std::endl;
Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
if (sender == nullptr) {
sender = this->AddParticipant(senderIpAddress, senderPort);
@ -394,6 +430,17 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
<< "/" << (int)msg->networkId << "]\n";
#endif
Participant* owner = Participant::GetParticipant(msg->networkId);
if (owner == nullptr)
return;
Thing* thing = owner->Get(msg->thingId);
if (thing == nullptr)
return;
if ((msg->poseType & PoseMsg::Pose_Position) != 0)
thing->SetPosition(msg->position);
std::cout << "update position for" << (int)thing->id << std::endl;
}
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
@ -409,10 +456,14 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
thing->ProcessBinary(msg->data);
#if !defined(NO_STD)
else {
#if defined(DEBUG)
std::cout << " unknown thing [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "]";
#endif
}
#if defined(DEBUG)
std::cout << std::endl;
#endif
#endif
}
}

3
Participants/ParticipantUDP.h
View File

@ -2,6 +2,7 @@
#include "Messages/BinaryMsg.h"
#include "Messages/InvestigateMsg.h"
#include "Messages/DestroyMsg.h"
#include "Messages/ModelUrlMsg.h"
#include "Messages/NameMsg.h"
#include "Messages/ParticipantMsg.h"
@ -100,6 +101,8 @@ class ParticipantUDP : public Participant {
bool connected = false;
virtual void Update(unsigned long currentTimeMs = 0) override;
virtual void UpdateMyThings(unsigned long currentTimeMs);
virtual void UpdateOtherThings(unsigned long currentTimeMs);
void SendThingInfo(Participant* remoteParticipant, Thing* thing);
void PublishThingInfo(Thing* thing);

35
Participants/SiteServer.cpp
View File

@ -16,17 +16,31 @@ SiteServer::SiteServer(int port) {
this->ipAddress = "0.0.0.0";
this->port = port;
// #if defined(NO_STD)
// this->senders[this->senderCount++] = this;
// #else
// this->senders.push_back(this);
// #endif
SetupUDP(port, ipAddress, 0);
}
#if !defined(NO_STD)
// Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
#endif
void SiteServer::UpdateMyThings(unsigned long currentTimeMs) {
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
thing->Update(currentTimeMs, true);
if (this->isIsolated == false) {
// Send to all other participants
for (Participant* participant : Participant::participants) {
if (participant == nullptr || participant == this)
continue;
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
this->Send(participant, poseMsg);
delete poseMsg;
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
this->Send(participant, binaryMsg);
delete binaryMsg;
}
}
}
}
void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
@ -46,8 +60,7 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->thingId);
if (thing == nullptr) {
// #if defined(NO_STD)
new Thing(sender, (Thing::Type)msg->thingType,
msg->thingId);
new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
// #else
// auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
// //Thing* newThing;

20
Participants/SiteServer.h
View File

@ -15,18 +15,7 @@ class SiteServer : public ParticipantUDP {
public:
SiteServer(int port = 7681);
// virtual void Update(unsigned long currentTimeMs = 0) override;
// #if !defined(NO_STD)
// template <typename ThingClass>
// void Register(unsigned char thingType) {
// thingMsgProcessors[thingType] = [](Participant* participant,
// unsigned char networkId,
// unsigned char thingId) {
// return new ThingClass(participant, networkId, thingId);
// };
// };
// #endif
virtual void UpdateMyThings(unsigned long currentTimeMs) override;
protected:
unsigned long nextPublishMe = 0;
@ -34,13 +23,6 @@ class SiteServer : public ParticipantUDP {
virtual void Process(Participant* sender, ParticipantMsg* msg) override;
virtual void Process(Participant* sender, SiteMsg* msg) override;
virtual void Process(Participant* sender, ThingMsg* msg) override;
// #if !defined(NO_STD)
// using ThingConstructor = std::function<Thing*(Participant* participant,
// unsigned char networkId,
// unsigned char thingId)>;
// std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
// #endif
};
} // namespace RoboidControl

13
Thing.cpp
View File

@ -17,12 +17,6 @@
namespace RoboidControl {
// LocalParticipant* Thing::CheckHiddenParticipant() {
// if (isolatedParticipant == nullptr)
// isolatedParticipant = new LocalParticipant(0);
// return isolatedParticipant;
// }
Thing::Thing(int thingType)
: Thing(IsolatedParticipant::Isolated(), thingType) {}
@ -36,7 +30,9 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
this->networkId = 0;
this->position = Spherical::zero;
this->positionUpdated = true;
this->orientation = SwingTwist::identity;
this->orientationUpdated = true;
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
@ -70,6 +66,7 @@ Thing::Thing(Thing* parent, int thingType, unsigned char thingId)
// }
void Thing::Terminate() {
this->isTerminated = true;
// Thing::Remove(this);
}
@ -244,18 +241,22 @@ SwingTwist Thing::GetOrientation() {
}
void Thing::SetLinearVelocity(Spherical linearVelocity) {
if (this->linearVelocity.distance != linearVelocity.distance) {
this->linearVelocity = linearVelocity;
this->linearVelocityUpdated = true;
}
}
Spherical Thing::GetLinearVelocity() {
return this->linearVelocity;
}
void Thing::SetAngularVelocity(Spherical angularVelocity) {
if (this->angularVelocity.distance != angularVelocity.distance) {
this->angularVelocity = angularVelocity;
this->angularVelocityUpdated = true;
}
}
Spherical Thing::GetAngularVelocity() {
return this->angularVelocity;

1
Thing.h
View File

@ -174,6 +174,7 @@ class Thing {
public:
/// @brief Terminated things are no longer updated
void Terminate();
bool isTerminated = false;
/// @brief Sets the location from where the 3D model of this Thing can be
/// loaded from

4
Things/TouchSensor.cpp
View File

@ -15,8 +15,8 @@ int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
}
void TouchSensor::ProcessBinary(char* bytes) {
// if (bytes[0] == 1)
// std::cout << this->name << " is Touching something!\n";
if (bytes[0] == 1)
std::cout << this->name << " is Touching something!\n";
this->touchedSomething |= (bytes[0] == 1);
}