Compare commits

...

3 Commits

Author SHA1 Message Date
2a83dbe7ca Alignment with C# 2025-06-04 12:17:30 +02:00
e36f3bb45d Alignment with C# 2025-06-04 11:49:47 +02:00
c24925a4da Compatibility fixes 2025-06-04 10:58:05 +02:00
11 changed files with 418 additions and 385 deletions

View File

@ -7,7 +7,9 @@ namespace Arduino {
#pragma region DRV8833 #pragma region DRV8833
DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) { DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) {
this->type = Type::Undetermined;
this->name = "DRV8833";
this->pinStandby = config.standby; this->pinStandby = config.standby;
if (pinStandby != 255) if (pinStandby != 255)
pinMode(pinStandby, OUTPUT); pinMode(pinStandby, OUTPUT);

View File

@ -7,8 +7,9 @@ namespace Arduino {
#pragma region Digital input #pragma region Digital input
DigitalInput::DigitalInput(unsigned char pin, Thing* parent) DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(parent) {
: Thing(Type::Undetermined, parent) { this->type = Type::Switch;
this->name = "Digital Input";
this->pin = pin; this->pin = pin;
pinMode(this->pin, INPUT); pinMode(this->pin, INPUT);
std::cout << "digital input start\n"; std::cout << "digital input start\n";

View File

@ -7,7 +7,8 @@ namespace RoboidControl {
namespace Arduino { namespace Arduino {
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
: Thing(Type::Undetermined, parent) { : Thing(parent) {
this->type = Type::DistanceSensor;
this->name = "Ultrasonic sensor"; this->name = "Ultrasonic sensor";
this->pinTrigger = config.trigger; this->pinTrigger = config.trigger;
this->pinEcho = config.echo; this->pinEcho = config.echo;

View File

@ -1,6 +1,10 @@
#include "Participant.h" #include "Participant.h"
#include <string.h> #include <string.h>
#include "Arduino/ArduinoParticipant.h"
#include "EspIdf/EspIdfParticipant.h"
#include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.h"
namespace RoboidControl { namespace RoboidControl {
@ -16,17 +20,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);
@ -56,6 +56,34 @@ void Participant::Update() {
} }
} }
bool Participant::Send(IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
return true;
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
// << " to " << remoteParticipant->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Send(remoteParticipant, bufferSize);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->Send(this, bufferSize);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->Send(remoteParticipant, bufferSize);
#else
return false;
#endif
}
Thing* Participant::Get(unsigned char thingId) { Thing* Participant::Get(unsigned char thingId) {
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
if (thing->id == thingId) if (thing->id == thingId)

View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include "Messages/IMessage.h"
#include "Thing.h" #include "Thing.h"
namespace RoboidControl { namespace RoboidControl {
@ -59,30 +60,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 +123,31 @@ 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 Send
public:
char buffer[1024];
virtual bool Send(IMessage* msg);
#pragma endregion Send
#pragma region Participant Registry
public: public:
static ParticipantRegistry registry; static ParticipantRegistry registry;
#pragma endregion Participant Registry
}; };
} // namespace RoboidControl } // namespace RoboidControl

View File

@ -46,14 +46,6 @@ ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
Participant::ReplaceLocalParticipant(*this); Participant::ReplaceLocalParticipant(*this);
} }
static ParticipantUDP* isolatedParticipant = nullptr;
ParticipantUDP* ParticipantUDP::Isolated() {
if (isolatedParticipant == nullptr)
isolatedParticipant = new ParticipantUDP(0);
return isolatedParticipant;
}
void ParticipantUDP::begin() { void ParticipantUDP::begin() {
if (this->isIsolated || this->remoteSite == nullptr) if (this->isIsolated || this->remoteSite == nullptr)
return; return;
@ -105,7 +97,8 @@ void ParticipantUDP::Update() {
if (this->remoteSite == nullptr) if (this->remoteSite == nullptr)
this->Publish(msg); this->Publish(msg);
else else
this->Send(this->remoteSite, msg); this->remoteSite->Send(msg);
delete msg; delete msg;
this->nextPublishMe = currentTimeMs + this->publishInterval; this->nextPublishMe = currentTimeMs + this->publishInterval;
@ -137,12 +130,12 @@ void ParticipantUDP::UpdateMyThings() {
if (thing->hierarchyChanged) { if (thing->hierarchyChanged) {
if (!(this->isIsolated || this->networkId == 0)) { if (!(this->isIsolated || this->networkId == 0)) {
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
this->Send(this->remoteSite, thingMsg); this->remoteSite->Send(thingMsg);
delete thingMsg; delete thingMsg;
if (thing->nameChanged) { if (thing->nameChanged) {
NameMsg* nameMsg = new NameMsg(this->networkId, thing); NameMsg* nameMsg = new NameMsg(this->networkId, thing);
this->Send(this->remoteSite, nameMsg); this->remoteSite->Send(nameMsg);
delete nameMsg; delete nameMsg;
} }
} }
@ -159,20 +152,20 @@ void ParticipantUDP::UpdateMyThings() {
if (!(this->isIsolated || this->networkId == 0)) { if (!(this->isIsolated || this->networkId == 0)) {
if (thing->terminate) { if (thing->terminate) {
DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
this->Send(this->remoteSite, destroyMsg); this->remoteSite->Send(destroyMsg);
delete destroyMsg; delete destroyMsg;
} else { } else {
// Send to remote site // Send to remote site
if (thing->nameChanged) { if (thing->nameChanged) {
NameMsg* nameMsg = new NameMsg(this->networkId, thing); NameMsg* nameMsg = new NameMsg(this->networkId, thing);
this->Send(this->remoteSite, nameMsg); this->remoteSite->Send(nameMsg);
delete nameMsg; delete nameMsg;
} }
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
this->Send(this->remoteSite, poseMsg); this->remoteSite->Send(poseMsg);
delete poseMsg; delete poseMsg;
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
this->Send(this->remoteSite, binaryMsg); this->remoteSite->Send(binaryMsg);
delete binaryMsg; delete binaryMsg;
} }
} }
@ -203,10 +196,10 @@ void ParticipantUDP::UpdateOtherThings() {
for (Thing* thing : participant->things) { for (Thing* thing : participant->things) {
PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing); PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
this->Send(participant, poseMsg); participant->Send(poseMsg);
delete poseMsg; delete poseMsg;
BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing); BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
this->Send(participant, binaryMsg); participant->Send(binaryMsg);
delete binaryMsg; delete binaryMsg;
} }
} }
@ -221,49 +214,50 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
Thing* thing) { Thing* thing) {
// std::cout << "Send thing info [" << (int)thing->id << "] \n"; // std::cout << "Send thing info [" << (int)thing->id << "] \n";
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
this->Send(remoteParticipant, thingMsg); remoteParticipant->Send(thingMsg);
delete thingMsg; delete thingMsg;
NameMsg* nameMsg = new NameMsg(this->networkId, thing); NameMsg* nameMsg = new NameMsg(this->networkId, thing);
this->Send(remoteParticipant, nameMsg); remoteParticipant->Send(nameMsg);
delete nameMsg; delete nameMsg;
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing); ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
this->Send(remoteParticipant, modelMsg); remoteParticipant->Send(modelMsg);
delete modelMsg; delete modelMsg;
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true); PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
this->Send(remoteParticipant, poseMsg); remoteParticipant->Send(poseMsg);
delete poseMsg; delete poseMsg;
BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing); BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
this->Send(remoteParticipant, customMsg); remoteParticipant->Send(binaryMsg);
delete customMsg; delete binaryMsg;
} }
bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { // bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer); // int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0) // if (bufferSize <= 0)
return true; // return true;
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff) // // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
// << " to " << remoteParticipant->ipAddress << std::endl; // // << " to " << remoteParticipant->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64) // #if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows = // Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this); // static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Send(remoteParticipant, bufferSize); // return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__) // #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this); // Posix::ParticipantUDP* thisPosix =
return thisPosix->Send(remoteParticipant, bufferSize); // static_cast<Posix::ParticipantUDP*>(this); return
#elif defined(ARDUINO) // thisPosix->Send(remoteParticipant, bufferSize);
Arduino::ParticipantUDP* thisArduino = // #elif defined(ARDUINO)
static_cast<Arduino::ParticipantUDP*>(this); // Arduino::ParticipantUDP* thisArduino =
return thisArduino->Send(remoteParticipant, bufferSize); // static_cast<Arduino::ParticipantUDP*>(this);
#elif defined(IDF_VER) // return thisArduino->Send(remoteParticipant, bufferSize);
EspIdf::ParticipantUDP* thisEspIdf = // #elif defined(IDF_VER)
static_cast<EspIdf::ParticipantUDP*>(this); // EspIdf::ParticipantUDP* thisEspIdf =
return thisEspIdf->Send(remoteParticipant, bufferSize); // static_cast<EspIdf::ParticipantUDP*>(this);
#else // return thisEspIdf->Send(remoteParticipant, bufferSize);
return false; // #else
#endif // return false;
} // #endif
// }
void ParticipantUDP::PublishThingInfo(Thing* thing) { void ParticipantUDP::PublishThingInfo(Thing* thing) {
// std::cout << "Publish thing info" << thing->networkId << "\n"; // std::cout << "Publish thing info" << thing->networkId << "\n";
@ -402,6 +396,18 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
Process(sender, msg); Process(sender, msg);
delete msg; delete msg;
} break; } break;
case TextMsg::id: {
TextMsg* msg = new TextMsg(this->buffer);
bufferSize -= msg->length + msg->textLength;
Process(sender, msg);
delete msg;
} break;
case DestroyMsg::id: {
DestroyMsg* msg = new DestroyMsg(this->buffer);
bufferSize -= msg->length;
Process(sender, msg);
delete msg;
} break;
}; };
// Check if the buffer has been read completely // Check if the buffer has been read completely
@ -532,6 +538,24 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
} }
} }
void ParticipantUDP::Process(Participant* sender, TextMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
<< (int)msg->text << "\n";
#endif
}
void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "]\n";
#endif
Thing* thing = sender->Get(msg->thingId);
if (thing != nullptr)
this->Remove(thing);
}
// Receive // Receive
#pragma endregion #pragma endregion

View File

@ -9,6 +9,7 @@
#include "Messages/PoseMsg.h" #include "Messages/PoseMsg.h"
#include "Messages/NetworkIdMsg.h" #include "Messages/NetworkIdMsg.h"
#include "Messages/ThingMsg.h" #include "Messages/ThingMsg.h"
#include "Messages/TextMsg.h"
#include "Participant.h" #include "Participant.h"
#if !defined(NO_STD) #if !defined(NO_STD)
@ -43,6 +44,7 @@ constexpr int MAX_SENDER_COUNT = 256;
/// RoboidControl::IsolatedParticipant::Isolated(). /// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing() /// @sa RoboidControl::Thing::Thing()
class ParticipantUDP : public Participant { class ParticipantUDP : public Participant {
#pragma region Init #pragma region Init
public: public:
@ -58,19 +60,15 @@ class ParticipantUDP : public Participant {
/// @param localPort The port used by the local participant /// @param localPort The port used by the local participant
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
/// @brief Isolated participant is used when the application is run without
/// networking
/// @return A participant without networking support
static ParticipantUDP* Isolated();
/// @brief True if the participant is running isolated.
/// Isolated participants do not communicate with other participants
#pragma endregion Init #pragma endregion Init
#pragma region Properties
public:
/// @brief True if the participant is running isolated. /// @brief True if the participant is running isolated.
/// Isolated participants do not communicate with other participants /// Isolated participants do not communicate with other participants
bool isIsolated = false; bool isIsolated = false;
/// @brief The remote site when this participant is connected to a site /// @brief The remote site when this participant is connected to a site
Participant* remoteSite = nullptr; Participant* remoteSite = nullptr;
@ -94,6 +92,8 @@ class ParticipantUDP : public Participant {
void begin(); void begin();
bool connected = false; bool connected = false;
#pragma endregion Properties
#pragma region Update #pragma region Update
public: public:
@ -114,7 +114,7 @@ class ParticipantUDP : public Participant {
void SendThingInfo(Participant* remoteParticipant, Thing* thing); void SendThingInfo(Participant* remoteParticipant, Thing* thing);
void PublishThingInfo(Thing* thing); void PublishThingInfo(Thing* thing);
bool Send(Participant* remoteParticipant, IMessage* msg); //bool Send(Participant* remoteParticipant, IMessage* msg);
bool Publish(IMessage* msg); bool Publish(IMessage* msg);
#pragma endregion Send #pragma endregion Send
@ -139,6 +139,8 @@ protected:
virtual void Process(Participant* sender, ModelUrlMsg* msg); virtual void Process(Participant* sender, ModelUrlMsg* msg);
virtual void Process(Participant* sender, PoseMsg* msg); virtual void Process(Participant* sender, PoseMsg* msg);
virtual void Process(Participant* sender, BinaryMsg* msg); virtual void Process(Participant* sender, BinaryMsg* msg);
virtual void Process(Participant* sender, TextMsg* msg);
virtual void Process(Participant* sender, DestroyMsg* msg);
#pragma endregion Receive #pragma endregion Receive

View File

@ -42,10 +42,10 @@ void SiteServer::UpdateMyThings() {
continue; continue;
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
this->Send(participant, poseMsg); participant->Send(poseMsg);
delete poseMsg; delete poseMsg;
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
this->Send(participant, binaryMsg); participant->Send(binaryMsg);
delete binaryMsg; delete binaryMsg;
} }
} }
@ -62,7 +62,7 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
// sender->ipAddress // sender->ipAddress
// << ":" << (int)sender->port << "\n"; // << ":" << (int)sender->port << "\n";
NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId); NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
this->Send(sender, msg); sender->Send(msg);
delete msg; delete msg;
} }
} }

View File

@ -2,11 +2,11 @@
#include "ParticipantUDP.h" #include "ParticipantUDP.h"
#if !defined(NO_STD) // #if !defined(NO_STD)
#include <functional> // #include <functional>
#include <memory> // #include <memory>
#include <unordered_map> // #include <unordered_map>
#endif // #endif
namespace RoboidControl { namespace RoboidControl {

252
Thing.cpp
View File

@ -16,22 +16,20 @@
#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;
@ -43,12 +41,15 @@ namespace RoboidControl
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);
}
Thing::Thing(Thing* parent) {
this->type = Type::Undetermined; this->type = Type::Undetermined;
this->position = Spherical::zero; this->position = Spherical::zero;
@ -66,87 +67,59 @@ namespace RoboidControl
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;
} }
#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;
{
Thing *parentThing = this->parent;
if (parentThing != nullptr) if (parentThing != nullptr)
parentThing->RemoveChild(this); parentThing->RemoveChild(this);
this->parent = nullptr; this->parent = nullptr;
} } else
else
parent->AddChild(this); parent->AddChild(this);
this->hierarchyChanged = true; this->hierarchyChanged = true;
} }
// void Thing::SetParent(Thing* parent) { bool Thing::IsRoot() const {
// parent->AddChild(this); return this == LocalRoot() || this->parent == nullptr;
// this->hierarchyChanged = true; }
// }
// const Thing& Thing::GetParent() { 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; return this->parent;
} }
Thing *Thing::GetChildByIndex(unsigned char ix) Thing* Thing::GetChildByIndex(unsigned char ix) {
{
return this->children[ix]; return this->children[ix];
} }
void Thing::AddChild(Thing *child) void Thing::AddChild(Thing* child) {
{
unsigned char newChildCount = this->childCount + 1; unsigned char newChildCount = this->childCount + 1;
Thing **newChildren = new Thing *[newChildCount]; Thing** newChildren = new Thing*[newChildCount];
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
{
newChildren[childIx] = this->children[childIx]; newChildren[childIx] = this->children[childIx];
if (this->children[childIx] == child) if (this->children[childIx] == child) {
{
// child is already present, stop copying do not update the children // child is already present, stop copying do not update the children
delete[] newChildren; delete[] newChildren;
return; return;
@ -161,25 +134,20 @@ namespace RoboidControl
this->children = newChildren; this->children = newChildren;
this->childCount = newChildCount; this->childCount = newChildCount;
} }
Thing *Thing::RemoveChild(Thing *child) Thing* Thing::RemoveChild(Thing* child) {
{
unsigned char newChildCount = this->childCount - 1; unsigned char newChildCount = this->childCount - 1;
Thing **newChildren = new Thing *[newChildCount]; Thing** newChildren = new Thing*[newChildCount];
unsigned char newChildIx = 0; unsigned char newChildIx = 0;
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
{ if (this->children[childIx] != child) {
if (this->children[childIx] != child) if (newChildIx == newChildCount) { // We did not find the child
{
if (newChildIx == newChildCount)
{ // We did not find the child
// stop copying and return nothing // stop copying and return nothing
delete[] newChildren; delete[] newChildren;
return nullptr; return nullptr;
} } else
else
newChildren[newChildIx++] = this->children[childIx]; newChildren[newChildIx++] = this->children[childIx];
} }
} }
@ -191,105 +159,89 @@ namespace RoboidControl
this->childCount = newChildCount; this->childCount = newChildCount;
return child; return child;
} }
Thing *Thing::GetChild(unsigned char id, bool recurse) Thing* Thing::GetChild(unsigned char id, bool recurse) {
{ for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) Thing* child = this->children[childIx];
{
Thing *child = this->children[childIx];
if (child == nullptr) if (child == nullptr)
continue; continue;
if (child->id == id) if (child->id == id)
return child; return child;
if (recurse) if (recurse) {
{ Thing* foundChild = child->GetChild(id, 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) Thing* Thing::FindChild(const char* name, bool recurse) {
{ for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) Thing* child = this->children[childIx];
{
Thing *child = this->children[childIx];
if (child == nullptr || child->name == nullptr) if (child == nullptr || child->name == nullptr)
continue; continue;
if (strcmp(child->name, name) == 0) if (strcmp(child->name, name) == 0)
return child; return child;
Thing *foundChild = child->FindChild(name); Thing* foundChild = child->FindChild(name);
if (foundChild != nullptr) if (foundChild != nullptr)
return foundChild; return foundChild;
} }
return nullptr; 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->linearVelocity = linearVelocity;
this->linearVelocityUpdated = true; 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->angularVelocity = angularVelocity;
this->angularVelocityUpdated = true; 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;
@ -299,54 +251,42 @@ namespace RoboidControl
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
this->positionUpdated = false; this->positionUpdated = false;
this->orientationUpdated = false; this->orientationUpdated = false;
// this->linearVelocityUpdated = false; this->linearVelocityUpdated = false;
// this->angularVelocityUpdated = false; this->angularVelocityUpdated = false;
this->hierarchyChanged = false; this->hierarchyChanged = false;
this->nameChanged = 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];
Thing *child = this->children[childIx];
if (child == nullptr) if (child == nullptr)
continue; continue;
child->Update(recursive); 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();