From a292179f87071860f62f3a6f3e0d489238664c44 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 23 Apr 2025 12:47:40 +0200 Subject: [PATCH] Steps toward hierarchy changes --- Arduino/Things/DRV8833.cpp | 4 +- Arduino/Things/UltrasonicSensor.cpp | 2 +- Messages/PoseMsg.cpp | 4 +- Participant.cpp | 16 ++++++-- Participants/ParticipantUDP.cpp | 63 +++++++++++++++++------------ Thing.cpp | 26 ++++-------- Thing.h | 5 ++- Things/DifferentialDrive.cpp | 37 ++++++++--------- 8 files changed, 83 insertions(+), 74 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 8e03f3c..17579d8 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -14,7 +14,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, unsigned char pinIn2, bool reverse) : Thing(driver->owner) { - this->parent = driver; + this->SetParent(driver); this->pinIn1 = pinIn1; this->pinIn2 = pinIn2; @@ -144,7 +144,7 @@ DRV8833::DRV8833(Thing* parent, pinStandby, reverseA, reverseB) { - this->parent = parent; + this->SetParent(parent); } } // namespace Arduino diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 0d1275c..646a4f8 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -20,7 +20,7 @@ UltrasonicSensor::UltrasonicSensor(Thing* parent, unsigned char pinTrigger, unsigned char pinEcho) : UltrasonicSensor(parent->owner, pinTrigger, pinEcho) { - this->parent = parent; + this->SetParent(parent); } float UltrasonicSensor::GetDistance() { diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index 3edd823..c476388 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { this->thingId = thing->id; this->poseType = 0; - if (thing->positionUpdated || (force && thing->GetLinearVelocity().distance == 0)) { + if (thing->positionUpdated || (force && thing->GetParent() != nullptr)) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; } - if (thing->orientationUpdated || (force && thing->GetAngularVelocity().distance == 0)) { + if (thing->orientationUpdated || (force && thing->GetParent() != nullptr)) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; } diff --git a/Participant.cpp b/Participant.cpp index f1a7847..bd38901 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -45,8 +45,8 @@ Participant* Participant::GetParticipant(const char* ipAddress, participant->port == port) return participant; } - std::cout << "Could not find participant " << ipAddress << ":" << (int)port - << std::endl; + // std::cout << "Could not find participant " << ipAddress << ":" << (int)port + // << std::endl; return nullptr; } @@ -57,7 +57,7 @@ Participant* Participant::GetParticipant(unsigned char participantId) { if (participant->networkId == participantId) return participant; } - std::cout << "Could not find participant " << (int)participantId << std::endl; + // std::cout << "Could not find participant " << (int)participantId << std::endl; return nullptr; } @@ -104,7 +104,15 @@ void Participant::Add(Thing* thing, bool checkId) { thing->id = this->thingCount + 1; this->things[this->thingCount++] = thing; #else - thing->id = (unsigned char)this->things.size() + 1; + // find highest id + int highestIx = 0; + for (Thing* thing : this->things) { + if (thing == nullptr) + continue; + if (thing->id > highestIx) + highestIx = thing->id; + } + thing->id = highestIx + 1; this->things.push_back(thing); #endif // std::cout << "Add thing with generated ID " << this->ipAddress << ":" diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 6394dc5..780701a 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -5,7 +5,6 @@ #include "Arduino/ArduinoParticipant.h" #include "EspIdf/EspIdfParticipant.h" - #if defined(_WIN32) || defined(_WIN64) #include #include @@ -109,28 +108,41 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { void ParticipantUDP::UpdateMyThings(unsigned long currentTimeMs = 0) { for (Thing* thing : this->things) { - if (thing == nullptr || thing->GetParent() != nullptr) + if (thing == nullptr) // || thing->GetParent() != nullptr) continue; - thing->Update(currentTimeMs, true); - - 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(this->remoteSite, poseMsg); - delete poseMsg; - BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); - this->Send(this->remoteSite, binaryMsg); - delete binaryMsg; + if (thing->hierarchyChanged) { + std::cout << "thing hierarchy changed " << (int)thing->id << std::endl; + if (!(this->isIsolated || this->networkId == 0)) { + ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); + this->Send(this->remoteSite, thingMsg); + delete thingMsg; + } } + + // Why don't we do recursive? + // Because when a thing creates a thing in the update, + // that new thing is not sent out (because of hierarchyChanged) + // before it is updated itself: it is immediatedly updated! + thing->Update(currentTimeMs, false); + + if (!(this->isIsolated || this->networkId == 0)) { + if (thing->isTerminated) { + DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); + this->Send(this->remoteSite, destroyMsg); + delete destroyMsg; + } else { + // Send to remote site + PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); + this->Send(this->remoteSite, poseMsg); + delete poseMsg; + BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); + this->Send(this->remoteSite, binaryMsg); + delete binaryMsg; + } + } + if (thing->isTerminated) + this->Remove(thing); } } @@ -139,7 +151,7 @@ void ParticipantUDP::UpdateOtherThings(unsigned long currentTimeMs = 0) { if (participant == nullptr || participant == this) continue; - participant->Update(currentTimeMs); + //participant->Update(currentTimeMs); if (this->isIsolated) continue; @@ -363,12 +375,13 @@ void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { << " -> " << (int)msg->networkId << "\n"; #endif - if (this->networkId != msg->networkId) + if (this->networkId != msg->networkId) { this->networkId = msg->networkId; - // std::cout << this->things.size() << " things\n"; - for (Thing* thing : this->things) - this->SendThingInfo(sender, thing); + // std::cout << this->things.size() << " things\n"; + for (Thing* thing : this->things) + this->SendThingInfo(sender, thing); + } } void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) { diff --git a/Thing.cpp b/Thing.cpp index 4764435..ca3a212 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -33,6 +33,7 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { this->positionUpdated = true; this->orientation = SwingTwist::identity; this->orientationUpdated = true; + this->hierarchyChanged = true; this->linearVelocity = Spherical::zero; this->angularVelocity = Spherical::zero; @@ -45,29 +46,11 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { Thing::Thing(Thing* parent, int thingType, unsigned char thingId) : Thing(parent->owner, thingType, thingId) { - this->parent = parent; + this->SetParent(parent); } -// 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); -// } - void Thing::Terminate() { this->isTerminated = true; - // Thing::Remove(this); } Thing* Thing::FindThing(const char* name) { @@ -94,6 +77,8 @@ void Thing::SetParent(Thing* parent) { this->parent = nullptr; } else parent->AddChild(this); + std::cout << "setting parent for " << (int) this->id << std::endl; + this->hierarchyChanged = true; } void Thing::SetParent(Thing* root, const char* name) { @@ -199,6 +184,9 @@ void Thing::Update(unsigned long currentTimeMs, bool recursive) { // OnPoseChanged callback this->positionUpdated = false; this->orientationUpdated = false; + // this->linearVelocityUpdated = false; + // this->angularVelocityUpdated = false; + this->hierarchyChanged = false; if (recursive) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { diff --git a/Thing.h b/Thing.h index bd748c9..0d45d65 100644 --- a/Thing.h +++ b/Thing.h @@ -107,7 +107,10 @@ class Thing { /// @return The found thing of nullptr when nothing is found Thing* GetChildByIndex(unsigned char ix); - protected: + /// @brief Indicator that the hierarchy of the thing has changed + bool hierarchyChanged = true; + + private: Thing* parent = nullptr; Thing** children = nullptr; diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp index bd09157..ad7ad14 100644 --- a/Things/DifferentialDrive.cpp +++ b/Things/DifferentialDrive.cpp @@ -42,30 +42,27 @@ void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) { } void DifferentialDrive::Update(unsigned long currentMs, bool recursive) { - if (this->linearVelocityUpdated == false) - return; - // this assumes forward velocity only.... - float linearVelocity = this->GetLinearVelocity().distance; + if (this->linearVelocityUpdated) { + // this assumes forward velocity only.... + float linearVelocity = this->GetLinearVelocity().distance; - Spherical angularVelocity = this->GetAngularVelocity(); - float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec - // Determine the rotation direction - if (angularVelocity.direction.horizontal.InDegrees() < 0) - angularSpeed = -angularSpeed; + Spherical angularVelocity = this->GetAngularVelocity(); + float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec + // Determine the rotation direction + if (angularVelocity.direction.horizontal.InDegrees() < 0) + angularSpeed = -angularSpeed; - // wheel separation can be replaced by this->leftwheel->position->distance - float speedLeft = - (linearVelocity + angularSpeed * this->wheelSeparation / 2) / - this->wheelRadius * Rad2Deg; - float speedRight = - (linearVelocity - angularSpeed * this->wheelSeparation / 2) / - this->wheelRadius * Rad2Deg; + // wheel separation can be replaced by this->leftwheel->position->distance + float speedLeft = + (linearVelocity + angularSpeed * this->wheelSeparation / 2) / + this->wheelRadius * Rad2Deg; + float speedRight = + (linearVelocity - angularSpeed * this->wheelSeparation / 2) / + this->wheelRadius * Rad2Deg; - this->SetWheelVelocity(speedLeft, speedRight); + this->SetWheelVelocity(speedLeft, speedRight); + } Thing::Update(currentMs, recursive); - // std::cout << "lin. speed " << linearVelocity << " ang. speed " << - // angularVelocity.distance << " left wheel " - // << speedLeft << " right wheel " << speedRight << "\n"; } } // namespace RoboidControl \ No newline at end of file