387 lines
12 KiB
C++
387 lines
12 KiB
C++
#include "ParticipantUDP.h"
|
|
|
|
#include "Thing.h"
|
|
|
|
#include "Arduino/ArduinoParticipant.h"
|
|
#include "EspIdf/EspIdfParticipant.h"
|
|
|
|
#if defined(_WIN32) || defined(_WIN64)
|
|
#include <winsock2.h>
|
|
#include <ws2tcpip.h>
|
|
#include "Windows/WindowsParticipant.h"
|
|
#pragma comment(lib, "ws2_32.lib")
|
|
|
|
#elif defined(__unix__) || defined(__APPLE__)
|
|
#include <arpa/inet.h>
|
|
#include <fcntl.h> // For fcntl
|
|
#include <netinet/in.h>
|
|
#include <sys/socket.h>
|
|
#include <unistd.h>
|
|
#include <chrono>
|
|
#include "Posix/PosixParticipant.h"
|
|
#endif
|
|
|
|
#include <string.h>
|
|
|
|
namespace RoboidControl {
|
|
|
|
ParticipantUDP::ParticipantUDP(int port) {
|
|
this->ipAddress = "0.0.0.0";
|
|
this->port = port;
|
|
if (this->port == 0)
|
|
this->isIsolated = true;
|
|
}
|
|
|
|
ParticipantUDP::ParticipantUDP(const char* ipAddress,
|
|
int port,
|
|
int localPort)
|
|
: Participant("127.0.0.1", localPort) {
|
|
if (this->port == 0)
|
|
this->isIsolated = true;
|
|
else
|
|
this->remoteSite = new Participant(ipAddress, port);
|
|
}
|
|
|
|
static ParticipantUDP* isolatedParticipant = nullptr;
|
|
|
|
ParticipantUDP* ParticipantUDP::Isolated() {
|
|
if (isolatedParticipant == nullptr)
|
|
isolatedParticipant = new ParticipantUDP(0);
|
|
return isolatedParticipant;
|
|
}
|
|
|
|
void ParticipantUDP::begin() {
|
|
if (this->isIsolated)
|
|
return;
|
|
|
|
SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
|
|
}
|
|
|
|
void ParticipantUDP::SetupUDP(int localPort,
|
|
const char* remoteIpAddress,
|
|
int remotePort) {
|
|
#if defined(_WIN32) || defined(_WIN64)
|
|
Windows::ParticipantUDP* thisWindows =
|
|
static_cast<Windows::ParticipantUDP*>(this);
|
|
thisWindows->Setup(localPort, remoteIpAddress, remotePort);
|
|
#elif defined(__unix__) || defined(__APPLE__)
|
|
Posix::ParticipantUDP* thisPosix =
|
|
static_cast<Posix::ParticipantUDP*>(this);
|
|
thisPosix->Setup(localPort, remoteIpAddress, remotePort);
|
|
#elif defined(ARDUINO)
|
|
Arduino::ParticipantUDP* thisArduino =
|
|
static_cast<Arduino::ParticipantUDP*>(this);
|
|
thisArduino->Setup(localPort, remoteIpAddress, remotePort);
|
|
#elif defined(IDF_VER)
|
|
EspIdf::ParticipantUDP* thisEspIdf =
|
|
static_cast<EspIdf::ParticipantUDP*>(this);
|
|
thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
|
|
#endif
|
|
this->connected = true;
|
|
}
|
|
|
|
void ParticipantUDP::Update(unsigned long currentTimeMs) {
|
|
if (currentTimeMs == 0)
|
|
currentTimeMs = Thing::GetTimeMs();
|
|
|
|
if (this->isIsolated == false) {
|
|
if (this->connected == false)
|
|
begin();
|
|
|
|
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
|
|
ParticipantMsg* msg = new ParticipantMsg(this->networkId);
|
|
if (this->remoteSite == nullptr)
|
|
this->Publish(msg);
|
|
else
|
|
this->Send(this->remoteSite, msg);
|
|
delete msg;
|
|
|
|
this->nextPublishMe = currentTimeMs + this->publishInterval;
|
|
}
|
|
|
|
this->ReceiveUDP();
|
|
}
|
|
|
|
for (Thing* thing : this->things) {
|
|
if (thing != nullptr) {
|
|
thing->Update(currentTimeMs);
|
|
if (this->isIsolated == false) {
|
|
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
|
|
for (Participant* sender : this->senders)
|
|
this->Send(sender, poseMsg);
|
|
delete poseMsg;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void ParticipantUDP::ReceiveUDP() {
|
|
#if defined(_WIN32) || defined(_WIN64)
|
|
Windows::ParticipantUDP* thisWindows =
|
|
static_cast<Windows::ParticipantUDP*>(this);
|
|
thisWindows->Receive();
|
|
#elif defined(__unix__) || defined(__APPLE__)
|
|
Posix::ParticipantUDP* thisPosix =
|
|
static_cast<Posix::ParticipantUDP*>(this);
|
|
thisPosix->Receive();
|
|
#elif defined(ARDUINO)
|
|
Arduino::ParticipantUDP* thisArduino =
|
|
static_cast<Arduino::ParticipantUDP*>(this);
|
|
thisArduino->Receive();
|
|
#elif defined(IDF_VER)
|
|
EspIdf::ParticipantUDP* thisEspIdf =
|
|
static_cast<EspIdf::ParticipantUDP*>(this);
|
|
thisEspIdf->Receive();
|
|
#endif
|
|
}
|
|
|
|
Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) {
|
|
for (Participant* sender : this->senders) {
|
|
if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
|
|
return sender;
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
|
|
// std::cout << "New Participant " << ipAddress << ":" << port << "\n";
|
|
Participant* participant = new Participant(ipAddress, port);
|
|
#if defined(NO_STD)
|
|
participant->networkId = this->senderCount;
|
|
this->senders[this->senderCount++] = participant;
|
|
#else
|
|
participant->networkId = (unsigned char)this->senders.size();
|
|
this->senders.push_back(participant);
|
|
#endif
|
|
return participant;
|
|
}
|
|
|
|
#pragma region Send
|
|
|
|
void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
|
|
Thing* thing) {
|
|
// std::cout << "Send thing info " << (int)thing->id << " \n";
|
|
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
|
this->Send(remoteParticipant, thingMsg);
|
|
delete thingMsg;
|
|
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
|
this->Send(remoteParticipant, nameMsg);
|
|
delete nameMsg;
|
|
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
|
|
this->Send(remoteParticipant, modelMsg);
|
|
delete modelMsg;
|
|
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
|
|
this->Send(remoteParticipant, poseMsg);
|
|
delete poseMsg;
|
|
BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
|
|
this->Send(remoteParticipant, customMsg);
|
|
delete customMsg;
|
|
}
|
|
|
|
bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
|
|
int bufferSize = msg->Serialize(this->buffer);
|
|
if (bufferSize <= 0)
|
|
return true;
|
|
|
|
#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(remoteParticipant, bufferSize);
|
|
#elif defined(IDF_VER)
|
|
EspIdf::ParticipantUDP* thisEspIdf =
|
|
static_cast<EspIdf::ParticipantUDP*>(this);
|
|
return thisEspIdf->Send(remoteParticipant, bufferSize);
|
|
#else
|
|
return false;
|
|
#endif
|
|
}
|
|
|
|
void ParticipantUDP::PublishThingInfo(Thing* thing) {
|
|
// std::cout << "Publish thing info" << thing->networkId << "\n";
|
|
// Strange, when publishing, the network id is irrelevant, because it is
|
|
// connected to a specific site...
|
|
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
|
this->Publish(thingMsg);
|
|
delete thingMsg;
|
|
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
|
this->Publish(nameMsg);
|
|
delete nameMsg;
|
|
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
|
|
this->Publish(modelMsg);
|
|
delete modelMsg;
|
|
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
|
|
this->Publish(poseMsg);
|
|
delete poseMsg;
|
|
BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
|
|
this->Publish(customMsg);
|
|
delete customMsg;
|
|
}
|
|
|
|
bool ParticipantUDP::Publish(IMessage* msg) {
|
|
#if defined(_WIN32) || defined(_WIN64)
|
|
Windows::ParticipantUDP* thisWindows =
|
|
static_cast<Windows::ParticipantUDP*>(this);
|
|
return thisWindows->Publish(msg);
|
|
#elif defined(__unix__) || defined(__APPLE__)
|
|
Posix::ParticipantUDP* thisPosix =
|
|
static_cast<Posix::ParticipantUDP*>(this);
|
|
return thisPosix->Publish(msg);
|
|
#elif defined(ARDUINO)
|
|
Arduino::ParticipantUDP* thisArduino =
|
|
static_cast<Arduino::ParticipantUDP*>(this);
|
|
return thisArduino->Publish(msg);
|
|
#elif defined(IDF_VER)
|
|
EspIdf::ParticipantUDP* thisEspIdf =
|
|
static_cast<EspIdf::ParticipantUDP*>(this);
|
|
return thisEspIdf->Publish(msg);
|
|
#else
|
|
return false;
|
|
#endif
|
|
}
|
|
|
|
// Send
|
|
#pragma endregion
|
|
|
|
#pragma region Receive
|
|
|
|
void ParticipantUDP::ReceiveData(unsigned char packetSize,
|
|
char* senderIpAddress,
|
|
unsigned int senderPort) {
|
|
Participant* remoteParticipant =
|
|
this->GetParticipant(senderIpAddress, senderPort);
|
|
if (remoteParticipant == nullptr) {
|
|
remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
|
|
// std::cout << "New sender " << senderIpAddress << ":" << senderPort
|
|
// << "\n";
|
|
std::cout << "New remote participant " << remoteParticipant->ipAddress
|
|
<< ":" << remoteParticipant->port << " "
|
|
<< (int)remoteParticipant->networkId << "\n";
|
|
}
|
|
|
|
ReceiveData(packetSize, remoteParticipant);
|
|
}
|
|
|
|
void ParticipantUDP::ReceiveData(unsigned char bufferSize,
|
|
Participant* remoteParticipant) {
|
|
unsigned char msgId = this->buffer[0];
|
|
// std::cout << "receive msg " << (int)msgId << "\n";
|
|
switch (msgId) {
|
|
case ParticipantMsg::id: {
|
|
ParticipantMsg* msg = new ParticipantMsg(this->buffer);
|
|
Process(remoteParticipant, msg);
|
|
delete msg;
|
|
} break;
|
|
case SiteMsg::id: {
|
|
SiteMsg* msg = new SiteMsg(this->buffer);
|
|
Process(remoteParticipant, msg);
|
|
delete msg;
|
|
} break;
|
|
case InvestigateMsg::id: {
|
|
InvestigateMsg* msg = new InvestigateMsg(this->buffer);
|
|
Process(remoteParticipant, msg);
|
|
delete msg;
|
|
} break;
|
|
case ThingMsg::id: {
|
|
ThingMsg* msg = new ThingMsg(this->buffer);
|
|
Process(remoteParticipant, msg);
|
|
delete msg;
|
|
} break;
|
|
case NameMsg::id: {
|
|
NameMsg* msg = new NameMsg(this->buffer);
|
|
Process(remoteParticipant, msg);
|
|
delete msg;
|
|
} break;
|
|
case PoseMsg::id: {
|
|
PoseMsg* msg = new PoseMsg(this->buffer);
|
|
Process(remoteParticipant, msg);
|
|
delete msg;
|
|
} break;
|
|
case BinaryMsg::id: {
|
|
BinaryMsg* msg = new BinaryMsg(this->buffer);
|
|
Process(remoteParticipant, msg);
|
|
delete msg;
|
|
} break;
|
|
};
|
|
}
|
|
|
|
void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {}
|
|
|
|
void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
|
|
std::cout << this->name << ": process Site Id " << (int)this->networkId
|
|
<< "->" << (int)msg->networkId << "\n";
|
|
if (this->networkId != msg->networkId) {
|
|
this->networkId = msg->networkId;
|
|
// std::cout << this->things.size() << " things\n";
|
|
for (Thing* thing : this->things)
|
|
this->SendThingInfo(sender, thing);
|
|
}
|
|
}
|
|
|
|
void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {}
|
|
|
|
void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
|
|
std::cout << this->name << ": process Thing [" << (int)msg->networkId << "/"
|
|
<< (int)msg->thingId << "]\n";
|
|
}
|
|
|
|
void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
|
|
std::cout << this->name << ": process Name [" << (int)msg->networkId << "/"
|
|
<< (int)msg->thingId << "]\n";
|
|
|
|
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
|
if (thing != nullptr) {
|
|
int nameLength = msg->nameLength;
|
|
int stringLen = nameLength + 1;
|
|
char* thingName = new char[stringLen];
|
|
#if defined(_WIN32) || defined(_WIN64)
|
|
strncpy_s(thingName, stringLen, msg->name,
|
|
stringLen - 1); // Leave space for null terminator
|
|
#else
|
|
// Use strncpy with bounds checking for other platforms (Arduino, POSIX,
|
|
// ESP-IDF)
|
|
strncpy(thingName, msg->name,
|
|
stringLen - 1); // Leave space for null terminator
|
|
thingName[stringLen - 1] = '\0'; // Ensure null termination
|
|
#endif
|
|
thingName[nameLength] = '\0';
|
|
thing->name = thingName;
|
|
// std::cout << "thing name = " << thing->name << " length = " <<
|
|
// nameLength
|
|
// << "\n";
|
|
}
|
|
}
|
|
|
|
void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
|
|
std::cout << this->name << ": process Pose [" << (int)this->networkId << "/"
|
|
<< (int)msg->networkId << "]\n";
|
|
}
|
|
|
|
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
|
|
std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/"
|
|
<< (int)msg->thingId << "]\n";
|
|
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
|
if (thing != nullptr)
|
|
thing->ProcessBinary(msg->bytes);
|
|
else {
|
|
thing = this->Get(msg->networkId, msg->thingId);
|
|
if (thing != nullptr)
|
|
thing->ProcessBinary(msg->bytes);
|
|
else
|
|
std::cout << "custom msg for unknown thing [" << (int)msg->networkId
|
|
<< "/" << (int)msg->thingId << "]\n";
|
|
}
|
|
}
|
|
|
|
// Receive
|
|
#pragma endregion
|
|
|
|
} // namespace RoboidControl
|