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

thing tytpe enum->const

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-02 12:30:21 +02:00
parent c3ba44d47a
commit 2883f5e0fa
3 changed files with 293 additions and 247 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Participant.cpp
View File

@ -16,7 +16,7 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
} }
Participant::Participant() { Participant::Participant() {
std::cout << "P\n"; //std::cout << "P\n";
//this->root.name = "Isolated"; //this->root.name = "Isolated";
this->root = new Thing(this); this->root = new Thing(this);
this->root->name = "Root"; this->root->name = "Root";

502
Thing.cpp
View File

@ -16,300 +16,344 @@
#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; {
Thing* localRoot = p->root; Participant *p = Participant::LocalParticipant;
return localRoot; Thing *localRoot = p->root;
} 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::Roboid; // should become root {
this->type = Type::Root; // should become 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, true);
std::cout << this->owner->name << ": New root thing " << std::endl; //std::cout << this->owner->name << ": New root thing " << std::endl;
} }
Thing::Thing(unsigned char thingType, Thing* parent) { Thing::Thing(unsigned char thingType, Thing *parent)
this->type = thingType; {
this->type = thingType;
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 = parent->owner; this->owner = parent->owner;
this->owner->Add(this, true); this->owner->Add(this, true);
this->SetParent(parent); this->SetParent(parent);
std::cout << this->owner->name << ": New thing for " << parent->name std::cout << this->owner->name << ": New thing for " << parent->name
<< std::endl; << std::endl;
} }
Thing::~Thing() { Thing::~Thing()
std::cout << "Destroy thing " << this->name << std::endl; {
} std::cout << "Destroy thing " << this->name << std::endl;
}
// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType, Thing Thing::Reconstruct(Participant *owner, unsigned char thingType, unsigned char thingId)
// unsigned char thingId) { {
// Thing thing = Thing(owner, thingType); Thing thing = Thing(thingType, owner->root);
// thing.id = thingId; thing.id = thingId;
// return thing; return thing;
// } }
#pragma endregion Init #pragma endregion Init
void Thing::SetName(const char* name) { void Thing::SetName(const char *name)
this->name = name; {
this->nameChanged = true; this->name = name;
} 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) { {
Thing* parentThing = this->parent; if (parent == nullptr)
if (parentThing != nullptr) {
parentThing->RemoveChild(this); Thing *parentThing = this->parent;
this->parent = nullptr; if (parentThing != nullptr)
} else parentThing->RemoveChild(this);
parent->AddChild(this); this->parent = nullptr;
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;
} }
else
parent->AddChild(this);
this->hierarchyChanged = true;
} }
newChildren[this->childCount] = child; // void Thing::SetParent(Thing* parent) {
child->parent = this; // parent->AddChild(this);
// this->hierarchyChanged = true;
// }
if (this->children != nullptr) // const Thing& Thing::GetParent() {
delete[] this->children; // return *this->parent;
// }
this->children = newChildren; bool Thing::IsRoot() const
this->childCount = newChildCount; {
} return this == LocalRoot() || this->parent == nullptr; //&Thing::Root;
}
Thing* Thing::RemoveChild(Thing* child) { // void Thing::SetParent(Thing* root, const char* name) {
unsigned char newChildCount = this->childCount - 1; // Thing* thing = root->FindChild(name);
Thing** newChildren = new Thing*[newChildCount]; // if (thing != nullptr)
// this->SetParent(thing);
// }
unsigned char newChildIx = 0; Thing *Thing::GetParent()
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { {
if (this->children[childIx] != child) { return this->parent;
if (newChildIx == newChildCount) { // We did not find the child }
// stop copying and return nothing
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; delete[] newChildren;
return nullptr; return;
} else }
newChildren[newChildIx++] = this->children[childIx];
} }
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; unsigned char newChildIx = 0;
this->children = newChildren; for (unsigned char childIx = 0; childIx < this->childCount; childIx++)
this->childCount = newChildCount; {
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) { delete[] this->children;
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { this->children = newChildren;
Thing* child = this->children[childIx]; this->childCount = newChildCount;
if (child == nullptr)
continue;
if (child->id == id)
return child;
if (recurse) { return child;
Thing* foundChild = child->GetChild(id, recurse); }
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) 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->positionUpdated = true; 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::GetPosition()
{
Spherical Thing::GetLinearVelocity() { return this->position;
return this->linearVelocity;
}
void Thing::SetAngularVelocity(Spherical angularVelocity) {
if (this->angularVelocity.distance != angularVelocity.distance) {
this->angularVelocity = angularVelocity;
this->angularVelocityUpdated = true;
} }
}
Spherical Thing::GetAngularVelocity() { void Thing::SetOrientation(SwingTwist orientation)
return this->angularVelocity; {
} 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 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)
// if (this->positionUpdated || this->orientationUpdated) {
// OnPoseChanged callback // if (this->positionUpdated || this->orientationUpdated)
this->positionUpdated = false; // OnPoseChanged callback
this->orientationUpdated = false; this->positionUpdated = false;
// this->linearVelocityUpdated = false; this->orientationUpdated = false;
// this->angularVelocityUpdated = false; // this->linearVelocityUpdated = false;
this->hierarchyChanged = false; // this->angularVelocityUpdated = false;
this->nameChanged = false; this->hierarchyChanged = false;
this->nameChanged = false;
if (recursive) { if (recursive)
// std::cout << "# children: " << (int)this->childCount << std::endl; {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { // std::cout << "# children: " << (int)this->childCount << std::endl;
Thing* child = this->children[childIx]; for (unsigned char childIx = 0; childIx < this->childCount; childIx++)
if (child == nullptr) {
continue; Thing *child = this->children[childIx];
child->Update(recursive); if (child == nullptr)
continue;
child->Update(recursive);
}
} }
} }
}
void Thing::UpdateThings() { void Thing::UpdateThings()
IsolatedParticipant::Isolated()->Update(); {
} 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)ix; (void)buffer;
return 0; (void)ix;
} return 0;
void Thing::ProcessBinary(char* bytes) { }
(void)bytes; void Thing::ProcessBinary(char *bytes)
}; {
(void)bytes;
};
} // namespace RoboidControl } // namespace RoboidControl

36
Thing.h
View File

@ -20,24 +20,26 @@ class ParticipantUDP;
class Thing { class Thing {
public: public:
/// @brief Predefined thing types /// @brief Predefined thing types
enum Type : unsigned char { struct Type {
Undetermined, static const unsigned char Undetermined = 0x00;
// Sensor, // Sensor
Switch, static const unsigned char Switch = 0x01;
DistanceSensor, static const unsigned char DistanceSensor = 0x02;
DirectionalSensor, static const unsigned char DirectionalSensor = 0x03;
TemperatureSensor, static const unsigned char TemperatureSensor = 0x04;
TouchSensor, static const unsigned char TouchSensor = 0x05;
// Motor, // Motor
ControlledMotor, static const unsigned char ControlledMotor = 0x06;
UncontrolledMotor, static const unsigned char UncontrolledMotor = 0x07;
Servo, static const unsigned char Servo = 0x08;
IncrementalEncoder, static const unsigned char IncrementalEncoder = 0x19;
// Other // Other
Roboid, static const unsigned char Root = 0x10;
Humanoid, static const unsigned char Roboid = 0x09;
ExternalSensor, static const unsigned char Humanoid = 0x0A;
DifferentialDrive static const unsigned char ExternalSensor = 0x08;
static const unsigned char Animator = 0x0C;
static const unsigned char DifferentialDrive = 0x0D;
}; };
#pragma region Init #pragma region Init