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

Aliged Participants

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-30 17:00:01 +02:00
parent 4db9164f2a
commit db1265a135
7 changed files with 104 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
Messages/NetworkIdMsg.cpp Normal file
View File

@ -0,0 +1,25 @@
#include "NetworkIdMsg.h"
namespace RoboidControl {
NetworkIdMsg::NetworkIdMsg(const char* buffer) {
this->networkId = buffer[1];
}
NetworkIdMsg::NetworkIdMsg(unsigned char networkId) {
this->networkId = networkId;
}
NetworkIdMsg::~NetworkIdMsg() {}
unsigned char NetworkIdMsg::Serialize(char* buffer) {
#if defined(DEBUG)
std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl;
#endif
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
return NetworkIdMsg::length;
}
} // namespace RoboidControl

8
Messages/SiteMsg.h → Messages/NetworkIdMsg.h
View File

@ -3,7 +3,7 @@
namespace RoboidControl {
/// @brief A message communicating the network ID for that participant
class SiteMsg : public IMessage {
class NetworkIdMsg : public IMessage {
public:
/// @brief The message ID
static const unsigned char id = 0xA1;
@ -14,11 +14,11 @@ public:
/// @brief Create a new message for sending
/// @param networkId The network ID for the participant
SiteMsg(unsigned char networkId);
NetworkIdMsg(unsigned char networkId);
/// @copydoc RoboidControl::IMessage::IMessage(char*)
SiteMsg(const char *buffer);
NetworkIdMsg(const char *buffer);
/// @brief Destructor for the message
virtual ~SiteMsg();
virtual ~NetworkIdMsg();
/// @copydoc RoboidControl::IMessage::Serialize
virtual unsigned char Serialize(char *buffer) override;

25
Messages/SiteMsg.cpp
View File

@ -1,25 +0,0 @@
#include "SiteMsg.h"
namespace RoboidControl {
SiteMsg::SiteMsg(const char* buffer) {
this->networkId = buffer[1];
}
SiteMsg::SiteMsg(unsigned char networkId) {
this->networkId = networkId;
}
SiteMsg::~SiteMsg() {}
unsigned char SiteMsg::Serialize(char* buffer) {
#if defined(DEBUG)
std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl;
#endif
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
return SiteMsg::length;
}
} // namespace RoboidControl

17
Participants/ParticipantUDP.cpp
View File

@ -325,8 +325,8 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
Process(sender, msg);
delete msg;
} break;
case SiteMsg::id: {
SiteMsg* msg = new SiteMsg(this->buffer);
case NetworkIdMsg::id: {
NetworkIdMsg* msg = new NetworkIdMsg(this->buffer);
bufferSize -= msg->length;
Process(sender, msg);
delete msg;
@ -382,7 +382,7 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
#endif
}
void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process SiteMsg " << (int)this->networkId
<< " -> " << (int)msg->networkId << "\n";
@ -452,9 +452,9 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
}
void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
#if defined(DEBUG)
#if !defined(DEBUG)
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
<< "/" << (int)msg->networkId << "]\n";
<< "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
#endif
Participant* owner = Participant::GetParticipant(msg->networkId);
if (owner == nullptr)
@ -466,7 +466,12 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
if ((msg->poseType & PoseMsg::Pose_Position) != 0)
thing->SetPosition(msg->position);
std::cout << "update position for" << (int)thing->id << std::endl;
if ((msg->poseType & PoseMsg::Pose_Orientation) != 0)
thing->SetOrientation(msg->orientation);
if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0)
thing->SetLinearVelocity(msg->linearVelocity);
if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0)
thing->SetAngularVelocity(msg->angularVelocity);
}
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {

102
Participants/ParticipantUDP.h
View File

@ -1,13 +1,13 @@
#pragma once
#include "Messages/BinaryMsg.h"
#include "Messages/InvestigateMsg.h"
#include "Messages/DestroyMsg.h"
#include "Messages/InvestigateMsg.h"
#include "Messages/ModelUrlMsg.h"
#include "Messages/NameMsg.h"
#include "Messages/ParticipantMsg.h"
#include "Messages/PoseMsg.h"
#include "Messages/SiteMsg.h"
#include "Messages/NetworkIdMsg.h"
#include "Messages/ThingMsg.h"
#include "Participant.h"
@ -30,7 +30,8 @@ namespace RoboidControl {
constexpr int MAX_SENDER_COUNT = 256;
/// @brief A local participant is the local device which can communicate with
/// @brief A participant using UDP communication
/// A local participant is the local device which can communicate with
/// other participants It manages all local things and communcation with other
/// participants. Each application has a local participant which is usually
/// explicit in the code. An participant can be isolated. In that case it is
@ -42,6 +43,8 @@ constexpr int MAX_SENDER_COUNT = 256;
/// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing()
class ParticipantUDP : public Participant {
#pragma region Init
public:
/// @brief Create a participant without connecting to a site
/// @param port The port on which the participant communicates
@ -52,11 +55,8 @@ class ParticipantUDP : public Participant {
/// @brief Create a participant which will try to connect to a site.
/// @param ipAddress The IP address of the site
/// @param port The port used by the site
ParticipantUDP(const char* ipAddress,
int port = 7681,
int localPort = 7681);
// Note to self: one cannot specify the port used by the local participant
// now!!
/// @param localPort The port used by the local participant
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
/// @brief Isolated participant is used when the application is run without
/// networking
@ -65,7 +65,14 @@ class ParticipantUDP : public Participant {
/// @brief True if the participant is running isolated.
/// Isolated participants do not communicate with other participants
#pragma endregion Init
/// @brief True if the participant is running isolated.
/// Isolated participants do not communicate with other participants
bool isIsolated = false;
/// @brief The remote site when this participant is connected to a site
Participant* remoteSite = nullptr;
/// The interval in milliseconds for publishing (broadcasting) data on the
/// local network
@ -74,19 +81,10 @@ class ParticipantUDP : public Participant {
/// @brief The name of the participant
const char* name = "ParticipantUDP";
// int localPort = 0;
/// @brief The remote site when this participant is connected to a site
Participant* remoteSite = nullptr;
#if defined(ARDUINO)
// const char* remoteIpAddress = nullptr;
// unsigned short remotePort = 0;
// char* broadcastIpAddress = nullptr;
// WiFiUDP udp;
#else
protected:
char buffer[1024];
#if !defined(ARDUINO)
#if defined(__unix__) || defined(__APPLE__)
int sock;
#elif defined(_WIN32) || defined(_WIN64)
@ -94,48 +92,48 @@ class ParticipantUDP : public Participant {
sockaddr_in server_addr;
sockaddr_in broadcast_addr;
#endif
#endif
public:
void begin();
bool connected = false;
#pragma region Update
public:
virtual void Update(unsigned long currentTimeMs = 0) override;
protected:
unsigned long nextPublishMe = 0;
virtual void UpdateMyThings(unsigned long currentTimeMs);
virtual void UpdateOtherThings(unsigned long currentTimeMs);
#pragma endregion Update
#pragma region Send
void SendThingInfo(Participant* remoteParticipant, Thing* thing);
void PublishThingInfo(Thing* thing);
bool Send(Participant* remoteParticipant, IMessage* msg);
bool Publish(IMessage* msg);
#pragma endregion Send
#pragma region Receive
protected:
void ReceiveData(unsigned char bufferSize,
char* senderIpAddress,
unsigned int senderPort);
void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
// #if defined(NO_STD)
// unsigned char senderCount = 0;
// Participant* senders[MAX_SENDER_COUNT];
// #else
// std::list<Participant*> senders;
// #endif
protected:
unsigned long nextPublishMe = 0;
char buffer[1024];
void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
//Participant* GetParticipant(const char* ipAddress, int port);
// Participant* AddParticipant(const char* ipAddress, int port);
void ReceiveUDP();
virtual void Process(Participant* sender, ParticipantMsg* msg);
virtual void Process(Participant* sender, SiteMsg* msg);
virtual void Process(Participant* sender, NetworkIdMsg* msg);
virtual void Process(Participant* sender, InvestigateMsg* msg);
virtual void Process(Participant* sender, ThingMsg* msg);
virtual void Process(Participant* sender, NameMsg* msg);
@ -143,34 +141,8 @@ class ParticipantUDP : public Participant {
virtual void Process(Participant* sender, PoseMsg* msg);
virtual void Process(Participant* sender, BinaryMsg* msg);
#if !defined(NO_STD)
// public:
// using ThingConstructor = std::function<Thing*(Participant* participant,
// unsigned char networkId,
// unsigned char thingId)>;
#pragma endregion Receive
// template <typename ThingClass>
// void Register(unsigned char thingType) {
// thingMsgProcessors[thingType] = [](Participant* participant,
// unsigned char networkId,
// unsigned char thingId) {
// return new ThingClass(participant, networkId, thingId);
// };
// };
// template <typename ThingClass>
// void Register2(unsigned char thingType, ThingConstructor f) {
// thingMsgProcessors[thingType] = [f](Participant* participant,
// unsigned char networkId,
// unsigned char thingId) {
// return f(participant, networkId, thingId);
// };
// };
// protected:
// std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
#endif
};
} // namespace RoboidControl

7
Participants/SiteServer.cpp
View File

@ -58,17 +58,17 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
// std::cout << this->name << " received New Client -> " <<
// sender->ipAddress
// << ":" << (int)sender->port << "\n";
SiteMsg* msg = new SiteMsg(sender->networkId);
NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
this->Send(sender, msg);
delete msg;
}
}
void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(Participant* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->thingId);
if (thing == nullptr) {
if (thing == nullptr)
new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
if (msg->parentId != 0) {
@ -78,7 +78,6 @@ void SiteServer::Process(Participant* sender, ThingMsg* msg) {
<< (int)msg->parentId << "]\n";
} else
thing->SetParent(nullptr);
}
}
#pragma endregion Receive

18
Participants/SiteServer.h
View File

@ -12,17 +12,33 @@ namespace RoboidControl {
/// @brief A participant is device which can communicate with other participants
class SiteServer : public ParticipantUDP {
#pragma region Init
public:
/// @brief Create a new site server
/// @param port The port of which to receive the messages
SiteServer(int port = 7681);
#pragma endregion Init
#pragma region Update
virtual void UpdateMyThings(unsigned long currentTimeMs) override;
#pragma endregion Update
#pragma region Receive
protected:
unsigned long nextPublishMe = 0;
virtual void Process(Participant* sender, ParticipantMsg* msg) override;
virtual void Process(Participant* sender, SiteMsg* msg) override;
virtual void Process(Participant* sender, NetworkIdMsg* msg) override;
virtual void Process(Participant* sender, ThingMsg* msg) override;
#pragma endregion Receive
};
} // namespace RoboidControl