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

Networked Ant random walk

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-24 17:42:37 +01:00
parent cf31f2099a
commit 25a0d5e9a4
12 changed files with 207 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
Arduino/Participant.cpp
View File

@ -51,14 +51,14 @@ void Participant::Receive() {
senderAddress.toCharArray(sender_ipAddress, 16);
int sender_port = udp.remotePort();
RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
RemoteParticipant* 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
// << "\n";
// std::cout << "New remote participant " << remoteParticipant->ipAddress
// << ":" << remoteParticipant->port << " "
// << (int)remoteParticipant->networkId << "\n";
std::cout << "New sender " << sender_ipAddress << ":" << sender_port
<< "\n";
std::cout << "New remote participant " << remoteParticipant->ipAddress
<< ":" << remoteParticipant->port << " "
<< (int)remoteParticipant->networkId << "\n";
}
ReceiveData(packetSize, remoteParticipant);
@ -69,12 +69,20 @@ void Participant::Receive() {
bool Participant::Send(RemoteParticipant* remoteParticipant, int bufferSize) {
#if defined(ARDUINO)
udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
udp.write((unsigned char*)buffer, bufferSize);
udp.endPacket();
// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
// << remoteParticipant->port << "\n";
int n = 0;
do {
if (n > 0) {
std::cout << "Retry sending\n";
delay(10);
}
n++;
udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
udp.write((unsigned char*)buffer, bufferSize);
} while (udp.endPacket() == 0 && n < 10);
std::cout << "Sent to " << remoteParticipant->ipAddress << ":"
<< remoteParticipant->port << "\n";
#endif
return true;
}

37
Messages/ModelUrlMsg.cpp
View File

@ -14,15 +14,6 @@ namespace RoboidControl {
// this->scale = scale;
// }
ModelUrlMsg::ModelUrlMsg(const char* buffer) {
unsigned char ix = 1; // first byte is msg id
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->urlLength = buffer[ix++];
this->url = &buffer[ix]; // dangerous! name should not be used anymore after
// buffer has been re-used...
}
ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
this->networkId = networkId;
this->thingId = thing->id;
@ -31,10 +22,34 @@ ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
else
this->urlLength = (unsigned char)strlen(thing->modelUrl);
this->url = thing->modelUrl; // dangerous!
//this->url = thing->modelUrl; // dangerous!
// the url string in the buffer is not \0 terminated!
char* url = new char[this->urlLength + 1];
for (int i = 0; i < this->urlLength; i++)
url[i] = thing->modelUrl[i];
url[this->urlLength] = '\0';
this->url = url;}
ModelUrlMsg::ModelUrlMsg(const char* buffer) {
unsigned char ix = 1; // first byte is msg id
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
this->urlLength = buffer[ix++];
// this->url = &buffer[ix]; // dangerous! name should not be used anymore after
// // buffer has been re-used...
// the url string in the buffer is not \0 terminated!
char* url = new char[this->urlLength + 1];
for (int i = 0; i < this->urlLength; i++)
url[i] = buffer[ix++];
url[this->urlLength] = '\0';
this->url = url;
}
ModelUrlMsg::~ModelUrlMsg() {}
ModelUrlMsg::~ModelUrlMsg() {
delete[] this->url;
}
unsigned char ModelUrlMsg::Serialize(char* buffer) {
if (this->urlLength == 0 || this->url == nullptr)

8
Messages/NameMsg.cpp
View File

@ -11,7 +11,13 @@ NameMsg::NameMsg(unsigned char networkId, Thing* thing) {
this->nameLength = 0;
else
this->nameLength = (unsigned char)strlen(thing->name);
this->name = thing->name; // dangerous!
// the name string in the buffer is not \0 terminated!
char* name = new char[this->nameLength + 1];
for (int i = 0; i < this->nameLength; i++)
name[i] = thing->name[i];
name[this->nameLength] = '\0';
this->name = name;
}
NameMsg::NameMsg(const char* buffer) {

32
Messages/PoseMsg.cpp
View File

@ -19,6 +19,33 @@ PoseMsg::PoseMsg(unsigned char networkId,
this->linearVelocity = linearVelocity;
this->angularVelocity = angularVelocity;
}
PoseMsg::PoseMsg(unsigned char networkId, Thing* thing) {
this->networkId = networkId;
this->thingId = thing->id;
this->poseType = 0;
if (thing->positionUpdated) {
this->position = thing->GetPosition();
this->poseType |= Pose_Position;
thing->positionUpdated = false;
}
if (thing->orientationUpdated) {
this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation;
thing->orientationUpdated = false;
}
if (thing->linearVelocityUpdated) {
this->linearVelocity = thing->GetLinearVelocity();
this->poseType |= Pose_LinearVelocity;
thing->linearVelocityUpdated = false;
}
if (thing->angularVelocityUpdated) {
this->angularVelocity = thing->GetAngularVelocity();
this->poseType |= Pose_AngularVelocity;
thing->angularVelocityUpdated = false;
}
}
PoseMsg::PoseMsg(const char* buffer) {
unsigned char ix = 1; // First byte is msg id
this->networkId = buffer[ix++];
@ -26,11 +53,16 @@ PoseMsg::PoseMsg(const char* buffer) {
this->poseType = buffer[ix++];
this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
// linearVelocity
// angularVelocity
}
PoseMsg::~PoseMsg() {}
unsigned char PoseMsg::Serialize(char* buffer) {
if (this->poseType == 0)
return 0;
unsigned char ix = 0;
buffer[ix++] = PoseMsg::id;
buffer[ix++] = this->networkId;

6
Messages/PoseMsg.h
View File

@ -52,6 +52,12 @@ class PoseMsg : public IMessage {
SwingTwist16 orientation,
Spherical16 linearVelocity = Spherical16(),
Spherical16 angularVelocity = Spherical16());
/// @brief Create a new message for sending
/// @param networkId he network ID of the thing
/// @param thing The thing for which the pose shouldbe sent
PoseMsg(unsigned char networkId, Thing* thing);
/// @copydoc RoboidControl::IMessage::IMessage(char*)
PoseMsg(const char* buffer);
/// @brief Destructor for the message

46
Participant.cpp
View File

@ -21,14 +21,13 @@
namespace RoboidControl {
//Participant::Participant() {}
// Participant::Participant() {}
Participant::Participant(int port) {
this->ipAddress = "0.0.0.0";
this->port = port;
this->senders.push_back(this);
this->senders.push_back(this);
// this->senders.push_back(this);
// int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
this->localPort = port;
@ -39,7 +38,7 @@ Participant::Participant(const char* ipAddress, int port) {
this->ipAddress = ipAddress;
this->port = port;
this->senders.push_back(this);
// this->senders.push_back(this);
// int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
this->localPort = port; // randomPort;
@ -82,14 +81,25 @@ void Participant::Update(unsigned long currentTimeMs) {
ParticipantMsg* msg = new ParticipantMsg(this->networkId);
this->Publish(msg);
delete msg;
//std::cout << this->name << " published ParticipantMsg\n";
// std::cout << this->name << " published ParticipantMsg\n";
// for (RemoteParticipant* sender : this->senders) {
// for (Thing* thing : this->things)
// SendThingInfo(sender, thing);
// }
this->nextPublishMe = currentTimeMs + this->publishInterval;
}
this->ReceiveUDP();
for (Thing* thing : this->things) {
if (thing != nullptr) //} && thing->GetParent() == nullptr) // update all root things
if (thing != nullptr) {
thing->Update(currentTimeMs);
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
for (RemoteParticipant* sender : this->senders)
this->Send(sender, poseMsg);
delete poseMsg;
}
}
}
@ -106,17 +116,17 @@ void Participant::ReceiveUDP() {
#endif
}
Participant* Participant::GetParticipant(const char* ipAddress, int port) {
for (Participant* sender : this->senders) {
RemoteParticipant* Participant::GetParticipant(const char* ipAddress, int port) {
for (RemoteParticipant* sender : this->senders) {
if (sender->ipAddress == ipAddress && sender->port == port)
return sender;
}
return nullptr;
}
Participant* Participant::AddParticipant(const char* ipAddress, int port) {
RemoteParticipant* Participant::AddParticipant(const char* ipAddress, int port) {
std::cout << "New Participant " << ipAddress << ":" << port << "\n";
Participant* participant = new Participant(ipAddress, port);
RemoteParticipant* participant = new RemoteParticipant(ipAddress, port);
participant->networkId = (unsigned char)this->senders.size();
this->senders.push_back(participant);
return participant;
@ -129,12 +139,15 @@ void Participant::SendThingInfo(RemoteParticipant* remoteParticipant, Thing* thi
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;
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);
this->Send(remoteParticipant, poseMsg);
delete poseMsg;
}
void Participant::PublishThingInfo(Thing* thing) {
@ -238,6 +251,7 @@ void Participant::Process(RemoteParticipant* sender, SiteMsg* msg) {
std::cout << this->name << ": process NetworkId [" << (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);
}

6
Participant.h
View File

@ -71,14 +71,14 @@ class Participant : public RemoteParticipant {
void ReceiveData(unsigned char bufferSize, RemoteParticipant* remoteParticipant);
protected:
std::list<Participant*> senders;
std::list<RemoteParticipant*> senders;
unsigned long nextPublishMe = 0;
void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
Participant* GetParticipant(const char* ipAddress, int port);
Participant* AddParticipant(const char* ipAddress, int port);
RemoteParticipant* GetParticipant(const char* ipAddress, int port);
RemoteParticipant* AddParticipant(const char* ipAddress, int port);
void ReceiveUDP();

64
RemoteParticipant.cpp
View File

@ -1,47 +1,67 @@
#include "RemoteParticipant.h"
#include <string.h>
namespace RoboidControl {
RemoteParticipant::RemoteParticipant() {}
RemoteParticipant::RemoteParticipant(const char *ipAddress, int port) {
this->ipAddress = ipAddress;
RemoteParticipant::RemoteParticipant(const char* ipAddress, int port) {
// make a copy of the ip address string
int addressLength = strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
strncpy(addressString, ipAddress, addressLength);
addressString[addressLength] = '\0';
this->ipAddress = addressString;
this->port = port;
}
Thing *RemoteParticipant::Get(unsigned char networkId, unsigned char thingId) {
for (Thing *thing : this->things) {
RemoteParticipant::~RemoteParticipant() {
delete[] this->ipAddress;
}
Thing* RemoteParticipant::Get(unsigned char networkId, unsigned char thingId) {
for (Thing* thing : this->things) {
if (thing->networkId == networkId && thing->id == thingId)
return thing;
}
// std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
// << "[" << (int)networkId << "/" << (int)thingId << "]\n";
// std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
// << "[" << (int)networkId << "/" << (int)thingId << "]\n";
return nullptr;
}
void RemoteParticipant::Add(Thing *thing) {
Thing *foundThing = Get(thing->networkId, thing->id);
if (foundThing == nullptr) {
void RemoteParticipant::Add(Thing* thing) {
if (thing->id == 0) {
// allocate a new thing ID
thing->id = this->things.size() + 1;
this->things.push_back(thing);
std::cout << "Add thing " << this->ipAddress << ":" << this->port << "["
<< (int)thing->networkId << "/" << (int)thing->id << "]\n";
} else
std::cout << "Did not add, existing thing " << this->ipAddress << ":" << this->port << "["
<< (int)thing->networkId << "/" << (int)thing->id << "]\n";
// std::cout << "Add thing with generated ID " << this->ipAddress << ":" << this->port << "[" << (int)thing->networkId << "/"
// << (int)thing->id << "]\n";
} else {
Thing* foundThing = Get(thing->networkId, thing->id);
if (foundThing == nullptr) {
this->things.push_back(thing);
// std::cout << "Add thing " << this->ipAddress << ":" << this->port << "[" << (int)thing->networkId << "/"
// << (int)thing->id << "]\n";
}
// else
// std::cout << "Did not add, existing thing " << this->ipAddress << ":" << this->port << "["
// << (int)thing->networkId << "/" << (int)thing->id << "]\n";
}
}
void RemoteParticipant::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 RemoteParticipant::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 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
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);
@ -49,4 +69,4 @@ void RemoteParticipant::UpdateAll(unsigned long currentTimeMs) {
}
}
} // namespace Control
} // namespace RoboidControl

1
RemoteParticipant.h
View File

@ -12,6 +12,7 @@ public:
RemoteParticipant();
RemoteParticipant(const char *ipAddress, int port);
~RemoteParticipant();
protected:
std::list<Thing *> things;

1
Sensors/TouchSensor.cpp
View File

@ -4,6 +4,7 @@ namespace RoboidControl {
TouchSensor::TouchSensor(RemoteParticipant* participant) : Thing(participant) {
this->touchedSomething = false;
this->type = (unsigned char) Thing::Type::TouchSensor;
}
// TouchSensor::TouchSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant, networkId, thingId) {

20
Thing.cpp
View File

@ -22,6 +22,7 @@ Thing::Thing(RemoteParticipant* participant, unsigned char thingType) {
this->linearVelocity = Spherical16::zero;
this->angularVelocity = Spherical16::zero;
// std::cout << "add thing to participant\n";
participant->Add(this);
}
@ -157,6 +158,15 @@ void Thing::Update() {
}
#endif
void Thing::Update(unsigned long currentTimeMs) {
(void)currentTimeMs;
// PoseMsg* poseMsg = new PoseMsg(this->networkId, this);
// participant->Send(remoteParticipant, poseMsg);
// delete poseMsg;
}
void Thing::GenerateBinary(char* buffer, unsigned char* ix) {
(void)buffer;
(void)ix;
@ -182,10 +192,20 @@ SwingTwist16 Thing::GetOrientation() {
return this->orientation;
}
void Thing::SetLinearVelocity(Spherical16 linearVelocity) {
this->linearVelocity = linearVelocity;
this->linearVelocityUpdated = true;
}
Spherical16 Thing::GetLinearVelocity() {
return this->linearVelocity;
}
void Thing::SetAngularVelocity(Spherical16 angularVelocity) {
this->angularVelocity = angularVelocity;
this->angularVelocityUpdated = true;
}
Spherical16 Thing::GetAngularVelocity() {
return this->angularVelocity;
}

30
Thing.h
View File

@ -28,6 +28,7 @@ class Thing {
DistanceSensor,
DirectionalSensor,
TemperatureSensor,
TouchSensor,
// Motor,
ControlledMotor,
UncontrolledMotor,
@ -123,7 +124,22 @@ class Thing {
/// @brief boolean indicating if the orientation was updated
bool orientationUpdated = false;
protected:
/// @brief Set the linear velocity of the thing
/// @param linearVelocity The new linear velocity in local space, in meters per second
void SetLinearVelocity(Spherical16 linearVelocity);
/// @brief Get the linear velocity of the thing
/// @return The linear velocity in local space, in meters per second
virtual Spherical16 GetLinearVelocity();
/// @brief Set the angular velocity of the thing
/// @param angularVelocity the new angular velocity in local space
void SetAngularVelocity(Spherical16 angularVelocity);
/// @brief Get the angular velocity of the thing
/// @return The angular velocity in local space
virtual Spherical16 GetAngularVelocity();
bool linearVelocityUpdated = false;
bool angularVelocityUpdated = false;
private:
/// @brief The position in local space
/// @remark When this Thing has a parent, the position is relative to the
/// parent's position and orientation
@ -133,17 +149,11 @@ class Thing {
/// parent's orientation
SwingTwist16 orientation;
public:
/// @brief The linear velocity in local space
Spherical16 linearVelocity;
/// @brief The angular velocity in local spze
Spherical16 angularVelocity;
/// @brief Get the linear velocity of the thing
/// @return The linear velocity in local space, in meters per second
virtual Spherical16 GetLinearVelocity();
/// @brief Get the angular velocity of the thing
/// @return The angular velocity in local space
virtual Spherical16 GetAngularVelocity();
public:
/// @brief Terminated things are no longer updated
void Terminate();
@ -161,7 +171,7 @@ class Thing {
/// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds
virtual void Update(unsigned long currentTimeMs) { (void)currentTimeMs; };
virtual void Update(unsigned long currentTimeMs); // { (void)currentTimeMs; };
/// @brief Function used to generate binary data for this thing
/// @param buffer The byte array for thw binary data