From 2883f5e0fa7cfa56ccb6c843d4b2d38412407fed Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 2 Jun 2025 12:30:21 +0200 Subject: [PATCH] thing tytpe enum->const --- Participant.cpp | 2 +- Thing.cpp | 502 ++++++++++++++++++++++++++---------------------- Thing.h | 36 ++-- 3 files changed, 293 insertions(+), 247 deletions(-) diff --git a/Participant.cpp b/Participant.cpp index 3f11839..549f78a 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -16,7 +16,7 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) { } Participant::Participant() { - std::cout << "P\n"; + //std::cout << "P\n"; //this->root.name = "Isolated"; this->root = new Thing(this); this->root->name = "Root"; diff --git a/Thing.cpp b/Thing.cpp index 1ec24b3..5fc4c72 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -16,300 +16,344 @@ #include #endif -namespace RoboidControl { +namespace RoboidControl +{ #pragma region Init -Thing* Thing::LocalRoot() { - Participant* p = Participant::LocalParticipant; - Thing* localRoot = p->root; - return localRoot; -} + Thing *Thing::LocalRoot() + { + Participant *p = Participant::LocalParticipant; + Thing *localRoot = p->root; + return localRoot; + } -// Only use this for root things -Thing::Thing(Participant* owner) { - this->type = Type::Roboid; // should become root + // Only use this for root things + Thing::Thing(Participant *owner) + { + this->type = Type::Root; // should become root - this->position = Spherical::zero; - this->positionUpdated = true; - this->orientation = SwingTwist::identity; - this->orientationUpdated = true; - this->hierarchyChanged = true; + this->position = Spherical::zero; + this->positionUpdated = true; + this->orientation = SwingTwist::identity; + this->orientationUpdated = true; + this->hierarchyChanged = true; - this->linearVelocity = Spherical::zero; - this->angularVelocity = Spherical::zero; + this->linearVelocity = Spherical::zero; + this->angularVelocity = Spherical::zero; - this->owner = owner; - //this->owner->Add(this, true); - std::cout << this->owner->name << ": New root thing " << std::endl; -} + this->owner = owner; + // this->owner->Add(this, true); + //std::cout << this->owner->name << ": New root thing " << std::endl; + } -Thing::Thing(unsigned char thingType, Thing* parent) { - this->type = thingType; + Thing::Thing(unsigned char thingType, Thing *parent) + { + this->type = thingType; - this->position = Spherical::zero; - this->positionUpdated = true; - this->orientation = SwingTwist::identity; - this->orientationUpdated = true; - this->hierarchyChanged = true; + this->position = Spherical::zero; + this->positionUpdated = true; + this->orientation = SwingTwist::identity; + this->orientationUpdated = true; + this->hierarchyChanged = true; - this->linearVelocity = Spherical::zero; - this->angularVelocity = Spherical::zero; + this->linearVelocity = Spherical::zero; + this->angularVelocity = Spherical::zero; - this->owner = parent->owner; - this->owner->Add(this, true); - this->SetParent(parent); + this->owner = parent->owner; + this->owner->Add(this, true); + this->SetParent(parent); - std::cout << this->owner->name << ": New thing for " << parent->name - << std::endl; -} + std::cout << this->owner->name << ": New thing for " << parent->name + << std::endl; + } -Thing::~Thing() { - std::cout << "Destroy thing " << this->name << std::endl; -} + Thing::~Thing() + { + std::cout << "Destroy thing " << this->name << std::endl; + } -// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, -// unsigned char thingId) { -// Thing thing = Thing(owner, thingType); -// thing.id = thingId; -// return thing; -// } + Thing Thing::Reconstruct(Participant *owner, unsigned char thingType, unsigned char thingId) + { + Thing thing = Thing(thingType, owner->root); + thing.id = thingId; + return thing; + } #pragma endregion Init -void Thing::SetName(const char* name) { - this->name = name; - this->nameChanged = true; -} + void Thing::SetName(const char *name) + { + this->name = name; + this->nameChanged = true; + } -const char* Thing::GetName() const { - return this->name; -} + const char *Thing::GetName() const + { + return this->name; + } -void Thing::SetModel(const char* url) { - this->modelUrl = url; -} + void Thing::SetModel(const char *url) + { + this->modelUrl = url; + } #pragma region Hierarchy -void Thing::SetParent(Thing* parent) { - if (parent == nullptr) { - Thing* parentThing = this->parent; - if (parentThing != nullptr) - parentThing->RemoveChild(this); - this->parent = nullptr; - } else - parent->AddChild(this); - this->hierarchyChanged = true; -} - -// void Thing::SetParent(Thing* parent) { -// parent->AddChild(this); -// this->hierarchyChanged = true; -// } - -// const Thing& Thing::GetParent() { -// return *this->parent; -// } - -bool Thing::IsRoot() const { - return this == LocalRoot() || this->parent == nullptr; //&Thing::Root; -} - -// void Thing::SetParent(Thing* root, const char* name) { -// Thing* thing = root->FindChild(name); -// if (thing != nullptr) -// this->SetParent(thing); -// } - -Thing* Thing::GetParent() { - return this->parent; -} - -Thing* Thing::GetChildByIndex(unsigned char ix) { - return this->children[ix]; -} - -void Thing::AddChild(Thing* child) { - unsigned char newChildCount = this->childCount + 1; - Thing** newChildren = new Thing*[newChildCount]; - - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - newChildren[childIx] = this->children[childIx]; - if (this->children[childIx] == child) { - // child is already present, stop copying do not update the children - delete[] newChildren; - return; + void Thing::SetParent(Thing *parent) + { + if (parent == nullptr) + { + Thing *parentThing = this->parent; + if (parentThing != nullptr) + parentThing->RemoveChild(this); + this->parent = nullptr; } + else + parent->AddChild(this); + this->hierarchyChanged = true; } - newChildren[this->childCount] = child; - child->parent = this; + // void Thing::SetParent(Thing* parent) { + // parent->AddChild(this); + // this->hierarchyChanged = true; + // } - if (this->children != nullptr) - delete[] this->children; + // const Thing& Thing::GetParent() { + // return *this->parent; + // } - this->children = newChildren; - this->childCount = newChildCount; -} + bool Thing::IsRoot() const + { + return this == LocalRoot() || this->parent == nullptr; //&Thing::Root; + } -Thing* Thing::RemoveChild(Thing* child) { - unsigned char newChildCount = this->childCount - 1; - Thing** newChildren = new Thing*[newChildCount]; + // void Thing::SetParent(Thing* root, const char* name) { + // Thing* thing = root->FindChild(name); + // if (thing != nullptr) + // this->SetParent(thing); + // } - unsigned char newChildIx = 0; - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - if (this->children[childIx] != child) { - if (newChildIx == newChildCount) { // We did not find the child - // stop copying and return nothing + Thing *Thing::GetParent() + { + return this->parent; + } + + Thing *Thing::GetChildByIndex(unsigned char ix) + { + return this->children[ix]; + } + + void Thing::AddChild(Thing *child) + { + unsigned char newChildCount = this->childCount + 1; + Thing **newChildren = new Thing *[newChildCount]; + + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + newChildren[childIx] = this->children[childIx]; + if (this->children[childIx] == child) + { + // child is already present, stop copying do not update the children delete[] newChildren; - return nullptr; - } else - newChildren[newChildIx++] = this->children[childIx]; + return; + } } + + newChildren[this->childCount] = child; + child->parent = this; + + if (this->children != nullptr) + delete[] this->children; + + this->children = newChildren; + this->childCount = newChildCount; } - child->parent = Thing::LocalRoot(); + Thing *Thing::RemoveChild(Thing *child) + { + unsigned char newChildCount = this->childCount - 1; + Thing **newChildren = new Thing *[newChildCount]; - delete[] this->children; - this->children = newChildren; - this->childCount = newChildCount; + unsigned char newChildIx = 0; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + if (this->children[childIx] != child) + { + if (newChildIx == newChildCount) + { // We did not find the child + // stop copying and return nothing + delete[] newChildren; + return nullptr; + } + else + newChildren[newChildIx++] = this->children[childIx]; + } + } - return child; -} + child->parent = Thing::LocalRoot(); -Thing* Thing::GetChild(unsigned char id, bool recurse) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->children[childIx]; - if (child == nullptr) - continue; - if (child->id == id) - return child; + delete[] this->children; + this->children = newChildren; + this->childCount = newChildCount; - if (recurse) { - Thing* foundChild = child->GetChild(id, recurse); + return child; + } + + Thing *Thing::GetChild(unsigned char id, bool recurse) + { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + Thing *child = this->children[childIx]; + if (child == nullptr) + continue; + if (child->id == id) + return child; + + if (recurse) + { + Thing *foundChild = child->GetChild(id, recurse); + if (foundChild != nullptr) + return foundChild; + } + } + return nullptr; + } + + Thing *Thing::FindChild(const char *name, bool recurse) + { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + Thing *child = this->children[childIx]; + if (child == nullptr || child->name == nullptr) + continue; + + if (strcmp(child->name, name) == 0) + return child; + + Thing *foundChild = child->FindChild(name); if (foundChild != nullptr) return foundChild; } + return nullptr; } - return nullptr; -} - -Thing* Thing::FindChild(const char* name, bool recurse) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->children[childIx]; - if (child == nullptr || child->name == nullptr) - continue; - - if (strcmp(child->name, name) == 0) - return child; - - Thing* foundChild = child->FindChild(name); - if (foundChild != nullptr) - return foundChild; - } - return nullptr; -} #pragma endregion Hierarchy #pragma region Pose -void Thing::SetPosition(Spherical position) { - this->position = position; - this->positionUpdated = true; -} -Spherical Thing::GetPosition() { - return this->position; -} - -void Thing::SetOrientation(SwingTwist orientation) { - this->orientation = orientation; - this->orientationUpdated = true; -} - -SwingTwist Thing::GetOrientation() { - return this->orientation; -} - -void Thing::SetLinearVelocity(Spherical linearVelocity) { - if (this->linearVelocity.distance != linearVelocity.distance) { - this->linearVelocity = linearVelocity; - this->linearVelocityUpdated = true; + void Thing::SetPosition(Spherical position) + { + this->position = position; + this->positionUpdated = 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::GetPosition() + { + return this->position; } -} -Spherical Thing::GetAngularVelocity() { - return this->angularVelocity; -} + void Thing::SetOrientation(SwingTwist orientation) + { + this->orientation = orientation; + this->orientationUpdated = true; + } + + SwingTwist Thing::GetOrientation() + { + return this->orientation; + } + + 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; + } #pragma endregion Pose #pragma region Update -unsigned long Thing::GetTimeMs() { + unsigned long Thing::GetTimeMs() + { #if defined(ARDUINO) - unsigned long ms = millis(); - return ms; + unsigned long ms = millis(); + return ms; #else - auto now = std::chrono::steady_clock::now(); - auto ms = std::chrono::duration_cast( - now.time_since_epoch()); - return static_cast(ms.count()); + auto now = std::chrono::steady_clock::now(); + auto ms = std::chrono::duration_cast( + now.time_since_epoch()); + return static_cast(ms.count()); #endif -} + } -// void Thing::Update(bool recursive) { -// Update(GetTimeMs(), recursive); -// } + // void Thing::Update(bool recursive) { + // Update(GetTimeMs(), recursive); + // } -void Thing::PrepareForUpdate() {} + void Thing::PrepareForUpdate() {} -void Thing::Update(bool recursive) { - // if (this->positionUpdated || this->orientationUpdated) - // OnPoseChanged callback - this->positionUpdated = false; - this->orientationUpdated = false; - // this->linearVelocityUpdated = false; - // this->angularVelocityUpdated = false; - this->hierarchyChanged = false; - this->nameChanged = false; + void Thing::Update(bool recursive) + { + // if (this->positionUpdated || this->orientationUpdated) + // OnPoseChanged callback + this->positionUpdated = false; + this->orientationUpdated = false; + // this->linearVelocityUpdated = false; + // this->angularVelocityUpdated = false; + this->hierarchyChanged = false; + this->nameChanged = false; - if (recursive) { - // std::cout << "# children: " << (int)this->childCount << std::endl; - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing* child = this->children[childIx]; - if (child == nullptr) - continue; - child->Update(recursive); + if (recursive) + { + // std::cout << "# children: " << (int)this->childCount << std::endl; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + { + Thing *child = this->children[childIx]; + if (child == nullptr) + continue; + child->Update(recursive); + } } } -} -void Thing::UpdateThings() { - IsolatedParticipant::Isolated()->Update(); -} + void Thing::UpdateThings() + { + IsolatedParticipant::Isolated()->Update(); + } #pragma endregion Update -int Thing::GenerateBinary(char* buffer, unsigned char* ix) { - (void)buffer; - (void)ix; - return 0; -} -void Thing::ProcessBinary(char* bytes) { - (void)bytes; -}; + int Thing::GenerateBinary(char *buffer, unsigned char *ix) + { + (void)buffer; + (void)ix; + return 0; + } + void Thing::ProcessBinary(char *bytes) + { + (void)bytes; + }; -} // namespace RoboidControl \ No newline at end of file +} // namespace RoboidControl \ No newline at end of file diff --git a/Thing.h b/Thing.h index 439606c..5e8a5a8 100644 --- a/Thing.h +++ b/Thing.h @@ -20,24 +20,26 @@ class ParticipantUDP; class Thing { public: /// @brief Predefined thing types - enum Type : unsigned char { - Undetermined, - // Sensor, - Switch, - DistanceSensor, - DirectionalSensor, - TemperatureSensor, - TouchSensor, - // Motor, - ControlledMotor, - UncontrolledMotor, - Servo, - IncrementalEncoder, + struct Type { + static const unsigned char Undetermined = 0x00; + // Sensor + static const unsigned char Switch = 0x01; + static const unsigned char DistanceSensor = 0x02; + static const unsigned char DirectionalSensor = 0x03; + static const unsigned char TemperatureSensor = 0x04; + static const unsigned char TouchSensor = 0x05; + // Motor + static const unsigned char ControlledMotor = 0x06; + static const unsigned char UncontrolledMotor = 0x07; + static const unsigned char Servo = 0x08; + static const unsigned char IncrementalEncoder = 0x19; // Other - Roboid, - Humanoid, - ExternalSensor, - DifferentialDrive + static const unsigned char Root = 0x10; + static const unsigned char Roboid = 0x09; + static const unsigned char Humanoid = 0x0A; + static const unsigned char ExternalSensor = 0x08; + static const unsigned char Animator = 0x0C; + static const unsigned char DifferentialDrive = 0x0D; }; #pragma region Init