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

Merge commit 'abd072a67c4d6e7ae11288d64a112c0105704abc'

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-01 14:27:48 +01:00
commit 1f2f9da715
8 changed files with 165 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
Participant.cpp
View File

@ -12,6 +12,7 @@
#pragma comment(lib, "ws2_32.lib")
#elif defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <chrono>
#include <fcntl.h> // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
@ -64,11 +65,18 @@ void Participant::SetupUDP(int localPort, const char *remoteIpAddress,
this->connected = true;
}
#if defined(ARDUINO)
void Participant::Update() { this->Update(millis()); }
#endif
void Participant::Update(unsigned long currentTimeMs) {
if (currentTimeMs == 0) {
#if defined(ARDUINO)
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());
currentTimeMs = static_cast<unsigned long>(ms.count());
#endif
}
if (this->connected == false)
begin();
@ -81,7 +89,7 @@ void Participant::Update(unsigned long currentTimeMs) {
}
this->ReceiveUDP();
Thing::UpdateAll(currentTimeMs);
this->UpdateAll(currentTimeMs);
}
void Participant::ReceiveUDP() {
@ -179,6 +187,7 @@ bool Participant::Publish(IMessage *msg) {
void Participant::ReceiveData(unsigned char bufferSize,
Participant *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);
@ -233,7 +242,7 @@ void Participant::Process(Participant *sender, NetworkIdMsg *msg) {
// sender->SendThingInfo(thing);
// }
for (Thing *thing : Thing::allThings) {
for (Thing *thing : this->things) {
sender->SendThingInfo(thing);
}
}
@ -244,7 +253,7 @@ void Participant::Process(InvestigateMsg *msg) {}
void Participant::Process(ThingMsg *msg) {}
void Participant::Process(NameMsg *msg) {
Thing *thing = Thing::Get(msg->networkId, msg->thingId);
Thing *thing = this->Get(msg->networkId, msg->thingId);
if (thing != nullptr) {
char *thingName = new char[strlen(msg->name)];
strcpy(thingName, msg->name);
@ -256,12 +265,66 @@ void Participant::Process(NameMsg *msg) {
void Participant::Process(PoseMsg *msg) {}
void Participant::Process(CustomMsg *msg) {
Thing *thing = Thing::Get(msg->networkId, msg->thingId);
Thing *thing = this->Get(msg->networkId, msg->thingId);
if (thing != nullptr)
thing->ProcessBytes(msg->bytes);
else
std::cout << "custom msg for unknown thing " << (int)msg->networkId << ":"
<< (int)msg->thingId << "\n";
std::cout << "Processed custom msg\n";
}
// Receive
#pragma endregion
#pragma region Things
Thing *Participant::Get(unsigned char networkId, unsigned char thingId) {
std::cout << "Get " << (int)networkId << "/" << (int)thingId << " from "
<< this->things.size() << " things\n";
for (auto &thing : this->things) {
std::cout << " ? " << (int)thing->networkId << "/" << (int)thing->id
<< "\n";
if (thing->networkId == networkId && thing->id == thingId) {
return thing;
}
}
return nullptr;
}
int Participant::Add(Thing *newThing) {
for (Thing *thing : this->things) {
if (thing == newThing) {
std::cout << "Thing already exists, not adding\n";
return thing->id;
}
}
std::cout << "Adding " << (int)newThing->networkId << "/" << (int)newThing->id
<< "\n";
this->things.push_back(newThing);
return this->things.size();
}
void Participant::Remove(Thing *thing) {
this->things.remove_if([thing](Thing *obj) { return obj == thing; });
std::cout << "Removing " << thing->networkId << "/" << thing->id
<< " list size = " << this->things.size() << "\n";
}
void Participant::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);
}
}
}
#pragma endregion
} // namespace Control
} // namespace Passer

14
Participant.h
View File

@ -61,17 +61,17 @@ public:
Participant();
Participant(int port);
Participant(const char *ipAddress, int port);
// Bad design, you cannot use constructor in global scope
// i.e.
// Participant p = Participant("127.0.0.1", 8000);
void begin();
bool connected = false;
#if defined(ARDUINO)
virtual void Update();
#endif
virtual void Update(unsigned long currentTimeMs);
virtual void Update(unsigned long currentTimeMs = 0);
std::list<Thing *> things;
Thing *Get(unsigned char networkId, unsigned char thingId);
int Add(Thing *thing);
void Remove(Thing *thing);
void UpdateAll(unsigned long currentTimeMs);
void SendThingInfo(Thing *thing);
void PublishThingInfo(Thing *thing);

23
SiteServer.cpp
View File

@ -10,6 +10,7 @@ namespace Control {
SiteServer::SiteServer(int port) {
this->name = "Site Server";
this->publishInterval = 0;
this->ipAddress = "0.0.0.0";
this->port = port;
@ -21,11 +22,6 @@ SiteServer::SiteServer(int port) {
Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
}
void SiteServer::Update(unsigned long currentTimeMs) {
this->ReceiveUDP();
Thing::UpdateAll(currentTimeMs);
}
void SiteServer::Process(Participant *sender, ClientMsg *msg) {
if (msg->networkId == 0) {
std::cout << this->name << " received New Client -> " << sender->ipAddress
@ -40,14 +36,19 @@ void SiteServer::Process(Participant *sender, NetworkIdMsg *msg) {}
void SiteServer::Process(ThingMsg *msg) {
Thing *thing = Thing::Get(msg->networkId, msg->thingId);
Thing *thing = this->Get(msg->networkId, msg->thingId);
if (thing == nullptr) {
std::cout << "could not find thing " << (int)msg->networkId << "/"
<< (int)msg->thingId << "\n";
auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
if (thingMsgProcessor != thingMsgProcessors.end()) // found item
thingMsgProcessor->second(msg->networkId, msg->thingId);
else
new Thing(this, msg->networkId, msg->thingId,
(Thing::Type)msg->thingType);
if (thingMsgProcessor != thingMsgProcessors.end()) { // found item
Thing *newThing = thingMsgProcessor->second(msg->networkId, msg->thingId);
this->Add(newThing);
} else {
Thing *newThing = new Thing(this, msg->networkId, msg->thingId,
(Thing::Type)msg->thingType);
this->Add(newThing);
}
}
}

16
SiteServer.h
View File

@ -2,9 +2,9 @@
#include "Participant.h"
#include <unordered_map>
#include <functional>
#include <memory>
#include <unordered_map>
namespace Passer {
namespace Control {
@ -14,13 +14,13 @@ class SiteServer : public Participant {
public:
SiteServer(int port = 7681);
virtual void Update(unsigned long currentTimeMs) override;
// virtual void Update(unsigned long currentTimeMs = 0) override;
template <typename ThingClass> void Register(unsigned char thingType) {
thingMsgProcessors[thingType] = [](unsigned char networkId,
unsigned char thingId) {
return std::make_unique<ThingClass>(networkId, thingId);
};
unsigned char thingId) {
return new ThingClass(networkId, thingId);
};
};
protected:
@ -30,9 +30,9 @@ protected:
virtual void Process(Participant *sender, NetworkIdMsg *msg) override;
virtual void Process(ThingMsg *msg) override;
using ThingConstructor = std::function<std::unique_ptr<Thing>(
unsigned char networkId, unsigned char thingId)>;
std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
using ThingConstructor =
std::function<Thing *(unsigned char networkId, unsigned char thingId)>;
std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
};
} // namespace Control

103
Thing.cpp
View File

@ -12,17 +12,18 @@ Thing::Thing(unsigned char thingType) {
// this->position = Spherical16::zero;
// this->orientation = SwingTwist16::identity;
this->id = 0;
this->type = thingType;
this->networkId = 0;
this->Init();
int thingId = Thing::Add(this);
// int thingId = Thing::Add(this);
if (thingId < 0) {
std::cout << "ERROR: Thing store is full\n";
this->id = 0; // what to do when we cannot store any more things?
} else
this->id = thingId;
// if (thingId < 0) {
// std::cout << "ERROR: Thing store is full\n";
// this->id = 0; // what to do when we cannot store any more things?
// } else
// this->id = thingId;
this->linearVelocity = Spherical16::zero;
this->angularVelocity = Spherical16::zero;
@ -36,21 +37,23 @@ Passer::Control::Thing::Thing(Participant *participant, unsigned char networkId,
this->type = (unsigned char)thingType;
this->Init();
thingId = Thing::Add(this);
// thingId = Thing::Add(this);
if (thingId < 0) {
std::cout << "ERROR: Thing store is full\n";
this->id = 0; // what to do when we cannot store any more things?
} else
this->id = thingId;
// if (thingId < 0) {
// std::cout << "ERROR: Thing store is full\n";
// this->id = 0; // what to do when we cannot store any more things?
// } else
// this->id = thingId;
this->linearVelocity = Spherical16::zero;
this->angularVelocity = Spherical16::zero;
std::cout << "Added thing " << (int)this->networkId << "/" << (int)this->id
std::cout << "Created thing " << (int)this->networkId << "/" << (int)this->id
<< "\n";
}
void Thing::Terminate() { Thing::Remove(this); }
void Thing::Terminate() {
// Thing::Remove(this);
}
void Thing::Init() {}
@ -178,40 +181,48 @@ Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }
// All things
std::list<Thing *> Thing::allThings;
// std::list<Thing *> Thing::allThings;
Thing *Thing::Get(unsigned char networkId, unsigned char thingId) {
// Thing *Thing::Get(unsigned char networkId, unsigned char thingId) {
// std::cout << "Get " << (int)networkId << "/" << (int)thingId << " from "
// << allThings.size() << " things\n";
// for (auto &thing : allThings) {
// std::cout << " ? " << (int)thing->networkId << "/" << (int)thing->id
// << "\n";
// if (thing->networkId == networkId && thing->id == thingId) {
// return thing;
// }
// }
// return nullptr;
// }
for (auto &thing : allThings) {
if (thing->networkId == networkId && thing->id == thingId) {
return thing;
}
}
return nullptr;
}
// int Thing::Add(Thing *newThing) {
// for (Thing *thing : allThings) {
// if (thing == newThing)
// return thing->id;
// }
// std::cout << "Adding " << (int)newThing->networkId << "/" <<
// (int)newThing->id
// << "\n";
// allThings.push_back(newThing);
// return allThings.size();
// }
int Thing::Add(Thing *newThing) {
for (Thing *thing : allThings) {
if (thing == newThing)
return thing->id;
}
allThings.push_back(newThing);
return allThings.size();
}
// void Thing::Remove(Thing *thing) {
// Thing::allThings.remove_if([thing](Thing *obj) { return obj == thing; });
// std::cout << "Removing " << thing->networkId << "/" << thing->id
// << " list size = " << allThings.size() << "\n";
// }
void Thing::Remove(Thing *thing) {
Thing::allThings.remove_if([thing](Thing *obj) { return obj == thing; });
}
// void Thing::UpdateAll(unsigned long currentTimeMs) {
// // Not very efficient, but it works for now.
void Thing::UpdateAll(unsigned long currentTimeMs) {
// Not very efficient, but it works for now.
for (Thing *thing : Thing::allThings) {
if (thing != nullptr &&
thing->parent == nullptr) { // update all root things
// std::cout << " update " << (int)ix << " thingid " << (int)thing->id
// << "\n";
thing->Update(currentTimeMs);
}
}
}
// for (Thing *thing : Thing::allThings) {
// if (thing != nullptr &&
// thing->parent == nullptr) { // update all root things
// // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
// // << "\n";
// thing->Update(currentTimeMs);
// }
// }
//}

12
Thing.h
View File

@ -128,13 +128,13 @@ protected:
virtual void Init();
//------------ All things
public:
static Thing *Get(unsigned char networkId, unsigned char thingId);
static int Add(Thing *thing);
static void Remove(Thing *thing);
static void UpdateAll(unsigned long currentTimeMs);
// public:
// static Thing *Get(unsigned char networkId, unsigned char thingId);
// static int Add(Thing *thing);
// static void Remove(Thing *thing);
// static void UpdateAll(unsigned long currentTimeMs);
static std::list<Thing *> allThings;
// static std::list<Thing *> allThings;
};
// static std::list<Thing *> allThings;

69
UdpPosix.cpp
View File

@ -16,20 +16,8 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress,
#if defined(__unix__) || defined(__APPLE__)
// Create a UDP socket
#if defined(_WIN32) || defined(_WIN64)
// Windows-specific Winsock initialization
WSADATA wsaData;
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
std::cerr << "WSAStartup failed" << std::endl;
return;
}
#endif
#if defined(_WIN32) || defined(_WIN64)
this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
#elif defined(__unix__) || defined(__APPLE__)
this->sock = socket(AF_INET, SOCK_DGRAM, 0);
#endif
if (this->sock < 0) {
std::cerr << "Error creating socket" << std::endl;
@ -52,12 +40,7 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress,
remote_addr.sin_port = htons(remotePort);
if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) {
std::cerr << "Invalid address" << std::endl;
#if defined(_WIN32) || defined(_WIN64)
closesocket(sock);
WSACleanup();
#elif defined(__unix__) || defined(__APPLE__)
close(sock);
#endif
return;
}
}
@ -68,12 +51,7 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress,
server_addr.sin_port = htons(localPort);
if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) {
std::cerr << "Invalid address" << std::endl;
#if defined(_WIN32) || defined(_WIN64)
closesocket(sock);
WSACleanup();
#elif defined(__unix__) || defined(__APPLE__)
close(sock);
#endif
return;
}
@ -81,12 +59,7 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress,
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);
WSACleanup();
#elif defined(__unix__) || defined(__APPLE__)
close(sock);
#endif
}
#endif
@ -94,29 +67,11 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress,
void UdpPosix::Receive() {
#if defined(__unix__) || defined(__APPLE__)
// char ip_str[INET_ADDRSTRLEN];
// inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
// std::cout << this->name << " Receive on " << ip_str << ":"
// << ntohs(server_addr.sin_port) << "\n";
sockaddr_in client_addr;
#if defined(_WIN32) || defined(_WIN64)
int len = sizeof(client_addr);
#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);
// std::cout << "received data " << packetSize << "\n";
if (packetSize < 0) {
#if defined(_WIN32) || defined(_WIN64)
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
// std::cerr << "recvfrom failed with error: " << packetSize << std::endl;
#endif
} else if (packetSize > 0) {
if (packetSize > 0) {
char sender_ipAddress[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress,
INET_ADDRSTRLEN);
@ -134,6 +89,7 @@ void UdpPosix::Receive() {
}
ReceiveData(packetSize, remoteParticipant);
std::cout << "Received data\n";
}
#endif
}
@ -142,7 +98,6 @@ bool UdpPosix::Send(
IMessage *msg) { // Send the message to the specified address and port
#if defined(__unix__) || defined(__APPLE__)
int bufferSize = msg->Serialize(this->buffer);
// std::cout << "buffer size " << bufferSize << "\n";
if (bufferSize <= 0)
return true;
@ -152,21 +107,11 @@ bool UdpPosix::Send(
<< "\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
std::cerr << "sendto failed with error: " << error_code << std::endl;
closesocket(sock);
WSACleanup();
return false;
}
#elif defined(__unix__) || defined(__APPLE__)
if (sent_bytes < 0) {
std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
close(sock);
return false;
}
#endif
#endif
return true;
}
@ -184,21 +129,11 @@ bool UdpPosix::Publish(IMessage *msg) {
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
std::cerr << "sendto failed with error: " << error_code << std::endl;
closesocket(sock);
WSACleanup();
return false;
}
#elif defined(__unix__) || defined(__APPLE__)
if (sent_bytes < 0) {
std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
close(sock);
return false;
}
#endif
#endif
return true;
}

3
test/second_test.cc
View File

@ -35,7 +35,8 @@ TEST_F(ControlCoreSuite2, Basic2) {
.time_since_epoch()
.count();
Thing::UpdateAll(milliseconds);
// Thing::UpdateAll(milliseconds);
t.Update(milliseconds);
}
#endif