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

Alignment with C#

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-04 11:49:47 +02:00
parent c24925a4da
commit e36f3bb45d
4 changed files with 276 additions and 314 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Participant.cpp
View File

@ -16,17 +16,13 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
} }
Participant::Participant() { Participant::Participant() {
//std::cout << "P\n"; Thing::CreateRoot(this);
//this->root.name = "Isolated"; //this->Add(this->root);
this->root = new Thing(this);
this->root->name = "Root";
this->Add(this->root);
} }
Participant::Participant(const char* ipAddress, int port) { Participant::Participant(const char* ipAddress, int port) {
// Add the root thing to the list of things, because we could not do that Thing::CreateRoot(this);
// earlier (I think) //this->Add(this->root);
this->Add(this->root);
// make a copy of the ip address string // make a copy of the ip address string
int addressLength = (int)strlen(ipAddress); int addressLength = (int)strlen(ipAddress);

55
Participant.h
View File

@ -59,30 +59,48 @@ class ParticipantRegistry {
/// participant. It is used as a basis for the local participant, but also as a /// participant. It is used as a basis for the local participant, but also as a
/// reference to remote participants. /// reference to remote participants.
class Participant { class Participant {
#pragma region Init
public:
/// @brief Create a generic participant
Participant();
/// @brief Create a new participant with the given communcation info
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
Participant(const char* ipAddress, int port);
/// @brief Destructor for the participant
~Participant();
/// @brief The local participant for this application
static Participant* LocalParticipant;
/// @brief Replace the local participant
/// @param newParticipant The new local Participant
static void ReplaceLocalParticipant(Participant& newParticipant);
#pragma endregion Init
#pragma region Properties
public: public:
/// @brief The name of the participant /// @brief The name of the participant
const char* name = "Participant"; const char* name = "Participant";
/// @brief The Ip Address of a participant. /// @brief The Ip Address of a participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
const char* ipAddress = "0.0.0.0"; const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant. /// @brief The port number for UDP communication with the participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
unsigned int port = 0; unsigned int port = 0;
/// @brief The network Id to identify the participant /// @brief The network Id to identify the participant
unsigned char networkId = 0; unsigned char networkId = 0;
Participant(); /// @brief The root thing for this participant
/// @brief Create a new participant with the given communcation info Thing* root = nullptr;
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
Participant(const char* ipAddress, int port);
/// @brief Destructor for the participant
~Participant();
static Participant* LocalParticipant;
static void ReplaceLocalParticipant(Participant& newParticipant);
Thing* root = new Thing(this);
public: public:
#if defined(NO_STD) #if defined(NO_STD)
@ -104,12 +122,23 @@ class Participant {
/// @param thing The thing to remove /// @param thing The thing to remove
void Remove(Thing* thing); void Remove(Thing* thing);
#pragma endregion Properties
#pragma region Update
public:
/// @brief Update all things for this participant /// @brief Update all things for this participant
/// @param currentTimeMs The current time in milliseconds (optional)
virtual void Update(); virtual void Update();
#pragma endregion Update
#pragma region Participant Registry
public: public:
static ParticipantRegistry registry; static ParticipantRegistry registry;
#pragma endregion Participant Registry
}; };
} // namespace RoboidControl } // namespace RoboidControl

470
Thing.cpp
View File

@ -16,337 +16,277 @@
#include <list> #include <list>
#endif #endif
namespace RoboidControl namespace RoboidControl {
{
#pragma region Init #pragma region Init
Thing *Thing::LocalRoot() Thing* Thing::LocalRoot() {
{ Participant* p = Participant::LocalParticipant;
Participant *p = Participant::LocalParticipant; Thing* localRoot = p->root;
Thing *localRoot = p->root; return localRoot;
return localRoot; }
}
// Only use this for root things // Only use this for root things
Thing::Thing(Participant *owner) Thing::Thing(Participant* owner) {
{ this->type = Type::Root;
this->type = Type::Root; // should become root this->name = "Root";
this->position = Spherical::zero; this->position = Spherical::zero;
this->positionUpdated = true; this->positionUpdated = true;
this->orientation = SwingTwist::identity; this->orientation = SwingTwist::identity;
this->orientationUpdated = true; this->orientationUpdated = true;
this->hierarchyChanged = true; this->hierarchyChanged = true;
this->linearVelocity = Spherical::zero; this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero; this->angularVelocity = Spherical::zero;
this->owner = owner; this->owner = owner;
// this->owner->Add(this, true); this->owner->Add(this);
//std::cout << this->owner->name << ": New root thing " << std::endl; // std::cout << this->owner->name << ": New root thing " << std::endl;
} }
Thing::Thing(Thing *parent) void Thing::CreateRoot(Participant* owner) {
{ owner->root = new Thing(owner);
this->type = Type::Undetermined; }
this->position = Spherical::zero; Thing::Thing(Thing* parent) {
this->positionUpdated = true; this->type = Type::Undetermined;
this->orientation = SwingTwist::identity;
this->orientationUpdated = true;
this->hierarchyChanged = true;
this->linearVelocity = Spherical::zero; this->position = Spherical::zero;
this->angularVelocity = Spherical::zero; this->positionUpdated = true;
this->orientation = SwingTwist::identity;
this->orientationUpdated = true;
this->hierarchyChanged = true;
this->owner = parent->owner; this->linearVelocity = Spherical::zero;
this->owner->Add(this, true); this->angularVelocity = Spherical::zero;
this->SetParent(parent);
std::cout << this->owner->name << ": New thing for " << parent->name this->owner = parent->owner;
<< std::endl; this->owner->Add(this, true);
} this->SetParent(parent);
Thing::~Thing() std::cout << this->owner->name << ": New thing for " << parent->name
{ << std::endl;
std::cout << "Destroy thing " << this->name << std::endl; }
}
Thing::~Thing() {
std::cout << "Destroy thing " << this->name << std::endl;
}
#pragma endregion Init #pragma endregion Init
void Thing::SetName(const char *name) void Thing::SetName(const char* name) {
{ this->name = name;
this->name = name; this->nameChanged = true;
this->nameChanged = true; }
}
const char *Thing::GetName() const const char* Thing::GetName() const {
{ return this->name;
return this->name; }
}
void Thing::SetModel(const char *url) void Thing::SetModel(const char* url) {
{ this->modelUrl = url;
this->modelUrl = url; }
}
#pragma region Hierarchy #pragma region Hierarchy
void Thing::SetParent(Thing *parent) void Thing::SetParent(Thing* parent) {
{ if (parent == nullptr) {
if (parent == nullptr) Thing* parentThing = this->parent;
{ if (parentThing != nullptr)
Thing *parentThing = this->parent; parentThing->RemoveChild(this);
if (parentThing != nullptr) this->parent = nullptr;
parentThing->RemoveChild(this); } else
this->parent = nullptr; parent->AddChild(this);
this->hierarchyChanged = true;
}
bool Thing::IsRoot() const {
return this == LocalRoot() || this->parent == nullptr;
}
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;
} }
else
parent->AddChild(this);
this->hierarchyChanged = true;
} }
// void Thing::SetParent(Thing* parent) { newChildren[this->childCount] = child;
// parent->AddChild(this); child->parent = 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;
}
}
newChildren[this->childCount] = child;
child->parent = this;
if (this->children != nullptr)
delete[] this->children;
this->children = newChildren;
this->childCount = newChildCount;
}
Thing *Thing::RemoveChild(Thing *child)
{
unsigned char newChildCount = this->childCount - 1;
Thing **newChildren = new Thing *[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];
}
}
child->parent = Thing::LocalRoot();
if (this->children != nullptr)
delete[] this->children; delete[] this->children;
this->children = newChildren;
this->childCount = newChildCount;
return child; this->children = newChildren;
} this->childCount = newChildCount;
}
Thing *Thing::GetChild(unsigned char id, bool recurse) Thing* Thing::RemoveChild(Thing* child) {
{ unsigned char newChildCount = this->childCount - 1;
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) Thing** newChildren = new Thing*[newChildCount];
{
Thing *child = this->children[childIx];
if (child == nullptr)
continue;
if (child->id == id)
return child;
if (recurse) unsigned char newChildIx = 0;
{ for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing *foundChild = child->GetChild(id, recurse); if (this->children[childIx] != child) {
if (foundChild != nullptr) if (newChildIx == newChildCount) { // We did not find the child
return foundChild; // stop copying and return nothing
} delete[] newChildren;
return nullptr;
} else
newChildren[newChildIx++] = this->children[childIx];
} }
return nullptr;
} }
Thing *Thing::FindChild(const char *name, bool recurse) child->parent = Thing::LocalRoot();
{
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) delete[] this->children;
return child; this->children = newChildren;
this->childCount = newChildCount;
Thing *foundChild = child->FindChild(name); 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) if (foundChild != nullptr)
return foundChild; 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 endregion Hierarchy
#pragma region Pose #pragma region Pose
void Thing::SetPosition(Spherical position) void Thing::SetPosition(Spherical position) {
{ this->position = position;
this->position = position; this->positionUpdated = true;
this->positionUpdated = true; }
} Spherical Thing::GetPosition() {
Spherical Thing::GetPosition() return this->position;
{ }
return this->position;
}
void Thing::SetOrientation(SwingTwist orientation) void Thing::SetOrientation(SwingTwist orientation) {
{ this->orientation = orientation;
this->orientation = orientation; this->orientationUpdated = true;
this->orientationUpdated = true; }
}
SwingTwist Thing::GetOrientation() SwingTwist Thing::GetOrientation() {
{ return this->orientation;
return this->orientation; }
}
void Thing::SetLinearVelocity(Spherical linearVelocity) void Thing::SetLinearVelocity(Spherical linearVelocity) {
{ if (this->linearVelocity.distance != linearVelocity.distance) {
if (this->linearVelocity.distance != linearVelocity.distance) this->linearVelocity = linearVelocity;
{ this->linearVelocityUpdated = true;
this->linearVelocity = linearVelocity;
this->linearVelocityUpdated = true;
}
} }
}
Spherical Thing::GetLinearVelocity() Spherical Thing::GetLinearVelocity() {
{ return this->linearVelocity;
return this->linearVelocity; }
}
void Thing::SetAngularVelocity(Spherical angularVelocity) void Thing::SetAngularVelocity(Spherical angularVelocity) {
{ if (this->angularVelocity.distance != angularVelocity.distance) {
if (this->angularVelocity.distance != angularVelocity.distance) this->angularVelocity = angularVelocity;
{ this->angularVelocityUpdated = true;
this->angularVelocity = angularVelocity;
this->angularVelocityUpdated = true;
}
} }
}
Spherical Thing::GetAngularVelocity() Spherical Thing::GetAngularVelocity() {
{ return this->angularVelocity;
return this->angularVelocity; }
}
#pragma endregion Pose #pragma endregion Pose
#pragma region Update #pragma region Update
unsigned long Thing::GetTimeMs() unsigned long Thing::GetTimeMs() {
{
#if defined(ARDUINO) #if defined(ARDUINO)
unsigned long ms = millis(); unsigned long ms = millis();
return ms; return ms;
#else #else
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>( auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
now.time_since_epoch()); now.time_since_epoch());
return static_cast<unsigned long>(ms.count()); return static_cast<unsigned long>(ms.count());
#endif #endif
} }
// void Thing::Update(bool recursive) { // void Thing::Update(bool recursive) {
// Update(GetTimeMs(), recursive); // Update(GetTimeMs(), recursive);
// } // }
void Thing::PrepareForUpdate() {} void Thing::PrepareForUpdate() {}
void Thing::Update(bool recursive) void Thing::Update(bool recursive) {
{ this->positionUpdated = false;
// if (this->positionUpdated || this->orientationUpdated) this->orientationUpdated = false;
// OnPoseChanged callback this->linearVelocityUpdated = false;
this->positionUpdated = false; this->angularVelocityUpdated = false;
this->orientationUpdated = false; this->hierarchyChanged = false;
// this->linearVelocityUpdated = false; this->nameChanged = false;
// this->angularVelocityUpdated = false;
this->hierarchyChanged = false;
this->nameChanged = false;
if (recursive) if (recursive) {
{ // std::cout << "# children: " << (int)this->childCount << std::endl;
// std::cout << "# children: " << (int)this->childCount << std::endl; for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) Thing* child = this->children[childIx];
{ if (child == nullptr)
Thing *child = this->children[childIx]; continue;
if (child == nullptr) child->Update(recursive);
continue;
child->Update(recursive);
}
} }
} }
}
void Thing::UpdateThings()
{
IsolatedParticipant::Isolated()->Update();
}
#pragma endregion Update #pragma endregion Update
int Thing::GenerateBinary(char *buffer, unsigned char *ix) int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
{ (void)buffer;
(void)buffer; (void)ix;
(void)ix; return 0;
return 0; }
} void Thing::ProcessBinary(char* bytes) {
void Thing::ProcessBinary(char *bytes) (void)bytes;
{ };
(void)bytes;
};
} // namespace RoboidControl } // namespace RoboidControl

53
Thing.h
View File

@ -43,32 +43,35 @@ class Thing {
}; };
#pragma region Init #pragma region Init
static Thing* LocalRoot();
private: private:
// Special constructor to create a root thing
Thing(Participant* parent);
// Which can only be used by the Participant
friend class Participant;
public: public:
/// @brief Create a new thing /// @brief Create a new Thing
/// @param thingType The type of thing (can use Thing::Type)
/// @param parent (optional) The parent thing /// @param parent (optional) The parent thing
/// The owner will be the same as the owner of the parent thing, it will /// The owner will be the same as the owner of the parent thing, it will
/// be Participant::LocalParticipant if the parent is not specified. A thing /// be Participant::LocalParticipant if the parent is not specified. A thing
/// without a parent will be a root thing. /// without a parent will be connected to the root thing.
Thing(Thing* parent = LocalRoot()); Thing(Thing* parent = LocalRoot());
/// @brief Create a new child thing private:
/// @param parent The parent thing /// @brief Constructor to create a root thing
/// @param thingType The type of thing (can use Thing::Type) /// @param owner The participant who will own this root thing
/// @param thingId The ID of the thing, leave out or set to zero to generate /// @remarks This function is private because CreateRoot() should be used
/// an ID /// instead
/// @note The owner will be the same as the owner of the parent thing Thing(Participant* owener);
public:
/// @brief Destructor for a Thing
~Thing(); ~Thing();
/// @brief Create a root thing for a participant
/// @param owner The participant who will own this root thing
static void CreateRoot(Participant* owner);
/// @brief The root thing for the local participant
/// @return The root thing for the local participant
static Thing* LocalRoot();
#pragma endregion Init #pragma endregion Init
public: public:
@ -77,9 +80,7 @@ class Thing {
#pragma region Properties #pragma region Properties
/// @brief The participant managing this thing public:
Participant* owner = nullptr;
/// @brief The ID of the thing /// @brief The ID of the thing
unsigned char id = 0; unsigned char id = 0;
@ -87,10 +88,12 @@ class Thing {
/// This can be either a Thing::Type of a byte value for custom types /// This can be either a Thing::Type of a byte value for custom types
unsigned char type = Type::Undetermined; unsigned char type = Type::Undetermined;
/// @brief The participant owning this thing
Participant* owner = nullptr;
/// @brief The name of the thing /// @brief The name of the thing
const char* name = nullptr; const char* name = nullptr;
public:
void SetName(const char* name); void SetName(const char* name);
const char* GetName() const; const char* GetName() const;
bool nameChanged = false; bool nameChanged = false;
@ -99,14 +102,12 @@ class Thing {
/// loaded from /// loaded from
/// @param url The url of the model /// @param url The url of the model
/// @remark Although the roboid implementation is not dependent on the model, /// @remark Although the roboid implementation is not dependent on the model,
/// the only official supported model format is .obj /// the only official supported model formats are .png (sprite), .gltf and .glb
void SetModel(const char* url); void SetModel(const char* url);
/// @brief An URL pointing to the location where a model of the thing can be /// @brief An URL pointing to the location where a model of the thing can be
/// found /// found
const char* modelUrl = nullptr; const char* modelUrl = nullptr;
/// @brief The scale of the model (deprecated I think)
float modelScale = 1;
#pragma endregion Properties #pragma endregion Properties
@ -114,13 +115,13 @@ class Thing {
/// @brief Sets the parent of this Thing /// @brief Sets the parent of this Thing
/// @param parent The Thing which should become the parent /// @param parent The Thing which should become the parent
// virtual void SetParent(Thing* parent);
void SetParent(Thing* parent); void SetParent(Thing* parent);
/// @brief Gets the parent of this Thing /// @brief Gets the parent of this Thing
/// @return The parent Thing /// @return The parent Thing
// Thing* GetParent();
Thing* GetParent(); Thing* GetParent();
/// @brief Check if this is a root thing
/// @return True is this thing is a root
bool IsRoot() const; bool IsRoot() const;
/// @brief The number of children /// @brief The number of children
@ -225,13 +226,9 @@ class Thing {
virtual void PrepareForUpdate(); virtual void PrepareForUpdate();
/// @brief Updates the state of the thing /// @brief Updates the state of the thing
/// @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 /// @param recurse When true, this will Update the descendants recursively
virtual void Update(bool recurse = false); virtual void Update(bool recurse = false);
static void UpdateThings();
/// @brief Get the current time in milliseconds /// @brief Get the current time in milliseconds
/// @return The current time in milliseconds /// @return The current time in milliseconds
static unsigned long GetTimeMs(); static unsigned long GetTimeMs();