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

Added gyroscope

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-30 09:52:13 +02:00
parent a826817a0b
commit a7a25bde6e
4 changed files with 156 additions and 358 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
Participant.cpp
View File

@ -19,34 +19,9 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
Participant::Participant() { Participant::Participant() {
Thing::CreateRoot(this); Thing::CreateRoot(this);
//this->Add(this->root);
} }
/* Participant::~Participant() {}
Participant::Participant(const char* ipAddress, int port) {
Thing::CreateRoot(this);
//this->Add(this->root);
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)
strncpy_s(addressString, stringLength, ipAddress,
addressLength); // Leave space for null terminator
#else
strncpy(addressString, ipAddress, addressLength);
#endif
addressString[addressLength] = '\0';
this->ipAddress = addressString;
this->port = port;
}
*/
Participant::~Participant() {
// registry.Remove(this);
// delete[] this->ipAddress;
}
void Participant::Update(bool recurse) { void Participant::Update(bool recurse) {
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
@ -56,48 +31,20 @@ void Participant::Update(bool recurse) {
} }
bool Participant::Send(IMessage* msg) { bool Participant::Send(IMessage* msg) {
std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff) std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff)
<< " to base Participant without communcation support " << std::endl; << " to base Participant without communcation support "
<< std::endl;
return true; return true;
} }
/*
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 " << this->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->SendTo(this, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->SendTo(this, bufferSize);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->SendTo(this, bufferSize);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->SendTo(this, bufferSize);
#else
return false;
#endif
}
*/
Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
if (thing->owner->networkId == networkId && thing->id == thingId) if (thing->owner->networkId == networkId && thing->id == thingId)
return thing; return thing;
} }
std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port std::cout << "Could not find thing " << "[" << (int)networkId << ": "
<< "[" << (int)thingId << "]\n"; << (int)thingId << "]\n";
return nullptr; return nullptr;
} }
@ -119,9 +66,8 @@ void Participant::Add(Thing* thing, bool checkId) {
thing->id = highestIx + 1; thing->id = highestIx + 1;
this->things.push_back(thing); this->things.push_back(thing);
#endif #endif
std::cout << "Add thing with generated ID " std::cout << "Add thing with generated ID "
//<< this->ipAddress << ":" << this->port << "[" << (int)networkId << ": " << (int)thing->id << "]\n";
<< "[" << (int)thing->id << "]\n";
} else { } else {
Thing* foundThing = Get(thing->owner->networkId, thing->id); Thing* foundThing = Get(thing->owner->networkId, thing->id);
if (foundThing == nullptr) { if (foundThing == nullptr) {
@ -130,13 +76,11 @@ void Participant::Add(Thing* thing, bool checkId) {
#else #else
this->things.push_back(thing); this->things.push_back(thing);
#endif #endif
std::cout << "Add thing " << //this->ipAddress << ":" << this->port << std::cout << "Add thing [" << (int)networkId << ": " << (int)thing->id
"[" << "]\n";
<< (int)thing->id << "]\n";
} else { } else {
std::cout << "Did not add, existing thing " std::cout << "Did not add, existing thing "
//<< this->ipAddress << ":" << this->port << "[" << (int)networkId << ": " << (int)thing->id << "]\n";
<< "[" << (int)thing->id << "]\n";
} }
} }
} }
@ -164,88 +108,4 @@ void Participant::Remove(Thing* thing) {
#pragma endregion #pragma endregion
// #pragma region ParticipantRegistry
// /*
// Participant* ParticipantRegistry::Get(const char* ipAddress,
// unsigned int port) {
// #if !defined(NO_STD)
// for (Participant* participant : ParticipantRegistry::participants) {
// if (participant == nullptr)
// continue;
// if (strcmp(participant->ipAddress, ipAddress) == 0 &&
// participant->port == port) {
// // std::cout << "found participant " << participant->ipAddress << ":"
// // << (int)participant->port << std::endl;
// return participant;
// }
// }
// std::cout << "Could not find participant " << ipAddress << ":" << (int)port
// << std::endl;
// #endif
// return nullptr;
// }
// */
// Participant* ParticipantRegistry::Get(unsigned char participantId) {
// #if !defined(NO_STD)
// for (Participant* participant : ParticipantRegistry::participants) {
// if (participant == nullptr)
// continue;
// if (participant->networkId == participantId)
// return participant;
// }
// std::cout << "Could not find participant " << (int)participantId << std::endl;
// #endif
// return nullptr;
// }
// // Participant* ParticipantRegistry::Add(const char* ipAddress,
// // unsigned int port) {
// // Participant* participant = new Participant(ipAddress, port);
// // Add(participant);
// // return participant;
// // }
// void ParticipantRegistry::Add(Participant* participant) {
// Participant* foundParticipant =
// Get(participant->networkId);
// //Get(participant->ipAddress, participant->port);
// if (foundParticipant == nullptr) {
// #if defined(NO_STD)
// // this->things[this->thingCount++] = thing;
// #else
// ParticipantRegistry::participants.push_back(participant);
// #endif
// // std::cout << "Add participant " << participant->ipAddress << ":"
// // << participant->port << "[" << (int)participant->networkId
// // << "]\n";
// // std::cout << "participants " <<
// // ParticipantRegistry::participants.size()
// // << "\n";
// // } else {
// // std::cout << "Did not add, existing participant " <<
// // participant->ipAddress
// // << ":" << participant->port << "[" <<
// // (int)participant->networkId
// // << "]\n";
// }
// }
// void ParticipantRegistry::Remove(Participant* participant) {
// // participants.remove(participant);
// }
// #if defined(NO_STD)
// Participant** ParticipantRegistry::GetAll() const {
// return ParticipantRegistry::participants;
// }
// #else
// const std::list<Participant*>& ParticipantRegistry::GetAll() const {
// return ParticipantRegistry::participants;
// }
// #endif
// #pragma endregion ParticipantRegistry
} // namespace RoboidControl } // namespace RoboidControl

55
Participant.h
View File

@ -7,55 +7,6 @@ namespace RoboidControl {
constexpr int MAX_THING_COUNT = 256; constexpr int MAX_THING_COUNT = 256;
/*
/// @brief class which manages all known participants
class ParticipantRegistry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
//Participant* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
Participant* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
//Participant* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(Participant* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(Participant* participant);
private:
#if defined(NO_STD)
public:
Participant** GetAll() const;
int count = 0;
private:
Participant** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<Participant*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<Participant*> participants;
#endif
};
*/
/// @brief A participant is a device which manages things. /// @brief A participant is a device which manages things.
/// It can communicate with other participant to synchronise the state of /// It can communicate with other participant to synchronise the state of
/// things. This class is used to register the things the participant is /// things. This class is used to register the things the participant is
@ -137,12 +88,6 @@ class Participant {
#pragma endregion Send #pragma endregion Send
// #pragma region Participant Registry
// public:
// static ParticipantRegistry registry;
// #pragma endregion Participant Registry
}; };
} // namespace RoboidControl } // namespace RoboidControl

189
Participants/ParticipantUDP.cpp
View File

@ -16,90 +16,6 @@
namespace RoboidControl { namespace RoboidControl {
#pragma region ParticipantRegistry
ParticipantRegistry ParticipantUDPGeneric::registry;
ParticipantSocket* ParticipantRegistry::Get(const char* ipAddress,
unsigned int port) {
#if !defined(NO_STD)
for (ParticipantSocket* participant : ParticipantRegistry::participants) {
if (participant == nullptr)
continue;
if (strcmp(participant->ipAddress, ipAddress) == 0 &&
participant->port == port) {
// std::cout << "found participant " << participant->ipAddress << ":"
// << (int)participant->port << std::endl;
return participant;
}
}
std::cout << "Could not find participant " << ipAddress << ":" << (int)port
<< std::endl;
#endif
return nullptr;
}
ParticipantSocket* ParticipantRegistry::Get(unsigned char participantId) {
#if !defined(NO_STD)
for (ParticipantSocket* participant : ParticipantRegistry::participants) {
if (participant == nullptr)
continue;
if (participant->networkId == participantId)
return participant;
}
std::cout << "Could not find participant " << (int)participantId << std::endl;
#endif
return nullptr;
}
ParticipantSocket* ParticipantRegistry::Add(const char* ipAddress,
unsigned int port) {
ParticipantSocket* participant = new ParticipantSocket(ipAddress, port);
Add(participant);
return participant;
}
void ParticipantRegistry::Add(ParticipantSocket* participant) {
Participant* foundParticipant = Get(participant->networkId);
// Get(participant->ipAddress, participant->port);
if (foundParticipant == nullptr) {
#if defined(NO_STD)
// this->things[this->thingCount++] = thing;
#else
ParticipantRegistry::participants.push_back(participant);
#endif
// std::cout << "Add participant " << participant->ipAddress << ":"
// << participant->port << "[" << (int)participant->networkId
// << "]\n";
// std::cout << "participants " <<
// ParticipantRegistry::participants.size()
// << "\n";
// } else {
// std::cout << "Did not add, existing participant " <<
// participant->ipAddress
// << ":" << participant->port << "[" <<
// (int)participant->networkId
// << "]\n";
}
}
void ParticipantRegistry::Remove(ParticipantSocket* participant) {
// participants.remove(participant);
}
#if defined(NO_STD)
RemoteParticipantUDP** ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#else
const std::list<ParticipantSocket*>& ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#endif
#pragma endregion ParticipantRegistry
bool ParticipantSocket::Send(IMessage* msg) { bool ParticipantSocket::Send(IMessage* msg) {
// No message is actually sent, because this class has no networking // No message is actually sent, because this class has no networking
// implementation // implementation
@ -179,7 +95,7 @@ void ParticipantUDPGeneric::Update(bool recurse) {
this->nextPublishMe = currentTimeMs + this->publishInterval; this->nextPublishMe = currentTimeMs + this->publishInterval;
} }
//this->ReceiveUDP(); this->ReceiveUDP();
} }
UpdateMyThings(); UpdateMyThings();
@ -191,7 +107,7 @@ void ParticipantUDPGeneric::UpdateMyThings() {
if (thing == nullptr) // || thing->GetParent() != nullptr) if (thing == nullptr) // || thing->GetParent() != nullptr)
continue; continue;
// Why don't we do recursive? // Why don't we do recursive?
// Because when a thing creates a thing in the update, // Because when a thing creates a thing in the update,
// that new thing is not sent out (because of hierarchyChanged) // that new thing is not sent out (because of hierarchyChanged)
// before it is updated itself: it is immediatedly updated! // before it is updated itself: it is immediatedly updated!
@ -294,8 +210,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
unsigned int senderPort) { unsigned int senderPort) {
// std::cout << "Receive data from " << senderIpAddress << ":" << senderPort // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
// << std::endl; // << std::endl;
ParticipantSocket* sender = ParticipantSocket* sender = this->registry.Get(senderIpAddress, senderPort);
this->registry.Get(senderIpAddress, senderPort);
if (sender == nullptr) { if (sender == nullptr) {
sender = this->registry.Add(senderIpAddress, senderPort); sender = this->registry.Add(senderIpAddress, senderPort);
#if !defined(NO_STD) #if !defined(NO_STD)
@ -413,8 +328,7 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
#endif #endif
} }
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, ThingMsg* msg) {
ThingMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
@ -459,8 +373,7 @@ Thing* ParticipantUDPGeneric::ProcessNewThing(ParticipantSocket* owner,
} }
} }
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, NameMsg* msg) {
NameMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/" std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "] "; << (int)msg->thingId << "] ";
@ -500,8 +413,7 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
#endif #endif
} }
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, PoseMsg* msg) {
PoseMsg* msg) {
#if !defined(DEBUG) && !defined(NO_STD) #if !defined(DEBUG) && !defined(NO_STD)
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
<< "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
@ -524,8 +436,7 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
thing->SetAngularVelocity(msg->angularVelocity); thing->SetAngularVelocity(msg->angularVelocity);
} }
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, BinaryMsg* msg) {
BinaryMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n"; << "/" << (int)msg->thingId << "]\n";
@ -547,8 +458,7 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
} }
} }
void ParticipantUDPGeneric::Process(ParticipantSocket* sender, void ParticipantUDPGeneric::Process(ParticipantSocket* sender, TextMsg* msg) {
TextMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " " std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
<< (int)msg->text << "\n"; << (int)msg->text << "\n";
@ -570,4 +480,87 @@ void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
// Receive // Receive
#pragma endregion #pragma endregion
#pragma region Registry
ParticipantUDP::Registry ParticipantUDPGeneric::registry;
ParticipantSocket* ParticipantUDP::Registry::Get(const char* ipAddress, unsigned int port) {
#if !defined(NO_STD)
for (ParticipantSocket* participant : Registry::participants) {
if (participant == nullptr)
continue;
if (strcmp(participant->ipAddress, ipAddress) == 0 &&
participant->port == port) {
// std::cout << "found participant " << participant->ipAddress << ":"
// << (int)participant->port << std::endl;
return participant;
}
}
std::cout << "Could not find participant " << ipAddress << ":" << (int)port
<< std::endl;
#endif
return nullptr;
}
ParticipantSocket* ParticipantUDP::Registry::Get(unsigned char participantId) {
#if !defined(NO_STD)
for (ParticipantSocket* participant : Registry::participants) {
if (participant == nullptr)
continue;
if (participant->networkId == participantId)
return participant;
}
std::cout << "Could not find participant " << (int)participantId << std::endl;
#endif
return nullptr;
}
ParticipantSocket* ParticipantUDP::Registry::Add(const char* ipAddress, unsigned int port) {
ParticipantSocket* participant = new ParticipantSocket(ipAddress, port);
Add(participant);
return participant;
}
void ParticipantUDP::Registry::Add(ParticipantSocket* participant) {
Participant* foundParticipant = Get(participant->networkId);
// Get(participant->ipAddress, participant->port);
if (foundParticipant == nullptr) {
#if defined(NO_STD)
// this->things[this->thingCount++] = thing;
#else
Registry::participants.push_back(participant);
#endif
// std::cout << "Add participant " << participant->ipAddress << ":"
// << participant->port << "[" << (int)participant->networkId
// << "]\n";
// std::cout << "participants " <<
// Registry::participants.size()
// << "\n";
// } else {
// std::cout << "Did not add, existing participant " <<
// participant->ipAddress
// << ":" << participant->port << "[" <<
// (int)participant->networkId
// << "]\n";
}
}
void ParticipantUDP::Registry::Remove(ParticipantSocket* participant) {
// participants.remove(participant);
}
#if defined(NO_STD)
RemoteParticipantUDP** Registry::GetAll() const {
return Registry::participants;
}
#else
const std::list<ParticipantSocket*>& ParticipantUDP::Registry::GetAll() const {
return Registry::participants;
}
#endif
// Registry
#pragma endregion Registry
} // namespace RoboidControl } // namespace RoboidControl

106
Participants/ParticipantUDP.h
View File

@ -32,55 +32,6 @@ namespace RoboidControl {
constexpr int MAX_SENDER_COUNT = 256; constexpr int MAX_SENDER_COUNT = 256;
/// @brief class which manages all known participants
class ParticipantRegistry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
ParticipantSocket* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
ParticipantSocket* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
ParticipantSocket* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(ParticipantSocket* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(ParticipantSocket* participant);
private:
#if defined(NO_STD)
public:
ParticipantSocket** GetAll() const;
int count = 0;
private:
ParticipantSocket** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<ParticipantSocket*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<ParticipantSocket*> participants;
#endif
};
/// @brief A participant using UDP communication /// @brief A participant using UDP communication
/// A local participant is the local device which can communicate with /// A local participant is the local device which can communicate with
/// other participants It manages all local things and communcation with other /// other participants It manages all local things and communcation with other
@ -174,9 +125,12 @@ class ParticipantUDPGeneric : public ParticipantSocket {
void ReceiveData(unsigned char bufferSize, void ReceiveData(unsigned char bufferSize,
char* senderIpAddress, char* senderIpAddress,
unsigned int senderPort); unsigned int senderPort);
void ReceiveData(unsigned char bufferSize, ParticipantSocket* remoteParticipant); void ReceiveData(unsigned char bufferSize,
ParticipantSocket* remoteParticipant);
virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0; virtual void SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) = 0;
virtual void ReceiveUDP() = 0; virtual void ReceiveUDP() = 0;
@ -198,9 +152,55 @@ class ParticipantUDPGeneric : public ParticipantSocket {
#pragma endregion Receive #pragma endregion Receive
public: public:
static ParticipantRegistry registry; /// @brief class which manages all known participants
class Registry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
ParticipantSocket* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
ParticipantSocket* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
ParticipantSocket* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(ParticipantSocket* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(ParticipantSocket* participant);
private:
#if defined(NO_STD)
public:
ParticipantSocket** GetAll() const;
int count = 0;
private:
ParticipantSocket** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<ParticipantSocket*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<ParticipantSocket*> participants;
#endif
};
static Registry registry;
}; };
} // namespace RoboidControl } // namespace RoboidControl