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

Starting to publish client msgs...

Browse Source
This commit is contained in:
Pascal Serrarens 2025-01-02 11:53:46 +01:00
parent b9ae2f574c
commit f630b0c7cc
12 changed files with 249 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
DoxyGen/Doxyfile
View File

@ -42,7 +42,7 @@ DOXYFILE_ENCODING = UTF-8
# title of most generated pages and in a few other places.
# The default value is: My Project.
PROJECT_NAME = "Control Core"
PROJECT_NAME = "Control Core for C++"
# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
# could be handy for archiving the generated documentation or if some version

16
LowLevelMessages.cpp
View File

@ -2,12 +2,12 @@
#include "float16.h"
void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
void LowLevelMessages::SendAngle8(char *buffer, unsigned char *ix,
const float angle) {
Angle8 packedAngle2 = Angle8::Degrees(angle);
buffer[(*ix)++] = packedAngle2.GetBinary();
}
Angle8 LowLevelMessages::ReceiveAngle8(const unsigned char *buffer,
Angle8 LowLevelMessages::ReceiveAngle8(const char *buffer,
unsigned char *startIndex) {
unsigned char binary = buffer[(*startIndex)++];
@ -16,7 +16,7 @@ Angle8 LowLevelMessages::ReceiveAngle8(const unsigned char *buffer,
return angle;
}
void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
void LowLevelMessages::SendFloat16(char *buffer, unsigned char *ix,
float value) {
float16 value16 = float16(value);
short binary = value16.getBinary();
@ -24,7 +24,7 @@ void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
buffer[(*ix)++] = (binary >> 8) & 0xFF;
buffer[(*ix)++] = binary & 0xFF;
}
float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer,
float LowLevelMessages::ReceiveFloat16(const char *buffer,
unsigned char *startIndex) {
unsigned char ix = *startIndex;
unsigned char msb = buffer[ix++];
@ -37,13 +37,13 @@ float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer,
return (float)f.toFloat();
}
void LowLevelMessages::SendSpherical16(unsigned char *buffer, unsigned char *ix,
void LowLevelMessages::SendSpherical16(char *buffer, unsigned char *ix,
Spherical16 s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
Spherical16 LowLevelMessages::ReceiveSpherical16(const unsigned char *buffer,
Spherical16 LowLevelMessages::ReceiveSpherical16(const char *buffer,
unsigned char *startIndex) {
float distance = ReceiveFloat16(buffer, startIndex);
@ -57,7 +57,7 @@ Spherical16 LowLevelMessages::ReceiveSpherical16(const unsigned char *buffer,
return s;
}
void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
void Passer::Control::LowLevelMessages::SendQuat32(char *buffer,
unsigned char *ix,
SwingTwist16 rotation) {
Quaternion q = rotation.ToQuaternion();
@ -78,7 +78,7 @@ void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
buffer[(*ix)++] = qw;
}
SwingTwist16 LowLevelMessages::ReceiveQuat32(const unsigned char *buffer,
SwingTwist16 LowLevelMessages::ReceiveQuat32(const char *buffer,
unsigned char *ix) {
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;

23
LowLevelMessages.h
View File

@ -6,25 +6,18 @@ namespace Control {
class LowLevelMessages {
public:
static void SendAngle8(unsigned char *buffer, unsigned char *ix,
const float angle);
static Angle8 ReceiveAngle8(const unsigned char *buffer,
unsigned char *startIndex);
static void SendAngle8(char *buffer, unsigned char *ix, const float angle);
static Angle8 ReceiveAngle8(const char *buffer, unsigned char *startIndex);
static void SendFloat16(unsigned char *buffer, unsigned char *ix,
float value);
static float ReceiveFloat16(const unsigned char *buffer,
unsigned char *startIndex);
static void SendFloat16(char *buffer, unsigned char *ix, float value);
static float ReceiveFloat16(const char *buffer, unsigned char *startIndex);
static void SendSpherical16(unsigned char *buffer, unsigned char *ix,
Spherical16 s);
static Spherical16 ReceiveSpherical16(const unsigned char *buffer,
static void SendSpherical16(char *buffer, unsigned char *ix, Spherical16 s);
static Spherical16 ReceiveSpherical16(const char *buffer,
unsigned char *startIndex);
static void SendQuat32(unsigned char *buffer, unsigned char *ix,
SwingTwist16 q);
static SwingTwist16 ReceiveQuat32(const unsigned char *buffer,
unsigned char *ix);
static void SendQuat32(char *buffer, unsigned char *ix, SwingTwist16 q);
static SwingTwist16 ReceiveQuat32(const char *buffer, unsigned char *ix);
};
} // namespace Control

37
Messages.cpp
View File

@ -10,7 +10,7 @@ IMessage::IMessage() {}
// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; }
unsigned char IMessage::Serialize(char *buffer) { return 0; }
// void IMessage::Deserialize(unsigned char *buffer) {}
@ -35,9 +35,9 @@ bool IMessage::SendTo(Participant *participant) {
#pragma region Client
ClientMsg::ClientMsg(unsigned char networkId) { this->networkId = networkId; }
ClientMsg::ClientMsg(char networkId) { this->networkId = networkId; }
unsigned char ClientMsg::Serialize(unsigned char *buffer) {
unsigned char ClientMsg::Serialize(char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
@ -52,16 +52,13 @@ unsigned char ClientMsg::Serialize(unsigned char *buffer) {
#pragma region Network Id
NetworkIdMsg::NetworkIdMsg(unsigned char *buffer) {
this->networkId = buffer[1];
}
NetworkIdMsg::NetworkIdMsg(char *buffer) { this->networkId = buffer[1]; }
// void NetworkIdMsg::Deserialize(unsigned char *buffer) {
// this->networkId = buffer[1];
// }
NetworkIdMsg NetworkIdMsg::Receive(unsigned char *buffer,
unsigned char bufferSize) {
NetworkIdMsg NetworkIdMsg::Receive(char *buffer, unsigned char bufferSize) {
NetworkIdMsg msg = NetworkIdMsg(buffer);
return msg;
}
@ -71,7 +68,7 @@ NetworkIdMsg NetworkIdMsg::Receive(unsigned char *buffer,
#pragma region Investigate
InvestigateMsg::InvestigateMsg(unsigned char *buffer) {
InvestigateMsg::InvestigateMsg(char *buffer) {
unsigned ix = 1; // first byte is msgId
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
@ -81,7 +78,7 @@ InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
this->thingId = thingId;
}
unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
unsigned char InvestigateMsg::Serialize(char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
@ -100,7 +97,7 @@ unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
#pragma region Thing
ThingMsg::ThingMsg(const unsigned char *buffer) {
ThingMsg::ThingMsg(char *buffer) {
unsigned char ix = 1; // first byte is msg id
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
@ -116,7 +113,7 @@ ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
this->parentId = parentId;
}
unsigned char ThingMsg::Serialize(unsigned char *buffer) {
unsigned char ThingMsg::Serialize(char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
@ -146,7 +143,7 @@ NameMsg::NameMsg(unsigned char networkId, unsigned char thingId,
this->nameLength = nameLength;
}
unsigned char NameMsg::Serialize(unsigned char *buffer) {
unsigned char NameMsg::Serialize(char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
@ -185,7 +182,7 @@ ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId,
this->scale = scale;
}
unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
unsigned char ModelUrlMsg::Serialize(char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
@ -215,7 +212,7 @@ PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
this->linearVelocity = linearVelocity;
this->angularVelocity = angularVelocity;
}
PoseMsg::PoseMsg(const unsigned char *buffer) {
PoseMsg::PoseMsg(const char *buffer) {
unsigned char ix = 1; // First byte is msg id
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
@ -224,7 +221,7 @@ PoseMsg::PoseMsg(const unsigned char *buffer) {
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
}
unsigned char PoseMsg::Serialize(unsigned char *buffer) {
unsigned char PoseMsg::Serialize(char *buffer) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg::id;
buffer[ix++] = this->networkId;
@ -246,7 +243,7 @@ unsigned char PoseMsg::Serialize(unsigned char *buffer) {
#pragma region CustomMsg
CustomMsg::CustomMsg(unsigned char *buffer) {
CustomMsg::CustomMsg(char *buffer) {
unsigned char ix = 1;
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
@ -261,7 +258,7 @@ CustomMsg::CustomMsg(unsigned char networkId, Thing *thing) {
this->thing = thing;
}
unsigned char CustomMsg::Serialize(unsigned char *buffer) {
unsigned char CustomMsg::Serialize(char *buffer) {
unsigned char ix = this->length;
this->thing->SendBytes(buffer, &ix);
if (ix <= this->length) // in this case, no data is actually sent
@ -273,7 +270,7 @@ unsigned char CustomMsg::Serialize(unsigned char *buffer) {
return ix;
}
CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
CustomMsg CustomMsg::Receive(char *buffer, unsigned char bufferSize) {
CustomMsg msg = CustomMsg(buffer);
return msg;
}
@ -288,7 +285,7 @@ DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
this->thingId = thing->id;
}
unsigned char DestroyMsg::Serialize(unsigned char *buffer) {
unsigned char DestroyMsg::Serialize(char *buffer) {
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;

39
Messages.h
View File

@ -13,7 +13,7 @@ class Participant;
class IMessage {
public:
IMessage();
virtual unsigned char Serialize(unsigned char *buffer);
virtual unsigned char Serialize(char *buffer);
static unsigned char *ReceiveMsg(unsigned char packetSize);
@ -21,13 +21,16 @@ public:
bool SendTo(Participant *participant);
};
/// @brief A client message announces the presence of a participant
/// When received by another participant, it can be followed by a NetworkIdMsg
/// to announce that participant to this client such that it can join privately
class ClientMsg : public IMessage {
public:
static const unsigned char id = 0xA0;
unsigned char networkId;
ClientMsg(unsigned char networkId);
virtual unsigned char Serialize(unsigned char *buffer) override;
ClientMsg(char networkId);
virtual unsigned char Serialize(char *buffer) override;
};
class NetworkIdMsg : public IMessage {
@ -36,9 +39,9 @@ public:
static const unsigned char length = 2;
unsigned char networkId;
NetworkIdMsg(unsigned char *buffer);
NetworkIdMsg(char *buffer);
static NetworkIdMsg Receive(unsigned char *buffer, unsigned char bufferSize);
static NetworkIdMsg Receive(char *buffer, unsigned char bufferSize);
};
class InvestigateMsg : public IMessage {
@ -48,10 +51,10 @@ public:
unsigned char networkId;
unsigned char thingId;
InvestigateMsg(unsigned char *buffer);
InvestigateMsg(char *buffer);
InvestigateMsg(unsigned char networkId, unsigned char thingId);
virtual unsigned char Serialize(unsigned char *buffer) override;
virtual unsigned char Serialize(char *buffer) override;
};
class ThingMsg : public IMessage {
@ -63,11 +66,11 @@ public:
unsigned char thingType;
unsigned char parentId;
ThingMsg(const unsigned char *buffer);
ThingMsg(char *buffer);
ThingMsg(unsigned char networkId, unsigned char thingId,
unsigned char thingType, unsigned char parentId);
virtual unsigned char Serialize(unsigned char *buffer) override;
virtual unsigned char Serialize(char *buffer) override;
};
class NameMsg : public IMessage {
@ -82,7 +85,7 @@ public:
NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
unsigned char nameLength);
virtual unsigned char Serialize(unsigned char *buffer) override;
virtual unsigned char Serialize(char *buffer) override;
};
class ModelUrlMsg : public IMessage {
@ -99,7 +102,7 @@ public:
ModelUrlMsg(unsigned char networkId, unsigned char thingId,
unsigned char urlLegth, const char *url, float scale = 1);
virtual unsigned char Serialize(unsigned char *buffer) override;
virtual unsigned char Serialize(char *buffer) override;
};
class PoseMsg : public IMessage {
@ -125,9 +128,9 @@ public:
unsigned char poseType, Spherical16 position,
SwingTwist16 orientation, Spherical16 linearVelocity = Spherical16(),
Spherical16 angularVelocity = Spherical16());
PoseMsg(const unsigned char *buffer);
PoseMsg(const char *buffer);
virtual unsigned char Serialize(unsigned char *buffer) override;
virtual unsigned char Serialize(char *buffer) override;
};
class CustomMsg : public IMessage {
@ -140,14 +143,14 @@ public:
Thing *thing;
unsigned char dataSize;
unsigned char *data;
char *data;
CustomMsg(unsigned char *buffer);
CustomMsg(char *buffer);
CustomMsg(unsigned char networkId, Thing *thing);
virtual unsigned char Serialize(unsigned char *buffer) override;
virtual unsigned char Serialize(char *buffer) override;
static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize);
static CustomMsg Receive(char *buffer, unsigned char bufferSize);
};
class DestroyMsg : public IMessage {
@ -159,7 +162,7 @@ public:
DestroyMsg(unsigned char networkId, Thing *thing);
virtual unsigned char Serialize(unsigned char *buffer) override;
virtual unsigned char Serialize(char *buffer) override;
};
} // namespace Control

88
Participant.cpp
View File

@ -1,13 +1,93 @@
#include "Participant.h"
Passer::Control::Participant::Participant(const char *ipAddress, int port)
{
#define BUF_SIZE 1024
#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
#pragma comment(lib, "ws2_32.lib")
#elif defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#endif
Passer::Control::Participant::Participant(const char *ipAddress, int port) {
sockaddr_in server_addr;
// 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__)
sock = socket(AF_INET, SOCK_DGRAM, 0);
#endif
if (sock < 0) {
std::cerr << "Error creating socket" << std::endl;
return;
}
// Set up the server address
// memset(&server_addr, 0, sizeof(server_addr));
// server_addr.sin_family = AF_INET;
// server_addr.sin_port = htons(PORT);
// server_addr.sin_addr.s_addr = INADDR_ANY;
// Set up the server address structure
memset(&server_addr, 0, sizeof(server_addr));
server_addr.sin_family = AF_INET;
server_addr.sin_port = htons(port); // Port to send the packet to
server_addr.sin_addr.s_addr =
inet_addr(ipAddress); // Destination IP address (localhost)
// Bind the socket
if (bind(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
std::cerr << "Error binding socket" << std::endl;
return;
}
// std::cout << "Server is listening on port " << PORT << "..." << std::endl;
}
void Participant::Update(unsigned long currentTimeMs) {
// std::cout << "update\n";
if (currentTimeMs > this->nextPublishMe) {
std::cout << "publish\n";
this->Publish(ClientMsg(this->networkId));
this->nextPublishMe = currentTimeMs + this->publishInterval;
}
}
bool Participant::SendBuffer(unsigned char bufferSize) { return false; }
bool Participant::PublishBuffer(unsigned char bufferSize) { return false; }
bool Participant::Publish(IMessage msg) {
// Send the message to the specified address and port
int bufferSize = msg.Serialize(this->buffer);
if (bufferSize <= 0)
return true;
std::cout << "Publish to " << "\n";
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0,
(struct sockaddr *)&server_addr, sizeof(server_addr));
if (sent_bytes == SOCKET_ERROR) {
std::cerr << "Error sending message" << std::endl;
closesocket(sock);
WSACleanup();
return false;
}
return true;
}
void Participant::ReceiveData(unsigned char bufferSize) {
unsigned char msgId = this->buffer[0];
switch (msgId) {
@ -41,8 +121,6 @@ void Participant::ProcessInvestigateMsg(InvestigateMsg msg) {}
void Participant::ProcessThingMsg(ThingMsg msg) {}
void Passer::Control::Participant::ProcessPoseMsg(PoseMsg msg)
{
}
void Passer::Control::Participant::ProcessPoseMsg(PoseMsg msg) {}
void Participant::ProcessCustomMsg(CustomMsg msg) {}

19
Participant.h
View File

@ -2,22 +2,37 @@
#include "Messages.h"
#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
#endif
namespace Passer {
namespace Control {
/// @brief A participant is device which can communicate with other participants
class Participant {
public:
unsigned char buffer[1024];
char buffer[1024];
long publishInterval = 3000; // 3 seconds
unsigned char networkId = 0;
Participant(const char* ipAddress, int port);
SOCKET sock;
sockaddr_in server_addr;
Participant(const char *ipAddress, int port);
virtual void Update(unsigned long currentTimeMs);
virtual bool SendBuffer(unsigned char bufferSize);
virtual bool PublishBuffer(unsigned char bufferSize);
bool Publish(IMessage msg);
void ReceiveData(unsigned char bufferSize);
protected:
long nextPublishMe = 0;
virtual void ProcessNetworkIdMsg(NetworkIdMsg msg);
virtual void ProcessInvestigateMsg(InvestigateMsg msg);
virtual void ProcessThingMsg(ThingMsg msg);

2
README.md
View File

@ -1,3 +1,3 @@
\mainpage Control Core
\mainpage Control Core for C++
Control Core contains generic functionality for Controlling Things.

3
Thing.cpp
View File

@ -5,6 +5,9 @@
#include <string.h>
Thing::Thing(unsigned char networkId, unsigned char thingType) {
this->position = Spherical16::zero;
this->orientation = SwingTwist16::identity;
this->type = thingType;
this->networkId = networkId;
this->Init();

9
Thing.h
View File

@ -110,10 +110,13 @@ public:
/// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds
virtual void Update(unsigned long currentTimeMs) {};
virtual void Update(unsigned long currentTimeMs) { currentTimeMs; };
virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
virtual void ProcessBytes(unsigned char *bytes) {};
virtual void SendBytes(char *buffer, unsigned char *ix) {
buffer;
ix;
};
virtual void ProcessBytes(char *bytes) { bytes; };
protected:
virtual void Init();

41
test/second_test.cc Normal file
View File

@ -0,0 +1,41 @@
#if GTEST
// #include <gmock/gmock.h>
// not supported using Visual Studio 2022 compiler...
#include <gtest/gtest.h>
// #include "../Thing.h"
#include <chrono>
#include "Participant.h"
#include "Thing.h"
class ControlCoreSuite2 : public ::testing::Test {
protected:
// SetUp and TearDown can be used to set up and clean up before/after each
// test
void SetUp() override {
// Initialize test data here
}
void TearDown() override {
// Clean up test data here
}
};
TEST_F(ControlCoreSuite2, Dummytest2) {
Participant participant = Participant("127.0.0.1", 7681);
ASSERT_EQ(1, 1);
}
TEST_F(ControlCoreSuite2, Basic2) {
Thing t = Thing();
unsigned long milliseconds = (unsigned long)std::chrono::steady_clock::now()
.time_since_epoch()
.count();
Thing::UpdateAll(milliseconds);
}
#endif

47
test/thing_test.cc
View File

@ -4,11 +4,50 @@
// not supported using Visual Studio 2022 compiler...
#include <gtest/gtest.h>
#include "Participant.h"
#include <chrono>
TEST(Dummy, Dummytest) {
Participant participant = Participant("127.0.0.1", 7681);
#include "Participant.h"
#include "Thing.h"
namespace Passer {
// Function to get the current time in milliseconds as unsigned long
unsigned long get_time_ms() {
auto now = std::chrono::steady_clock::now();
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
now.time_since_epoch());
return static_cast<unsigned long>(ms.count());
}
class ControlCoreSuite : public ::testing::Test {
protected:
// SetUp and TearDown can be used to set up and clean up before/after each
// test
void SetUp() override {
// Initialize test data here
}
void TearDown() override {
// Clean up test data here
}
};
TEST_F(ControlCoreSuite, Dummytest) {
// Participant participant = Participant("127.0.0.1", 7681);
ASSERT_EQ(1, 1);
}
TEST_F(ControlCoreSuite, Participant) {
Participant participant = Participant("127.0.0.1", 7681);
unsigned long milliseconds = get_time_ms();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
participant.Update(milliseconds);
milliseconds = get_time_ms();
}
ASSERT_EQ(1, 1);
}
} // namespace Passer
#endif