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

Created specific support folders

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-21 14:40:51 +01:00
parent 5aa541cbae
commit f51ef240db
16 changed files with 245 additions and 234 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
ArduinoUtils.cpp → Arduino/ArduinoUtils.cpp
View File

0
ArduinoUtils.h → Arduino/ArduinoUtils.h
View File

23
UdpArduino.cpp → Arduino/UdpArduino.cpp
View File

@ -5,9 +5,9 @@
#endif
namespace Passer {
namespace RoboidControl {
namespace Arduino {
void UdpArduino::Setup(int localPort, const char *remoteIpAddress,
int remotePort) {
void Participant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
#if ARDUINO
this->remoteIpAddress = remoteIpAddress;
this->remotePort = remotePort;
@ -23,19 +23,18 @@ void UdpArduino::Setup(int localPort, const char *remoteIpAddress,
#endif
}
void UdpArduino::GetBroadcastAddress() {
void Participant::GetBroadcastAddress() {
#if ARDUINO
IPAddress broadcastAddress = WiFi.localIP();
broadcastAddress[3] = 255;
String broadcastIpString = broadcastAddress.toString();
this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
broadcastIpString.toCharArray(this->broadcastIpAddress,
broadcastIpString.length() + 1);
broadcastIpString.toCharArray(this->broadcastIpAddress, broadcastIpString.length() + 1);
std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
#endif
}
void UdpArduino::Receive() {
void Participant::Receive() {
#if ARDUINO
int packetSize = udp.parsePacket();
while (packetSize > 0) {
@ -46,8 +45,7 @@ void UdpArduino::Receive() {
senderAddress.toCharArray(sender_ipAddress, 16);
int sender_port = udp.remotePort();
Participant *remoteParticipant =
this->GetParticipant(sender_ipAddress, sender_port);
Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
if (remoteParticipant == nullptr) {
remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
// std::cout << "New sender " << sender_ipAddress << ":" << sender_port
@ -63,7 +61,7 @@ void UdpArduino::Receive() {
#endif
}
bool UdpArduino::Send(RemoteParticipant* remoteParticipant, int bufferSize) {
bool Participant::Send(RemoteParticipant* remoteParticipant, int bufferSize) {
#if ARDUINO
udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
udp.write(buffer, bufferSize);
@ -75,7 +73,7 @@ bool UdpArduino::Send(RemoteParticipant* remoteParticipant, int bufferSize) {
return true;
}
bool UdpArduino::Publish(IMessage *msg) {
bool Participant::Publish(IMessage* msg) {
#ifdef ARDUINO
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
@ -91,5 +89,6 @@ bool UdpArduino::Publish(IMessage *msg) {
return true;
};
} // namespace Control
} // namespace Passer
} // namespace Arduino
} // namespace RoboidControl
} // namespace Passer

22
Arduino/UdpArduino.h Normal file
View File

@ -0,0 +1,22 @@
#pragma once
#include "Participant.h"
namespace Passer {
namespace RoboidControl {
namespace Arduino {
class Participant : public RoboidControl::Participant {
public:
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive();
bool Send(RemoteParticipant* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
protected:
void GetBroadcastAddress();
};
} // namespace Arduino
} // namespace RoboidControl
} // namespace Passer

9
CMakeLists.txt
View File

@ -27,7 +27,14 @@ else()
.
LinearAlgebra
)
file(GLOB srcs *.cpp Sensors/*.cpp Messages/*.cpp)
file(GLOB srcs
*.cpp
Sensors/*.cpp
Messages/*.cpp
Arduino/*.cpp
Posix/*.cpp
Windows/*.cpp
)
add_library(ControlCore STATIC ${srcs})
enable_testing()

190
Participant.cpp
View File

@ -2,9 +2,9 @@
#include "Thing.h"
#include "UdpArduino.h"
#include "UdpPosix.h"
#include "UdpWindows.h"
#include "Arduino/UdpArduino.h"
#include "Posix/Participant.h"
#include "Windows/Participant.h"
#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
@ -12,11 +12,11 @@
#pragma comment(lib, "ws2_32.lib")
#elif defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <chrono>
#include <fcntl.h> // For fcntl
#include <fcntl.h> // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#include <chrono>
#endif
namespace Passer {
@ -35,14 +35,14 @@ Participant::Participant(int port) {
// SetupUDP(randomPort, ipAddress, port);
}
Participant::Participant(const char *ipAddress, int port) {
Participant::Participant(const char* ipAddress, int port) {
this->ipAddress = ipAddress;
this->port = port;
this->senders.push_back(this);
// int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
this->localPort = port; // randomPort;
this->localPort = port; // randomPort;
// SetupUDP(randomPort, ipAddress, port);
}
@ -50,16 +50,15 @@ void Participant::begin() {
SetupUDP(this->localPort, this->ipAddress, this->port);
}
void Participant::SetupUDP(int localPort, const char *remoteIpAddress,
int remotePort) {
void Participant::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) {
#if defined(_WIN32) || defined(_WIN64)
UdpWindows *thisWindows = static_cast<UdpWindows *>(this);
Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);
thisWindows->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(__unix__) || defined(__APPLE__)
UdpPosix *thisPosix = static_cast<UdpPosix *>(this);
Posix::Participant* thisPosix = static_cast<Posix::Participant*>(this);
thisPosix->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(ARDUINO)
UdpArduino *thisArduino = static_cast<UdpArduino *>(this);
Arduino::Participant* thisArduino = static_cast<Arduino::Participant*>(this);
thisArduino->Setup(localPort, remoteIpAddress, remotePort);
#endif
this->connected = true;
@ -71,8 +70,7 @@ void Participant::Update(unsigned long currentTimeMs) {
currentTimeMs = millis();
#elif defined(__unix__) || defined(__APPLE__)
auto now = std::chrono::steady_clock::now();
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
now.time_since_epoch());
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
currentTimeMs = static_cast<unsigned long>(ms.count());
#endif
}
@ -81,7 +79,7 @@ void Participant::Update(unsigned long currentTimeMs) {
begin();
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
ClientMsg *msg = new ClientMsg(this->networkId);
ClientMsg* msg = new ClientMsg(this->networkId);
this->Publish(msg);
delete msg;
std::cout << this->name << " published ClientMsg\n";
@ -89,33 +87,36 @@ void Participant::Update(unsigned long currentTimeMs) {
}
this->ReceiveUDP();
this->UpdateAll(currentTimeMs);
for (Thing* thing : this->things) {
if (thing != nullptr) //} && thing->GetParent() == nullptr) // update all root things
thing->Update(currentTimeMs);
}
}
void Participant::ReceiveUDP() {
#if defined(_WIN32) || defined(_WIN64)
UdpWindows *thisWindows = static_cast<UdpWindows *>(this);
Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);
thisWindows->Receive();
#elif defined(__unix__) || defined(__APPLE__)
UdpPosix *thisPosix = static_cast<UdpPosix *>(this);
Posix::Participant* thisPosix = static_cast<Posix::Participant*>(this);
thisPosix->Receive();
#elif defined(ARDUINO)
UdpArduino *thisArduino = static_cast<UdpArduino *>(this);
Arduino::Participant* thisArduino = static_cast<Arduino::Participant*>(this);
thisArduino->Receive();
#endif
}
Participant *Participant::GetParticipant(const char *ipAddress, int port) {
for (Participant *sender : this->senders) {
Participant* Participant::GetParticipant(const char* ipAddress, int port) {
for (Participant* sender : this->senders) {
if (sender->ipAddress == ipAddress && sender->port == port)
return sender;
}
return nullptr;
}
Participant *Participant::AddParticipant(const char *ipAddress, int port) {
Participant* Participant::AddParticipant(const char* ipAddress, int port) {
std::cout << "New Participant " << ipAddress << ":" << port << "\n";
Participant *participant = new Participant(ipAddress, port);
Participant* participant = new Participant(ipAddress, port);
participant->networkId = (unsigned char)this->senders.size();
this->senders.push_back(participant);
return participant;
@ -123,64 +124,63 @@ Participant *Participant::AddParticipant(const char *ipAddress, int port) {
#pragma region Send
void Participant::SendThingInfo(RemoteParticipant *remoteParticipant,
Thing *thing) {
void Participant::SendThingInfo(RemoteParticipant* remoteParticipant, Thing* thing) {
std::cout << "Send thing info\n";
ThingMsg *thingMsg = new ThingMsg(this->networkId, thing);
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
this->Send(remoteParticipant, thingMsg);
delete thingMsg;
NameMsg *nameMsg = new NameMsg(this->networkId, thing);
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
this->Send(remoteParticipant, nameMsg);
delete nameMsg;
ModelUrlMsg *modelMsg = new ModelUrlMsg(this->networkId, thing);
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
this->Send(remoteParticipant, modelMsg);
delete modelMsg;
}
void Passer::RoboidControl::Participant::PublishThingInfo(Thing *thing) {
void Passer::RoboidControl::Participant::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);
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
this->Publish(thingMsg);
delete thingMsg;
NameMsg *nameMsg = new NameMsg(this->networkId, thing);
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
this->Publish(nameMsg);
delete nameMsg;
ModelUrlMsg *modelMsg = new ModelUrlMsg(this->networkId, thing);
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
this->Publish(modelMsg);
delete modelMsg;
CustomMsg *customMsg = new CustomMsg(this->networkId, thing);
CustomMsg* customMsg = new CustomMsg(this->networkId, thing);
this->Publish(customMsg);
delete customMsg;
}
bool Participant::Send(RemoteParticipant *remoteParticipant, IMessage *msg) {
bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
return true;
#if defined(_WIN32) || defined(_WIN64)
UdpWindows *thisWindows = static_cast<UdpWindows *>(this);
Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);
return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
UdpPosix *thisPosix = static_cast<UdpPosix *>(this);
Posix::Participant* thisPosix = static_cast<Posix::Particiapant*>(this);
return thisPosix->Send(remoteParticipant, bufferSize);
#elif defined(ARDUINO)
UdpArduino *thisArduino = static_cast<UdpArduino *>(this);
Arduino::Participant* thisArduino = static_cast<Arduino::Participant*>(this);
return thisArduino->Send(remoteParticipant, bufferSize);
#endif
}
bool Participant::Publish(IMessage *msg) {
bool Participant::Publish(IMessage* msg) {
#if defined(_WIN32) || defined(_WIN64)
UdpWindows *thisWindows = static_cast<UdpWindows *>(this);
Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);
return thisWindows->Publish(msg);
#elif defined(__unix__) || defined(__APPLE__)
UdpPosix *thisPosix = static_cast<UdpPosix *>(this);
Posix::Participant* thisPosix = static_cast<Posix::Participant*>(this);
return thisPosix->Publish(msg);
#elif defined(ARDUINO)
UdpArduino *thisArduino = static_cast<UdpArduino *>(this);
Arduino::Participant* thisArduino = static_cast<Arduino::Participant*>(this);
return thisArduino->Publish(msg);
#endif
}
@ -190,93 +190,89 @@ bool Participant::Publish(IMessage *msg) {
#pragma region Receive
void Participant::ReceiveData(unsigned char bufferSize,
RemoteParticipant *remoteParticipant) {
void Participant::ReceiveData(unsigned char bufferSize, RemoteParticipant* remoteParticipant) {
unsigned char msgId = this->buffer[0];
// std::cout << "receive msg " << (int)msgId << "\n";
switch (msgId) {
case ClientMsg::id: {
ClientMsg *msg = new ClientMsg(this->buffer);
Process(remoteParticipant, msg);
delete msg;
} break;
case NetworkIdMsg::id: {
NetworkIdMsg *msg = new NetworkIdMsg(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 CustomMsg::id: {
CustomMsg *msg = new CustomMsg(this->buffer);
Process(remoteParticipant, msg);
delete msg;
} break;
case ClientMsg::id: {
ClientMsg* msg = new ClientMsg(this->buffer);
Process(remoteParticipant, msg);
delete msg;
} break;
case NetworkIdMsg::id: {
NetworkIdMsg* msg = new NetworkIdMsg(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 CustomMsg::id: {
CustomMsg* msg = new CustomMsg(this->buffer);
Process(remoteParticipant, msg);
delete msg;
} break;
};
}
void Participant::Process(RemoteParticipant *sender, ClientMsg *msg) {}
void Participant::Process(RemoteParticipant* sender, ClientMsg* msg) {}
void Participant::Process(RemoteParticipant *sender, NetworkIdMsg *msg) {
std::cout << this->name << ": process NetworkId [" << (int)this->networkId
<< "/" << (int)msg->networkId << "]\n";
void Participant::Process(RemoteParticipant* sender, NetworkIdMsg* msg) {
std::cout << this->name << ": process NetworkId [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n";
if (this->networkId != msg->networkId) {
this->networkId = msg->networkId;
for (Thing *thing : this->things)
for (Thing* thing : this->things)
this->SendThingInfo(sender, thing);
}
}
void Participant::Process(RemoteParticipant *sender, InvestigateMsg *msg) {}
void Participant::Process(RemoteParticipant* sender, InvestigateMsg* msg) {}
void Participant::Process(RemoteParticipant *sender, ThingMsg *msg) {}
void Participant::Process(RemoteParticipant* sender, ThingMsg* msg) {}
void Participant::Process(RemoteParticipant *sender, NameMsg *msg) {
Thing *thing = sender->Get(msg->networkId, msg->thingId);
void Participant::Process(RemoteParticipant* sender, NameMsg* msg) {
Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing != nullptr) {
int nameLength = msg->nameLength;
char *thingName = new char[nameLength + 1];
char* thingName = new char[nameLength + 1];
strcpy(thingName, msg->name);
thingName[nameLength] = '\0';
thing->name = thingName;
std::cout << "thing name = " << thing->name << " length = " << nameLength
<< "\n";
std::cout << "thing name = " << thing->name << " length = " << nameLength << "\n";
}
}
void Participant::Process(RemoteParticipant *sender, PoseMsg *msg) {}
void Participant::Process(RemoteParticipant* sender, PoseMsg* msg) {}
void Participant::Process(RemoteParticipant *sender, CustomMsg *msg) {
void Participant::Process(RemoteParticipant* sender, CustomMsg* msg) {
// std::cout << this->name << ": process Binary [" << (int)this->networkId << "/"
// << (int)msg->networkId << "]\n";
Thing *thing = sender->Get(msg->networkId, msg->thingId);
Thing* thing = sender->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";
std::cout << "custom msg for unknown thing " << (int)msg->networkId << ":" << (int)msg->thingId << "\n";
}
// Receive
#pragma endregion
} // namespace Control
} // namespace Passer
} // namespace RoboidControl
} // namespace Passer

43
UdpPosix.cpp → Posix/Participant.cpp
View File

@ -1,8 +1,8 @@
#include "UdpPosix.h"
#include "Participant.h"
#if defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <fcntl.h> // For fcntl
#include <fcntl.h> // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
@ -10,9 +10,9 @@
namespace Passer {
namespace RoboidControl {
namespace Posix {
void UdpPosix::Setup(int localPort, const char *remoteIpAddress,
int remotePort) {
void Participant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
#if defined(__unix__) || defined(__APPLE__)
// Create a UDP socket
@ -26,7 +26,7 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress,
// Set the socket to non-blocking mode
#if defined(_WIN32) || defined(_WIN64)
u_long mode = 1; // 1 to enable non-blocking socket
u_long mode = 1; // 1 to enable non-blocking socket
ioctlsocket(this->sock, FIONBIO, &mode);
#elif defined(__unix__) || defined(__APPLE__)
int flags = fcntl(this->sock, F_GETFL, 0);
@ -56,8 +56,7 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress,
}
// Bind the socket to the specified port
if (bind(this->sock, (const struct sockaddr *)&server_addr,
sizeof(server_addr)) < 0) {
if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
std::cerr << "Bind failed" << std::endl;
close(sock);
}
@ -65,20 +64,17 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress,
#endif
}
void UdpPosix::Receive() {
void Participant::Receive() {
#if defined(__unix__) || defined(__APPLE__)
sockaddr_in client_addr;
socklen_t len = sizeof(client_addr);
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0,
(struct sockaddr *)&client_addr, &len);
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
if (packetSize > 0) {
char sender_ipAddress[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress,
INET_ADDRSTRLEN);
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, INET_ADDRSTRLEN);
int sender_port = ntohs(client_addr.sin_port);
Participant *remoteParticipant =
this->GetParticipant(sender_ipAddress, sender_port);
Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
if (remoteParticipant == nullptr) {
remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
// std::cout << "New sender " << sender_ipAddress << ":" << sender_port
@ -94,7 +90,7 @@ void UdpPosix::Receive() {
#endif
}
bool UdpPosix::Send(RemoteParticipant *remoteParticipant, int bufferSize) {
bool Participant::Send(RemoteParticipant* remoteParticipant, int bufferSize) {
#if defined(__unix__) || defined(__APPLE__)
// Set up the destination address
// char ip_str[INET_ADDRSTRLEN];
@ -108,8 +104,7 @@ bool UdpPosix::Send(RemoteParticipant *remoteParticipant, int bufferSize) {
dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);
// Send the message
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0,
(struct sockaddr *)&remote_addr, sizeof(remote_addr));
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
if (sent_bytes < 0) {
std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
close(sock);
@ -119,7 +114,7 @@ bool UdpPosix::Send(RemoteParticipant *remoteParticipant, int bufferSize) {
return true;
}
bool UdpPosix::Publish(IMessage *msg) {
bool Participant::Publish(IMessage* msg) {
#if defined(__unix__) || defined(__APPLE__)
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
@ -127,11 +122,8 @@ bool UdpPosix::Publish(IMessage *msg) {
char ip_str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port)
<< "\n";
int sent_bytes =
sendto(sock, this->buffer, bufferSize, 0,
(struct sockaddr *)&broadcast_addr, sizeof(broadcast_addr));
std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) << "\n";
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
if (sent_bytes < 0) {
std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
close(sock);
@ -141,5 +133,6 @@ bool UdpPosix::Publish(IMessage *msg) {
return true;
}
} // namespace Control
} // namespace Passer
} // namespace Posix
} // namespace RoboidControl
} // namespace Passer

19
Posix/Participant.h Normal file
View File

@ -0,0 +1,19 @@
#pragma once
#include "../Participant.h"
namespace Passer {
namespace RoboidControl {
namespace Posix {
class Participant : public RoboidControl::Participant {
public:
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive();
bool Send(RemoteParticipant* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
};
} // namespace Posix
} // namespace RoboidControl
} // namespace Passer

24
RemoteParticipant.cpp
View File

@ -1,7 +1,7 @@
#include "RemoteParticipant.h"
namespace Passer {
namespace Control {
namespace RoboidControl {
RemoteParticipant::RemoteParticipant() {}
@ -37,18 +37,18 @@ void RemoteParticipant::Remove(Thing *thing) {
<< " list size = " << this->things.size() << "\n";
}
void RemoteParticipant::UpdateAll(unsigned long currentTimeMs) {
// Not very efficient, but it works for now.
// void RemoteParticipant::UpdateAll(unsigned long currentTimeMs) {
// // Not very efficient, but it works for now.
for (Thing *thing : this->things) {
if (thing != nullptr &&
thing->GetParent() == nullptr) { // update all root things
// std::cout << " update " << (int)ix << " thingid " << (int)thing->id
// << "\n";
thing->Update(currentTimeMs);
}
}
}
// for (Thing *thing : this->things) {
// if (thing != nullptr &&
// thing->GetParent() == nullptr) { // update all root things
// // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
// // << "\n";
// thing->Update(currentTimeMs);
// }
// }
// }
} // namespace Control
} // namespace Passer

22
RemoteParticipant.h
View File

@ -2,26 +2,44 @@
#include "Thing.h"
namespace Passer {
namespace Control {
namespace RoboidControl {
/// @brief A reference to a participant, possibly on a remote location
class RemoteParticipant {
public:
/// @brief The internet address of the participant
const char *ipAddress = "0.0.0.0";
/// @brief The UDP port on which the participant can be reached
int port = 0;
/// @brief The network ID of the participant
unsigned char networkId = 0;
/// @brief The default constructor
RemoteParticipant();
/// @brief Create a new participant
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
RemoteParticipant(const char *ipAddress, int port);
protected:
/// @brief The things reported by this participant
std::list<Thing *> things;
public:
/// @brief Get a thing with the give ids
/// @param networkId The network ID of the thing
/// @param thingId The ID of the thing
/// @return The thing when it is found, nullptr in other cases
Thing *Get(unsigned char networkId, unsigned char thingId);
/// @brief Add a new thing for this participant
/// @param thing The thing to add
void Add(Thing *thing);
/// @brief Remove a thing fror this participant
/// @param thing The thing to remove
void Remove(Thing *thing);
void UpdateAll(unsigned long currentTimeMs);
//void UpdateAll(unsigned long currentTimeMs);
};
} // namespace Control

2
Thing.cpp
View File

@ -20,7 +20,7 @@ Thing::Thing(unsigned char thingType) {
this->angularVelocity = Spherical16::zero;
}
Passer::Control::Thing::Thing(RemoteParticipant *participant, unsigned char networkId,
Thing::Thing(RemoteParticipant *participant, unsigned char networkId,
unsigned char thingId, Type thingType) {
// no participant reference yet..
this->participant = participant;

20
UdpArduino.h
View File

@ -1,20 +0,0 @@
#pragma once
#include "Participant.h"
namespace Passer {
namespace RoboidControl {
class UdpArduino : public Participant {
public:
void Setup(int localPort, const char *remoteIpAddress, int remotePort);
void Receive();
bool Send(RemoteParticipant* remoteParticipant, int bufferSize);
bool Publish(IMessage *msg);
protected:
void GetBroadcastAddress();
};
} // namespace Control
} // namespace Passer

17
UdpPosix.h
View File

@ -1,17 +0,0 @@
#pragma once
#include "Participant.h"
namespace Passer {
namespace RoboidControl {
class UdpPosix : public Participant {
public:
void Setup(int localPort, const char *remoteIpAddress, int remotePort);
void Receive();
bool Send(RemoteParticipant* remoteParticipant, int bufferSize);
bool Publish(IMessage *msg);
};
} // namespace Control
} // namespace Passer

17
UdpWindows.h
View File

@ -1,17 +0,0 @@
#pragma once
#include "Participant.h"
namespace Passer {
namespace RoboidControl {
class UdpWindows : public Participant {
public:
void Setup(int localPort, const char *remoteIpAddress, int remotePort);
void Receive();
bool Send(RemoteParticipant* remoteParticipant, int bufferSize);
bool Publish(IMessage *msg);
};
} // namespace Control
} // namespace Passer

52
UdpWindows.cpp → Windows/Participant.cpp
View File

@ -1,4 +1,4 @@
#include "UdpWindows.h"
#include "Participant.h"
#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
@ -6,7 +6,7 @@
#pragma comment(lib, "ws2_32.lib")
#elif defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <fcntl.h> // For fcntl
#include <fcntl.h> // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
@ -14,9 +14,9 @@
namespace Passer {
namespace RoboidControl {
namespace Windows {
void UdpWindows::Setup(int localPort, const char *remoteIpAddress,
int remotePort) {
void Participant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
#if defined(_WIN32) || defined(_WIN64)
// Create a UDP socket
@ -42,7 +42,7 @@ void UdpWindows::Setup(int localPort, const char *remoteIpAddress,
// Set the socket to non-blocking mode
#if defined(_WIN32) || defined(_WIN64)
u_long mode = 1; // 1 to enable non-blocking socket
u_long mode = 1; // 1 to enable non-blocking socket
ioctlsocket(this->sock, FIONBIO, &mode);
#elif defined(__unix__) || defined(__APPLE__)
int flags = fcntl(this->sock, F_GETFL, 0);
@ -82,8 +82,7 @@ void UdpWindows::Setup(int localPort, const char *remoteIpAddress,
}
// Bind the socket to the specified port
if (bind(this->sock, (const struct sockaddr *)&server_addr,
sizeof(server_addr)) < 0) {
if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
std::cerr << "Bind failed" << std::endl;
#if defined(_WIN32) || defined(_WIN64)
closesocket(sock);
@ -96,7 +95,7 @@ void UdpWindows::Setup(int localPort, const char *remoteIpAddress,
#endif
}
void UdpWindows::Receive() {
void Participant::Receive() {
#if defined(_WIN32) || defined(_WIN64)
// char ip_str[INET_ADDRSTRLEN];
// inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@ -109,12 +108,11 @@ void UdpWindows::Receive() {
#elif defined(__unix__) || defined(__APPLE__)
socklen_t len = sizeof(client_addr);
#endif
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0,
(struct sockaddr *)&client_addr, &len);
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
// std::cout << "received data " << packetSize << "\n";
if (packetSize < 0) {
#if defined(_WIN32) || defined(_WIN64)
int error_code = WSAGetLastError(); // Get the error code on Windows
int error_code = WSAGetLastError(); // Get the error code on Windows
if (error_code != WSAEWOULDBLOCK)
std::cerr << "recvfrom failed with error: " << error_code << std::endl;
#else
@ -122,12 +120,10 @@ void UdpWindows::Receive() {
#endif
} else if (packetSize > 0) {
char sender_ipAddress[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress,
INET_ADDRSTRLEN);
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, INET_ADDRSTRLEN);
int sender_port = ntohs(client_addr.sin_port);
Participant *remoteParticipant =
this->GetParticipant(sender_ipAddress, sender_port);
RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
if (remoteParticipant == nullptr) {
remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
// std::cout << "New sender " << sender_ipAddress << ":"
@ -144,17 +140,15 @@ void UdpWindows::Receive() {
#endif
}
bool UdpWindows::Send(RemoteParticipant *remoteParticipant, int bufferSize) {
bool Participant::Send(RemoteParticipant* remoteParticipant, int bufferSize) {
#if defined(_WIN32) || defined(_WIN64)
char ip_str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port)
<< "\n";
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0,
(struct sockaddr *)&remote_addr, sizeof(remote_addr));
std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port) << "\n";
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
#if defined(_WIN32) || defined(_WIN64)
if (sent_bytes <= SOCKET_ERROR) {
int error_code = WSAGetLastError(); // Get the error code on Windows
int error_code = WSAGetLastError(); // Get the error code on Windows
std::cerr << "sendto failed with error: " << error_code << std::endl;
closesocket(sock);
WSACleanup();
@ -171,7 +165,7 @@ bool UdpWindows::Send(RemoteParticipant *remoteParticipant, int bufferSize) {
return true;
}
bool UdpWindows::Publish(IMessage *msg) {
bool Participant::Publish(IMessage* msg) {
#if defined(_WIN32) || defined(_WIN64)
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
@ -179,14 +173,11 @@ bool UdpWindows::Publish(IMessage *msg) {
char ip_str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port)
<< "\n";
int sent_bytes =
sendto(sock, this->buffer, bufferSize, 0,
(struct sockaddr *)&broadcast_addr, sizeof(broadcast_addr));
std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) << "\n";
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
#if defined(_WIN32) || defined(_WIN64)
if (sent_bytes <= SOCKET_ERROR) {
int error_code = WSAGetLastError(); // Get the error code on Windows
int error_code = WSAGetLastError(); // Get the error code on Windows
std::cerr << "sendto failed with error: " << error_code << std::endl;
closesocket(sock);
WSACleanup();
@ -203,5 +194,6 @@ bool UdpWindows::Publish(IMessage *msg) {
return true;
}
} // namespace Control
} // namespace Passer
} // namespace Windows
} // namespace RoboidControl
} // namespace Passer

19
Windows/Participant.h Normal file
View File

@ -0,0 +1,19 @@
#pragma once
#include "../Participant.h"
namespace Passer {
namespace RoboidControl {
namespace Windows {
class Participant : public RoboidControl::Participant {
public:
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive();
bool Send(RemoteParticipant* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
};
} // namespace Windows
} // namespace RoboidControl
} // namespace Passer