From 75a0cf350dd1b47041f8974918416e34eef0f119 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 29 Apr 2025 11:47:15 +0200 Subject: [PATCH] Aligned Thing implementation --- README.md | 3 +- Thing.cpp | 154 +++++++++++++++++++++++--------------------- Thing.h | 187 +++++++++++++++++++++++++++++++----------------------- 3 files changed, 189 insertions(+), 155 deletions(-) diff --git a/README.md b/README.md index 28e4abd..e0a3c1a 100644 --- a/README.md +++ b/README.md @@ -14,5 +14,4 @@ Supporting: # Basic components - RoboidControl::Thing -- RoboidControl::LocalParticipant -- RoboidControl::SiteServer \ No newline at end of file +- RoboidControl::Participant \ No newline at end of file diff --git a/Thing.cpp b/Thing.cpp index 65579f6..4fe968a 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -17,17 +17,17 @@ namespace RoboidControl { -Thing::Thing(int thingType) +#pragma region Init + +Thing::Thing(unsigned char thingType) : Thing(IsolatedParticipant::Isolated(), thingType) {} -Thing::Thing(Participant* owner, Type thingType, unsigned char thingId) - : Thing(owner, (unsigned char)thingType, thingId) {} - -Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { +Thing::Thing(Participant* owner, + unsigned char thingType, + unsigned char thingId) { this->owner = owner; this->id = thingId; this->type = thingType; - this->networkId = 0; this->position = Spherical::zero; this->positionUpdated = true; @@ -44,11 +44,13 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { this->owner->Add(this, true); } -Thing::Thing(Thing* parent, int thingType, unsigned char thingId) +Thing::Thing(Thing* parent, unsigned char thingType, unsigned char thingId) : Thing(parent->owner, thingType, thingId) { this->SetParent(parent); } +#pragma endregion Init + void Thing::SetName(const char* name) { this->name = name; this->nameChanged = true; @@ -58,22 +60,12 @@ const char* Thing::GetName() const { return this->name; } -Thing* Thing::FindThing(const char* name) { - 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->FindThing(name); - if (foundChild != nullptr) - return foundChild; - } - return nullptr; +void Thing::SetModel(const char* url) { + this->modelUrl = url; } +#pragma region Hierarchy + void Thing::SetParent(Thing* parent) { if (parent == nullptr) { Thing* parentThing = this->parent; @@ -85,16 +77,20 @@ void Thing::SetParent(Thing* parent) { this->hierarchyChanged = true; } -void Thing::SetParent(Thing* root, const char* name) { - Thing* thing = root->FindThing(name); - if (thing != nullptr) - this->SetParent(thing); -} +// 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]; @@ -143,7 +139,7 @@ Thing* Thing::RemoveChild(Thing* child) { return child; } -Thing* Thing::GetChild(unsigned char id, bool recursive) { +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) @@ -151,8 +147,8 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) { if (child->id == id) return child; - if (recursive) { - Thing* foundChild = child->GetChild(id, recursive); + if (recurse) { + Thing* foundChild = child->GetChild(id, recurse); if (foundChild != nullptr) return foundChild; } @@ -160,13 +156,66 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) { return nullptr; } -Thing* Thing::GetChildByIndex(unsigned char ix) { - return this->children[ix]; +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; } -void Thing::SetModel(const char* url) { - this->modelUrl = url; +#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; + } +} + +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 @@ -220,43 +269,4 @@ void Thing::ProcessBinary(char* bytes) { (void)bytes; }; -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; - } -} - -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; -} - } // namespace RoboidControl \ No newline at end of file diff --git a/Thing.h b/Thing.h index 21c557b..2fac262 100644 --- a/Thing.h +++ b/Thing.h @@ -20,7 +20,7 @@ class ParticipantUDP; class Thing { public: /// @brief Predefined thing types - enum Type { + enum Type : unsigned char { Undetermined, // Sensor, Switch, @@ -38,54 +38,86 @@ class Thing { ExternalSensor, }; - /// @brief Create a new thing using an implicit local participant - /// @param thingType The type of 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, +#pragma region Init + + /// @brief Create a new thing without communication abilities + /// @param thingType The type of thing (can use Thing::Type) + Thing(unsigned char thingType = Type::Undetermined); + + /// @brief Create a new thing for a participant + /// @param owner The owning participant + /// @param thingType The type of thing (can use Thing::Type) + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + Thing(Participant* owner, + unsigned char 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, 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(Thing* parent, int thingType = 0, unsigned char thingId = 0); + + /// @brief Create a new child thing + /// @param parent The parent thing + /// @param thingType The type of thing (can use Thing::Type) + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + /// @note The owner will be the same as the owner of the parent thing + Thing(Thing* parent, unsigned char thingType = 0, unsigned char thingId = 0); + +#pragma endregion Init + + public: + /// @brief Terminated things are no longer updated + void Terminate(); + bool terminate = false; + +#pragma region Properties /// @brief The participant managing this thing - Participant* owner; - /// @brief The network ID of this thing - /// @note This field will likely disappear in future versions - unsigned char networkId = 0; + Participant* owner = nullptr; /// @brief The ID of the thing unsigned char id = 0; /// @brief The type of Thing /// This can be either a Thing::Type of a byte value for custom types - unsigned char type = 0; + unsigned char type = Type::Undetermined; - /// @brief Find a thing by name - /// @param name Rhe name of the thing - /// @return The found thing or nullptr when nothing is found - Thing* FindThing(const char* name); + /// @brief The name of the thing + const char* name = nullptr; - /// @brief Sets the parent Thing - /// @param parent The Thing which should become the parnet - /// @remark This is equivalent to calling parent->AddChild(this); + public: + void SetName(const char* name); + const char* GetName() const; + bool nameChanged = false; + + /// @brief Sets the location from where the 3D model of this Thing can be + /// loaded from + /// @param url The url of the model + /// @remark Although the roboid implementation is not dependent on the model, + /// the only official supported model format is .obj + void SetModel(const char* url); + + /// @brief An URL pointing to the location where a model of the thing can be + /// found + const char* modelUrl = nullptr; + /// @brief The scale of the model (deprecated I think) + float modelScale = 1; + +#pragma endregion Properties + +#pragma region Hierarchy + + /// @brief Sets the parent of this Thing + /// @param parent The Thing which should become the parent virtual void SetParent(Thing* parent); - void SetParent(Thing* root, const char* name); - /// @brief Gets the parent Thing + /// @brief Gets the parent of this Thing /// @return The parent Thing Thing* GetParent(); + /// @brief The number of children + unsigned char childCount = 0; + /// @brief Get a child by index + /// @param ix The child index + /// @return The found thing of nullptr when nothing is found + Thing* GetChildByIndex(unsigned char ix); + /// @brief Add a child Thing to this Thing /// @param child The Thing which should become a child /// @remark When the Thing is already a child, it will not be added again @@ -95,17 +127,17 @@ class Thing { /// @return The removed child or nullptr if the child could not be found Thing* RemoveChild(Thing* child); - /// @brief The number of children - unsigned char childCount = 0; /// @brief Get a child by thing Id /// @param id The thing ID to find - /// @param recursive Look recursively through all descendants + /// @param recurse Look recursively through all descendants /// @return The found thing of nullptr when nothing is found - Thing* GetChild(unsigned char id, bool recursive = false); - /// @brief Get a child by index - /// @param ix The child index - /// @return The found thing of nullptr when nothing is found - Thing* GetChildByIndex(unsigned char ix); + Thing* GetChild(unsigned char id, bool recurse = false); + + /// @brief Find a thing by name + /// @param name The name of the thing + /// @param recurse Look recursively through all descendants + /// @return The found thing or nullptr when nothing is found + Thing* FindChild(const char* name, bool recurse = true); /// @brief Indicator that the hierarchy of the thing has changed bool hierarchyChanged = true; @@ -114,38 +146,27 @@ class Thing { Thing* parent = nullptr; Thing** children = nullptr; - /// @brief The name of the thing - const char* name = nullptr; +#pragma endregion Hierarchy + +#pragma region Pose public: - void SetName(const char* name); - const char* GetName() const; - bool nameChanged = false; - - /// @brief An URL pointing to the location where a model of the thing can be - /// found - const char* modelUrl = nullptr; - /// @brief The scale of the model (deprecated I think) - float modelScale = 1; - /// @brief Set the position of the thing /// @param position The new position in local space, in meters void SetPosition(Spherical position); /// @brief Get the position of the thing /// @return The position in local space, in meters Spherical GetPosition(); + /// @brief Boolean indicating that the thing has an updated position + bool positionUpdated = false; + /// @brief Set the orientation of the thing /// @param orientation The new orientation in local space void SetOrientation(SwingTwist orientation); /// @brief Get the orientation of the thing /// @return The orienation in local space SwingTwist GetOrientation(); - /// @brief The scale of the thing (deprecated I think) - // float scale = 1; // assuming uniform scale - - /// @brief boolean indicating if the position was updated - bool positionUpdated = false; - /// @brief boolean indicating if the orientation was updated + /// @brief Boolean indicating the thing has an updated orientation bool orientationUpdated = false; /// @brief Set the linear velocity of the thing @@ -155,58 +176,62 @@ class Thing { /// @brief Get the linear velocity of the thing /// @return The linear velocity in local space, in meters per second virtual Spherical GetLinearVelocity(); + /// @brief Boolean indicating the thing has an updated linear velocity + bool linearVelocityUpdated = false; + /// @brief Set the angular velocity of the thing /// @param angularVelocity the new angular velocity in local space virtual void SetAngularVelocity(Spherical angularVelocity); /// @brief Get the angular velocity of the thing /// @return The angular velocity in local space virtual Spherical GetAngularVelocity(); - bool linearVelocityUpdated = false; + /// @brief Boolean indicating the thing has an updated angular velocity bool angularVelocityUpdated = false; private: - /// @brief The position in local space + /// @brief The position of the thing in local space, in meters /// @remark When this Thing has a parent, the position is relative to the /// parent's position and orientation Spherical position; - /// @brief The orientation in local space + /// @brief The orientation of the thing in local space /// @remark When this Thing has a parent, the orientation is relative to the /// parent's orientation SwingTwist orientation; - /// @brief The linear velocity in local space + /// @brief The linear velocity of the thing in local space, in meters per second Spherical linearVelocity; - /// @brief The angular velocity in local spze + /// @brief The angular velocity of the thing in local space, in degrees per second Spherical angularVelocity; +#pragma endregion Pose + +#pragma region Update + public: - /// @brief Terminated things are no longer updated - void Terminate(); - bool terminate = false; - - /// @brief Sets the location from where the 3D model of this Thing can be - /// loaded from - /// @param url The url of the model - /// @remark Although the roboid implementation is not dependent on the model, - /// the only official supported model format is .obj - void SetModel(const char* url); - + /// @brief Get the current time in milliseconds + /// @return The current time in milliseconds static unsigned long GetTimeMs(); + /// @brief Updates the state of the thing void Update(bool recursive = false); /// @brief Updates the state of the thing - /// @param currentTimeMs The current clock time in milliseconds - virtual void Update(unsigned long currentTimeMs, bool recursive = false); + /// @param currentTimeMs The current clock time in milliseconds; if this is + /// zero, the current time is retrieved automatically + /// @param recurse When true, this will Update the descendants recursively + virtual void Update(unsigned long currentTimeMs, bool recurse = false); static void UpdateThings(unsigned long currentTimeMs); +#pragma endregion Update + + public: /// @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 - /// @returns The size of the binary data + /// @return 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 + /// @brief Function used to process binary data received for this thing /// @param bytes The binary data virtual void ProcessBinary(char* bytes); };