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

Collisions work!

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-26 11:13:18 +01:00
parent e7990cf00a
commit 1dd0c3a4a9
5 changed files with 56 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Arduino/Participant.cpp
View File

@ -23,7 +23,7 @@ void Participant::Setup(int localPort, const char* remoteIpAddress, int remotePo
std::cout << "No network available!\n";
return;
}
udp.begin(this->localPort);
udp.begin(localPort);
std::cout << "Wifi sync started to port " << this->remotePort << "\n";
#endif

36
Messages/PoseMsg.cpp
View File

@ -3,33 +3,33 @@
namespace RoboidControl {
PoseMsg::PoseMsg(unsigned char networkId,
unsigned char thingId,
unsigned char poseType,
Spherical16 position,
SwingTwist16 orientation,
Spherical16 linearVelocity,
Spherical16 angularVelocity) {
this->networkId = networkId;
this->thingId = thingId;
// PoseMsg::PoseMsg(unsigned char networkId,
// unsigned char thingId,
// unsigned char poseType,
// Spherical16 position,
// SwingTwist16 orientation,
// Spherical16 linearVelocity,
// Spherical16 angularVelocity) {
// this->networkId = networkId;
// this->thingId = thingId;
this->poseType = poseType;
this->position = position;
this->orientation = orientation;
this->linearVelocity = linearVelocity;
this->angularVelocity = angularVelocity;
}
PoseMsg::PoseMsg(unsigned char networkId, Thing* thing) {
// this->poseType = poseType;
// this->position = position;
// this->orientation = orientation;
// this->linearVelocity = linearVelocity;
// this->angularVelocity = angularVelocity;
// }
PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
this->networkId = networkId;
this->thingId = thing->id;
this->poseType = 0;
if (thing->positionUpdated) {
if (thing->positionUpdated || force) {
this->position = thing->GetPosition();
this->poseType |= Pose_Position;
thing->positionUpdated = false;
}
if (thing->orientationUpdated) {
if (thing->orientationUpdated || force ) {
this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation;
thing->orientationUpdated = false;

16
Messages/PoseMsg.h
View File

@ -45,18 +45,18 @@ class PoseMsg : public IMessage {
/// @param orientation The orientation of the thing in local space
/// @param linearVelocity The linear velocity of the thing in local space in meters per second
/// @param angularVelocity The angular velocity of the thing in local space
PoseMsg(unsigned char networkId,
unsigned char thingId,
unsigned char poseType,
Spherical16 position,
SwingTwist16 orientation,
Spherical16 linearVelocity = Spherical16(),
Spherical16 angularVelocity = Spherical16());
// PoseMsg(unsigned char networkId,
// unsigned char thingId,
// unsigned char poseType,
// Spherical16 position,
// 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);
PoseMsg(unsigned char networkId, Thing* thing, bool force = false);
/// @copydoc RoboidControl::IMessage::IMessage(char*)
PoseMsg(const char* buffer);

66
Participant.cpp
View File

@ -26,27 +26,16 @@ namespace RoboidControl {
Participant::Participant(int port) {
this->ipAddress = "0.0.0.0";
this->port = port;
// this->senders.push_back(this);
// int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
this->localPort = port;
// SetupUDP(randomPort, ipAddress, port);
}
Participant::Participant(const char* ipAddress, int port) {
this->ipAddress = ipAddress;
this->ipAddress = ipAddress; // maybe this is not needed anymore, keeping it to "0.0.0.0"
this->port = port;
// this->senders.push_back(this);
// int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
this->localPort = port; // randomPort;
// SetupUDP(randomPort, ipAddress, port);
this->site = new RemoteParticipant(ipAddress, port);
}
void Participant::begin() {
SetupUDP(this->localPort, this->ipAddress, this->port);
SetupUDP(this->port, this->ipAddress, this->port);
}
void Participant::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) {
@ -79,14 +68,11 @@ void Participant::Update(unsigned long currentTimeMs) {
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
ParticipantMsg* msg = new ParticipantMsg(this->networkId);
this->Publish(msg);
if (this->site == nullptr)
this->Publish(msg);
else
this->Send(this->site, msg);
delete msg;
// 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;
}
@ -135,7 +121,7 @@ RemoteParticipant* Participant::AddParticipant(const char* ipAddress, int port)
#pragma region Send
void Participant::SendThingInfo(RemoteParticipant* owner, Thing* thing) {
std::cout << "Send thing info " << thing->id << " \n";
std::cout << "Send thing info " << (int)thing->id << " \n";
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
this->Send(owner, thingMsg);
delete thingMsg;
@ -145,11 +131,28 @@ void Participant::SendThingInfo(RemoteParticipant* owner, Thing* thing) {
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
this->Send(owner, modelMsg);
delete modelMsg;
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
this->Send(owner, poseMsg);
delete poseMsg;
}
bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
return true;
#if defined(_WIN32) || defined(_WIN64)
Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);
return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::Participant* thisPosix = static_cast<Posix::Participant*>(this);
return thisPosix->Send(remoteParticipant, bufferSize);
#elif defined(ARDUINO)
Arduino::Participant* thisArduino = static_cast<Arduino::Participant*>(this);
return thisArduino->Send(remoteParticipant, bufferSize);
#endif
}
void Participant::PublishThingInfo(Thing* thing) {
// std::cout << "Publish thing info" << thing->networkId << "\n";
// Strange, when publishing, the network id is irrelevant, because it is
@ -168,23 +171,6 @@ void Participant::PublishThingInfo(Thing* thing) {
delete customMsg;
}
bool Participant::Send(RemoteParticipant* remoteParticipant, IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
return true;
#if defined(_WIN32) || defined(_WIN64)
Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);
return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::Participant* thisPosix = static_cast<Posix::Participant*>(this);
return thisPosix->Send(remoteParticipant, bufferSize);
#elif defined(ARDUINO)
Arduino::Participant* thisArduino = static_cast<Arduino::Participant*>(this);
return thisArduino->Send(remoteParticipant, bufferSize);
#endif
}
bool Participant::Publish(IMessage* msg) {
#if defined(_WIN32) || defined(_WIN64)
Windows::Participant* thisWindows = static_cast<Windows::Participant*>(this);

5
Participant.h
View File

@ -33,7 +33,8 @@ class Participant : public RemoteParticipant {
const char* name = "Participant";
int localPort = 0;
//int localPort = 0;
RemoteParticipant* site = nullptr;
#if defined(ARDUINO)
const char* remoteIpAddress = nullptr;
@ -55,7 +56,7 @@ class Participant : public RemoteParticipant {
#endif
Participant(int port = 7681);
Participant(const char* ipAddress, int port);
Participant(const char* ipAddress, int port = 7681);
void begin();
bool connected = false;