RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases 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) {
#if defined(IDF_VER)
std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
<< remoteParticipant->port << "\n";
// std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
// << remoteParticipant->port << "\n";
int err = sendto(this->sockfd, buffer, bufferSize, 0, (struct sockaddr*)&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) {
this->position = thing->GetPosition();
this->poseType |= Pose_Position;
thing->positionUpdated = false;
}
if (thing->orientationUpdated || force) {
this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation;
thing->orientationUpdated = false;
}
if (thing->linearVelocityUpdated) {
this->linearVelocity = thing->GetLinearVelocity();

7
Participant.cpp
View File

@ -27,7 +27,12 @@ Participant::~Participant() {
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) {
for (Thing* thing : this->things) {

62
Participants/ParticipantUDP.cpp
View File

@ -32,9 +32,7 @@ ParticipantUDP::ParticipantUDP(int port) {
this->isIsolated = true;
}
ParticipantUDP::ParticipantUDP(const char* ipAddress,
int port,
int localPort)
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
: Participant("127.0.0.1", localPort) {
if (this->port == 0)
this->isIsolated = true;
@ -58,15 +56,14 @@ void ParticipantUDP::begin() {
}
void ParticipantUDP::SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) {
const char* remoteIpAddress,
int remotePort) {
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
thisWindows->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix =
static_cast<Posix::ParticipantUDP*>(this);
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
thisPosix->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
@ -90,6 +87,7 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
ParticipantMsg* msg = new ParticipantMsg(this->networkId);
std::cout << "Send Participant\n";
if (this->remoteSite == nullptr)
this->Publish(msg);
else
@ -98,20 +96,22 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) {
this->nextPublishMe = currentTimeMs + this->publishInterval;
}
this->ReceiveUDP();
}
for (Thing* thing : this->things) {
if (thing != nullptr) {
thing->Update(currentTimeMs);
if (this->isIsolated == false) {
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
for (Participant* sender : this->senders)
this->Send(sender, poseMsg);
delete poseMsg;
}
if (thing == nullptr)
continue;
if (this->isIsolated == false) {
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
this->Send(thing->owner, poseMsg);
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
this->Send(thing->owner, binaryMsg);
delete poseMsg;
}
thing->Update(currentTimeMs, true);
}
}
@ -121,8 +121,7 @@ void ParticipantUDP::ReceiveUDP() {
static_cast<Windows::ParticipantUDP*>(this);
thisWindows->Receive();
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix =
static_cast<Posix::ParticipantUDP*>(this);
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
thisPosix->Receive();
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
@ -159,8 +158,8 @@ Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
#pragma region Send
void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
Thing* thing) {
// std::cout << "Send thing info " << (int)thing->id << " \n";
Thing* thing) {
std::cout << "Send thing info [" << (int)thing->id << "] \n";
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
this->Send(remoteParticipant, thingMsg);
delete thingMsg;
@ -188,8 +187,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix =
static_cast<Posix::ParticipantUDP*>(this);
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Send(remoteParticipant, bufferSize);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
@ -231,8 +229,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Publish(msg);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix =
static_cast<Posix::ParticipantUDP*>(this);
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Publish(msg);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
@ -253,14 +250,12 @@ bool ParticipantUDP::Publish(IMessage* msg) {
#pragma region Receive
void ParticipantUDP::ReceiveData(unsigned char packetSize,
char* senderIpAddress,
unsigned int senderPort) {
char* senderIpAddress,
unsigned int senderPort) {
Participant* remoteParticipant =
this->GetParticipant(senderIpAddress, senderPort);
if (remoteParticipant == nullptr) {
remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
// std::cout << "New sender " << senderIpAddress << ":" << senderPort
// << "\n";
std::cout << "New remote participant " << remoteParticipant->ipAddress
<< ":" << remoteParticipant->port << " "
<< (int)remoteParticipant->networkId << "\n";
@ -270,7 +265,7 @@ void ParticipantUDP::ReceiveData(unsigned char packetSize,
}
void ParticipantUDP::ReceiveData(unsigned char bufferSize,
Participant* remoteParticipant) {
Participant* remoteParticipant) {
unsigned char msgId = this->buffer[0];
// std::cout << "receive msg " << (int)msgId << "\n";
switch (msgId) {
@ -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) {
std::cout << this->name << ": process Site Id " << (int)this->networkId
<< "->" << (int)msg->networkId << "\n";
// std::cout << this->name << ": process Site Id " << (int)this->networkId
// << "->" << (int)msg->networkId << "\n";
if (this->networkId != msg->networkId) {
this->networkId = msg->networkId;
// std::cout << this->things.size() << " things\n";

7
Thing.cpp
View File

@ -2,6 +2,7 @@
#include "Participant.h"
#include "Participants/IsolatedParticipant.h"
#include "Messages/PoseMsg.h"
#include <string.h>
@ -190,7 +191,11 @@ void Thing::Update(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) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx];