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

Pose is synchronized to de sim.env.

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-09 11:07:25 +02:00
parent aede0e5cd3
commit 2062e4a71c
5 changed files with 44 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EspIdf/EspIdfParticipant.cpp
View File

@ -126,8 +126,8 @@ void ParticipantUDP::Receive() {
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
#if defined(IDF_VER) #if defined(IDF_VER)
std::cout << "Sending to " << remoteParticipant->ipAddress << ":" // std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
<< remoteParticipant->port << "\n"; // << remoteParticipant->port << "\n";
int err = sendto(this->sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, int err = sendto(this->sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
sizeof(dest_addr)); sizeof(dest_addr));

2
Messages/PoseMsg.cpp
View File

@ -11,12 +11,10 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
if (thing->positionUpdated || force) { if (thing->positionUpdated || force) {
this->position = thing->GetPosition(); this->position = thing->GetPosition();
this->poseType |= Pose_Position; this->poseType |= Pose_Position;
thing->positionUpdated = false;
} }
if (thing->orientationUpdated || force) { if (thing->orientationUpdated || force) {
this->orientation = thing->GetOrientation(); this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation; this->poseType |= Pose_Orientation;
thing->orientationUpdated = false;
} }
if (thing->linearVelocityUpdated) { if (thing->linearVelocityUpdated) {
this->linearVelocity = thing->GetLinearVelocity(); this->linearVelocity = thing->GetLinearVelocity();

7
Participant.cpp
View File

@ -27,7 +27,12 @@ Participant::~Participant() {
delete[] this->ipAddress; delete[] this->ipAddress;
} }
void Participant::Update(unsigned long currentTimeMs) {} void Participant::Update(unsigned long currentTimeMs) {
for (Thing* thing : this->things) {
if (thing != nullptr)
thing->Update(currentTimeMs, true);
}
}
Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
for (Thing* thing : this->things) { for (Thing* thing : this->things) {

42
Participants/ParticipantUDP.cpp
View File

@ -32,9 +32,7 @@ ParticipantUDP::ParticipantUDP(int port) {
this->isIsolated = true; this->isIsolated = true;
} }
ParticipantUDP::ParticipantUDP(const char* ipAddress, ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
int port,
int localPort)
: Participant("127.0.0.1", localPort) { : Participant("127.0.0.1", localPort) {
if (this->port == 0) if (this->port == 0)
this->isIsolated = true; this->isIsolated = true;
@ -65,8 +63,7 @@ void ParticipantUDP::SetupUDP(int localPort,
static_cast<Windows::ParticipantUDP*>(this); static_cast<Windows::ParticipantUDP*>(this);
thisWindows->Setup(localPort, remoteIpAddress, remotePort); thisWindows->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
static_cast<Posix::ParticipantUDP*>(this);
thisPosix->Setup(localPort, remoteIpAddress, remotePort); thisPosix->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(ARDUINO) #elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino = Arduino::ParticipantUDP* thisArduino =
@ -90,6 +87,7 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
ParticipantMsg* msg = new ParticipantMsg(this->networkId); ParticipantMsg* msg = new ParticipantMsg(this->networkId);
std::cout << "Send Participant\n";
if (this->remoteSite == nullptr) if (this->remoteSite == nullptr)
this->Publish(msg); this->Publish(msg);
else else
@ -103,15 +101,17 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
} }
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
if (thing != nullptr) { if (thing == nullptr)
thing->Update(currentTimeMs); continue;
if (this->isIsolated == false) { if (this->isIsolated == false) {
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
for (Participant* sender : this->senders) this->Send(thing->owner, poseMsg);
this->Send(sender, poseMsg); BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
this->Send(thing->owner, binaryMsg);
delete poseMsg; delete poseMsg;
} }
} thing->Update(currentTimeMs, true);
} }
} }
@ -121,8 +121,7 @@ void ParticipantUDP::ReceiveUDP() {
static_cast<Windows::ParticipantUDP*>(this); static_cast<Windows::ParticipantUDP*>(this);
thisWindows->Receive(); thisWindows->Receive();
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
static_cast<Posix::ParticipantUDP*>(this);
thisPosix->Receive(); thisPosix->Receive();
#elif defined(ARDUINO) #elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino = Arduino::ParticipantUDP* thisArduino =
@ -160,7 +159,7 @@ Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
Thing* thing) { Thing* thing) {
// std::cout << "Send thing info " << (int)thing->id << " \n"; std::cout << "Send thing info [" << (int)thing->id << "] \n";
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
this->Send(remoteParticipant, thingMsg); this->Send(remoteParticipant, thingMsg);
delete thingMsg; delete thingMsg;
@ -188,8 +187,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
static_cast<Windows::ParticipantUDP*>(this); static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Send(remoteParticipant, bufferSize); return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Send(remoteParticipant, bufferSize); return thisPosix->Send(remoteParticipant, bufferSize);
#elif defined(ARDUINO) #elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino = Arduino::ParticipantUDP* thisArduino =
@ -231,8 +229,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
static_cast<Windows::ParticipantUDP*>(this); static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Publish(msg); return thisWindows->Publish(msg);
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Publish(msg); return thisPosix->Publish(msg);
#elif defined(ARDUINO) #elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino = Arduino::ParticipantUDP* thisArduino =
@ -259,8 +256,6 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize,
this->GetParticipant(senderIpAddress, senderPort); this->GetParticipant(senderIpAddress, senderPort);
if (remoteParticipant == nullptr) { if (remoteParticipant == nullptr) {
remoteParticipant = this->AddParticipant(senderIpAddress, senderPort); remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
// std::cout << "New sender " << senderIpAddress << ":" << senderPort
// << "\n";
std::cout << "New remote participant " << remoteParticipant->ipAddress std::cout << "New remote participant " << remoteParticipant->ipAddress
<< ":" << remoteParticipant->port << " " << ":" << remoteParticipant->port << " "
<< (int)remoteParticipant->networkId << "\n"; << (int)remoteParticipant->networkId << "\n";
@ -312,11 +307,14 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
}; };
} }
void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {} void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
std::cout << this->name << ": Process Participant " << (int)msg->networkId
<< "\n";
}
void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) { void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
std::cout << this->name << ": process Site Id " << (int)this->networkId // std::cout << this->name << ": process Site Id " << (int)this->networkId
<< "->" << (int)msg->networkId << "\n"; // << "->" << (int)msg->networkId << "\n";
if (this->networkId != msg->networkId) { if (this->networkId != msg->networkId) {
this->networkId = msg->networkId; this->networkId = msg->networkId;
// std::cout << this->things.size() << " things\n"; // std::cout << this->things.size() << " things\n";

7
Thing.cpp
View File

@ -2,6 +2,7 @@
#include "Participant.h" #include "Participant.h"
#include "Participants/IsolatedParticipant.h" #include "Participants/IsolatedParticipant.h"
#include "Messages/PoseMsg.h"
#include <string.h> #include <string.h>
@ -190,7 +191,11 @@ void Thing::Update(bool recursive) {
} }
void Thing::Update(unsigned long currentTimeMs, bool recursive) { void Thing::Update(unsigned long currentTimeMs, bool recursive) {
(void)currentTimeMs; // if (this->positionUpdated || this->orientationUpdated)
// OnPoseChanged callback
this->positionUpdated = false;
this->orientationUpdated = false;
if (recursive) { if (recursive) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx]; Thing* child = this->children[childIx];