Communication improvements, adding gyro msg
This commit is contained in:
parent
28e42df771
commit
92be21a9c1
@ -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,33 +2,41 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
BinaryMsg::BinaryMsg(char* buffer) {
|
|
||||||
unsigned char ix = 1;
|
|
||||||
this->networkId = buffer[ix++];
|
|
||||||
this->thingId = buffer[ix++];
|
|
||||||
this->bytes = buffer + ix; // This is only valid because the code ensures the the msg
|
|
||||||
// lifetime is shorter than the buffer lifetime...
|
|
||||||
}
|
|
||||||
|
|
||||||
BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
|
BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
|
||||||
this->networkId = networkId;
|
this->networkId = networkId;
|
||||||
this->thingId = thing->id;
|
this->thingId = thing->id;
|
||||||
this->thing = thing;
|
this->thing = thing;
|
||||||
|
unsigned char ix = BinaryMsg::length;
|
||||||
|
this->dataLength = this->thing->GenerateBinary(this->data, &ix);
|
||||||
}
|
}
|
||||||
|
|
||||||
BinaryMsg::~BinaryMsg() {}
|
BinaryMsg::BinaryMsg(char* buffer) {
|
||||||
|
unsigned char ix = 1;
|
||||||
|
this->networkId = buffer[ix++];
|
||||||
|
this->thingId = buffer[ix++];
|
||||||
|
this->dataLength = buffer[ix++];
|
||||||
|
char* data = new char[this->dataLength];
|
||||||
|
for (int i = 0; i < this->dataLength; i++)
|
||||||
|
data[i] = buffer[ix++];
|
||||||
|
this->data = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
BinaryMsg::~BinaryMsg() {
|
||||||
|
delete[] this->data;
|
||||||
|
}
|
||||||
|
|
||||||
unsigned char BinaryMsg::Serialize(char* buffer) {
|
unsigned char BinaryMsg::Serialize(char* buffer) {
|
||||||
unsigned char ix = this->length;
|
// unsigned char ix = this->length;
|
||||||
this->thing->GenerateBinary(buffer, &ix);
|
// this->dataLength = this->thing->GenerateBinary(buffer, &ix);
|
||||||
if (ix <= this->length) // in this case, no data is actually sent
|
if (this->dataLength <= 0) // in this case, no data is actually sent
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
buffer[0] = this->id;
|
unsigned char ix = 0;
|
||||||
buffer[1] = this->networkId;
|
buffer[ix++] = this->id;
|
||||||
buffer[2] = this->thingId;
|
buffer[ix++] = this->networkId;
|
||||||
return ix;
|
buffer[ix++] = this->thingId;
|
||||||
|
buffer[ix++] = this->dataLength;
|
||||||
|
return this->length + this->dataLength;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
|
@ -10,7 +10,7 @@ class BinaryMsg : public IMessage {
|
|||||||
/// @brief The message ID
|
/// @brief The message ID
|
||||||
static const unsigned char id = 0xB1;
|
static const unsigned char id = 0xB1;
|
||||||
/// @brief The length of the message without the binary data itslef
|
/// @brief The length of the message without the binary data itslef
|
||||||
static const unsigned length = 3;
|
static const unsigned length = 4;
|
||||||
|
|
||||||
/// @brief The network ID of the thing
|
/// @brief The network ID of the thing
|
||||||
unsigned char networkId;
|
unsigned char networkId;
|
||||||
@ -19,8 +19,9 @@ class BinaryMsg : public IMessage {
|
|||||||
/// @brief The thing for which the binary data is communicated
|
/// @brief The thing for which the binary data is communicated
|
||||||
Thing* thing;
|
Thing* thing;
|
||||||
|
|
||||||
|
unsigned char dataLength;
|
||||||
/// @brief The binary data which is communicated
|
/// @brief The binary data which is communicated
|
||||||
char* bytes = nullptr;
|
char* data = nullptr;
|
||||||
|
|
||||||
/// @brief Create a new message for sending
|
/// @brief Create a new message for sending
|
||||||
/// @param networkId The network ID of the thing
|
/// @param networkId The network ID of the thing
|
||||||
|
@ -1,7 +1,6 @@
|
|||||||
#include "Messages.h"
|
#include "Messages.h"
|
||||||
|
|
||||||
#include "LowLevelMessages.h"
|
#include "LowLevelMessages.h"
|
||||||
//#include "Participant.h"
|
|
||||||
#include "string.h"
|
#include "string.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
@ -10,26 +9,10 @@ namespace RoboidControl {
|
|||||||
|
|
||||||
IMessage::IMessage() {}
|
IMessage::IMessage() {}
|
||||||
|
|
||||||
// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
|
|
||||||
|
|
||||||
// IMessage::IMessage(char* buffer) {}
|
|
||||||
|
|
||||||
unsigned char IMessage::Serialize(char* buffer) {
|
unsigned char IMessage::Serialize(char* buffer) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// bool IMessage::SendMsg(ParticipantUDP *client, IMessage msg) {
|
|
||||||
// // return SendMsg(client, client.buffer, );nameLength
|
|
||||||
// return client->SendBuffer(msg.Serialize(client->buffer));
|
|
||||||
// }
|
|
||||||
|
|
||||||
// bool IMessage::Publish(ParticipantUDP *participant) {
|
|
||||||
// return participant->PublishBuffer(Serialize(participant->buffer));
|
|
||||||
// }
|
|
||||||
// bool IMessage::SendTo(ParticipantUDP *participant) {
|
|
||||||
// return participant->SendBuffer(Serialize(participant->buffer));
|
|
||||||
// }
|
|
||||||
|
|
||||||
// IMessage
|
// IMessage
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
|
||||||
|
@ -8,7 +8,7 @@ class ModelUrlMsg : public IMessage {
|
|||||||
/// @brief The message ID
|
/// @brief The message ID
|
||||||
static const unsigned char id = 0x90;
|
static const unsigned char id = 0x90;
|
||||||
/// @brief The length of the message without the URL string itself
|
/// @brief The length of the message without the URL string itself
|
||||||
static const unsigned char length = 3;
|
static const unsigned char length = 4;
|
||||||
|
|
||||||
/// @brief The network ID of the thing
|
/// @brief The network ID of the thing
|
||||||
unsigned char networkId;
|
unsigned char networkId;
|
||||||
|
@ -25,6 +25,7 @@ NameMsg::NameMsg(const char* buffer) {
|
|||||||
this->networkId = buffer[ix++];
|
this->networkId = buffer[ix++];
|
||||||
this->thingId = buffer[ix++];
|
this->thingId = buffer[ix++];
|
||||||
this->nameLength = buffer[ix++];
|
this->nameLength = buffer[ix++];
|
||||||
|
|
||||||
// the name string in the buffer is not \0 terminated!
|
// the name string in the buffer is not \0 terminated!
|
||||||
char* name = new char[this->nameLength + 1];
|
char* name = new char[this->nameLength + 1];
|
||||||
for (int i = 0; i < this->nameLength; i++)
|
for (int i = 0; i < this->nameLength; i++)
|
||||||
|
@ -29,20 +29,18 @@ Participant::~Participant() {
|
|||||||
|
|
||||||
void Participant::Update(unsigned long currentTimeMs) {
|
void Participant::Update(unsigned long currentTimeMs) {
|
||||||
for (Thing* thing : this->things) {
|
for (Thing* thing : this->things) {
|
||||||
if (thing != nullptr)
|
if (thing != nullptr)
|
||||||
thing->Update(currentTimeMs, true);
|
thing->Update(currentTimeMs, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
|
Thing* Participant::Get(unsigned char thingId) {
|
||||||
for (Thing* thing : this->things) {
|
for (Thing* thing : this->things) {
|
||||||
// if (thing->networkId == networkId && thing->id == thingId)
|
|
||||||
if (thing->id == thingId)
|
if (thing->id == thingId)
|
||||||
return thing;
|
return thing;
|
||||||
}
|
}
|
||||||
// std::cout << "Could not find thing " << this->ipAddress << ":" <<
|
std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
|
||||||
// this->port
|
<< "[" << (int)thingId << "]\n";
|
||||||
// << "[" << (int)networkId << "/" << (int)thingId << "]\n";
|
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -56,25 +54,22 @@ void Participant::Add(Thing* thing, bool checkId) {
|
|||||||
thing->id = (unsigned char)this->things.size() + 1;
|
thing->id = (unsigned char)this->things.size() + 1;
|
||||||
this->things.push_back(thing);
|
this->things.push_back(thing);
|
||||||
#endif
|
#endif
|
||||||
// std::cout << "Add thing with generated ID " << this->ipAddress << ":" <<
|
// std::cout << "Add thing with generated ID " << this->ipAddress << ":"
|
||||||
// this->port << "[" << (int)thing->networkId << "/"
|
// << this->port << "[" << (int)thing->id << "]\n";
|
||||||
// << (int)thing->id << "]\n";
|
|
||||||
} else {
|
} else {
|
||||||
Thing* foundThing = Get(thing->networkId, thing->id);
|
Thing* foundThing = Get(thing->id);
|
||||||
if (foundThing == nullptr) {
|
if (foundThing == nullptr) {
|
||||||
#if defined(NO_STD)
|
#if defined(NO_STD)
|
||||||
this->things[this->thingCount++] = thing;
|
this->things[this->thingCount++] = thing;
|
||||||
#else
|
#else
|
||||||
this->things.push_back(thing);
|
this->things.push_back(thing);
|
||||||
#endif
|
#endif
|
||||||
// std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
|
// std::cout << "Add thing " << this->ipAddress << ":" << this->port << "["
|
||||||
// "[" << (int)thing->networkId << "/"
|
|
||||||
// << (int)thing->id << "]\n";
|
// << (int)thing->id << "]\n";
|
||||||
|
} else {
|
||||||
|
// std::cout << "Did not add, existing thing " << this->ipAddress << ":"
|
||||||
|
// << this->port << "[" << (int)thing->id << "]\n";
|
||||||
}
|
}
|
||||||
// else
|
|
||||||
// std::cout << "Did not add, existing thing " << this->ipAddress << ":"
|
|
||||||
// << this->port << "["
|
|
||||||
// << (int)thing->networkId << "/" << (int)thing->id << "]\n";
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -50,7 +50,7 @@ class Participant {
|
|||||||
/// @param thingId The ID of the thing
|
/// @param thingId The ID of the thing
|
||||||
/// @return The thing if found or nullptr when no thing has been found
|
/// @return The thing if found or nullptr when no thing has been found
|
||||||
/// @note The use of the network ID is likely to disappear in future versions.
|
/// @note The use of the network ID is likely to disappear in future versions.
|
||||||
Thing* Get(unsigned char networkId, unsigned char thingId);
|
Thing* Get(unsigned char thingId);
|
||||||
/// @brief Add a new thing for this participant.
|
/// @brief Add a new thing for this participant.
|
||||||
/// @param thing The thing to add
|
/// @param thing The thing to add
|
||||||
/// @param checkId Checks the thing ID of the thing. If it is 0, a new thing
|
/// @param checkId Checks the thing ID of the thing. If it is 0, a new thing
|
||||||
|
@ -252,59 +252,75 @@ bool ParticipantUDP::Publish(IMessage* msg) {
|
|||||||
void ParticipantUDP::ReceiveData(unsigned char packetSize,
|
void ParticipantUDP::ReceiveData(unsigned char packetSize,
|
||||||
char* senderIpAddress,
|
char* senderIpAddress,
|
||||||
unsigned int senderPort) {
|
unsigned int senderPort) {
|
||||||
Participant* remoteParticipant =
|
Participant* sender =
|
||||||
this->GetParticipant(senderIpAddress, senderPort);
|
this->GetParticipant(senderIpAddress, senderPort);
|
||||||
if (remoteParticipant == nullptr) {
|
if (sender == nullptr) {
|
||||||
remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
|
sender = this->AddParticipant(senderIpAddress, senderPort);
|
||||||
std::cout << "New remote participant " << remoteParticipant->ipAddress
|
std::cout << "New remote participant " << sender->ipAddress
|
||||||
<< ":" << remoteParticipant->port << " "
|
<< ":" << sender->port << std::endl;
|
||||||
<< (int)remoteParticipant->networkId << "\n";
|
|
||||||
}
|
}
|
||||||
|
|
||||||
ReceiveData(packetSize, remoteParticipant);
|
ReceiveData(packetSize, sender);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDP::ReceiveData(unsigned char bufferSize,
|
void ParticipantUDP::ReceiveData(unsigned char bufferSize,
|
||||||
Participant* remoteParticipant) {
|
Participant* sender) {
|
||||||
unsigned char msgId = this->buffer[0];
|
unsigned char msgId = this->buffer[0];
|
||||||
std::cout << "receive msg " << (int)msgId << "\n";
|
std::cout << "receive msg " << (int)msgId << "\n";
|
||||||
|
// std::cout << " buffer size = " <<(int) bufferSize << "\n";
|
||||||
switch (msgId) {
|
switch (msgId) {
|
||||||
case ParticipantMsg::id: {
|
case ParticipantMsg::id: {
|
||||||
ParticipantMsg* msg = new ParticipantMsg(this->buffer);
|
ParticipantMsg* msg = new ParticipantMsg(this->buffer);
|
||||||
Process(remoteParticipant, msg);
|
bufferSize -= msg->length;
|
||||||
|
Process(sender, msg);
|
||||||
delete msg;
|
delete msg;
|
||||||
} break;
|
} break;
|
||||||
case SiteMsg::id: {
|
case SiteMsg::id: {
|
||||||
SiteMsg* msg = new SiteMsg(this->buffer);
|
SiteMsg* msg = new SiteMsg(this->buffer);
|
||||||
Process(remoteParticipant, msg);
|
bufferSize -= msg->length;
|
||||||
|
Process(sender, msg);
|
||||||
delete msg;
|
delete msg;
|
||||||
} break;
|
} break;
|
||||||
case InvestigateMsg::id: {
|
case InvestigateMsg::id: {
|
||||||
InvestigateMsg* msg = new InvestigateMsg(this->buffer);
|
InvestigateMsg* msg = new InvestigateMsg(this->buffer);
|
||||||
Process(remoteParticipant, msg);
|
Process(sender, msg);
|
||||||
delete msg;
|
delete msg;
|
||||||
} break;
|
} break;
|
||||||
case ThingMsg::id: {
|
case ThingMsg::id: {
|
||||||
ThingMsg* msg = new ThingMsg(this->buffer);
|
ThingMsg* msg = new ThingMsg(this->buffer);
|
||||||
Process(remoteParticipant, msg);
|
bufferSize -= msg->length;
|
||||||
|
Process(sender, msg);
|
||||||
delete msg;
|
delete msg;
|
||||||
} break;
|
} break;
|
||||||
case NameMsg::id: {
|
case NameMsg::id: {
|
||||||
NameMsg* msg = new NameMsg(this->buffer);
|
NameMsg* msg = new NameMsg(this->buffer);
|
||||||
Process(remoteParticipant, msg);
|
bufferSize -= msg->length + msg->nameLength;
|
||||||
|
Process(sender, msg);
|
||||||
|
delete msg;
|
||||||
|
} break;
|
||||||
|
case ModelUrlMsg::id: {
|
||||||
|
ModelUrlMsg* msg = new ModelUrlMsg(this->buffer);
|
||||||
|
bufferSize -= msg->length + msg->urlLength;
|
||||||
|
Process(sender, msg);
|
||||||
delete msg;
|
delete msg;
|
||||||
} break;
|
} break;
|
||||||
case PoseMsg::id: {
|
case PoseMsg::id: {
|
||||||
PoseMsg* msg = new PoseMsg(this->buffer);
|
PoseMsg* msg = new PoseMsg(this->buffer);
|
||||||
Process(remoteParticipant, msg);
|
bufferSize -= msg->length;
|
||||||
|
Process(sender, msg);
|
||||||
delete msg;
|
delete msg;
|
||||||
} break;
|
} break;
|
||||||
case BinaryMsg::id: {
|
case BinaryMsg::id: {
|
||||||
BinaryMsg* msg = new BinaryMsg(this->buffer);
|
BinaryMsg* msg = new BinaryMsg(this->buffer);
|
||||||
Process(remoteParticipant, msg);
|
bufferSize -= msg->length + msg->dataLength;
|
||||||
|
Process(sender, msg);
|
||||||
delete msg;
|
delete msg;
|
||||||
} break;
|
} break;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// Check if the buffer has been read completely
|
||||||
|
if (bufferSize > 0)
|
||||||
|
std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
|
void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
|
||||||
@ -313,8 +329,8 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
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";
|
||||||
@ -332,9 +348,9 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
|
|||||||
|
|
||||||
void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
|
void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
|
||||||
std::cout << this->name << ": process Name [" << (int)msg->networkId << "/"
|
std::cout << this->name << ": process Name [" << (int)msg->networkId << "/"
|
||||||
<< (int)msg->thingId << "]\n";
|
<< (int)msg->thingId << "] ";
|
||||||
|
|
||||||
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
Thing* thing = sender->Get(msg->thingId);
|
||||||
if (thing != nullptr) {
|
if (thing != nullptr) {
|
||||||
int nameLength = msg->nameLength;
|
int nameLength = msg->nameLength;
|
||||||
int stringLen = nameLength + 1;
|
int stringLen = nameLength + 1;
|
||||||
@ -346,15 +362,19 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
|
|||||||
// Use strncpy with bounds checking for other platforms (Arduino, POSIX,
|
// Use strncpy with bounds checking for other platforms (Arduino, POSIX,
|
||||||
// ESP-IDF)
|
// ESP-IDF)
|
||||||
strncpy(thingName, msg->name,
|
strncpy(thingName, msg->name,
|
||||||
stringLen - 1); // Leave space for null terminator
|
nameLength); // Leave space for null terminator
|
||||||
thingName[stringLen - 1] = '\0'; // Ensure null termination
|
|
||||||
#endif
|
#endif
|
||||||
thingName[nameLength] = '\0';
|
thingName[nameLength] = '\0';
|
||||||
thing->name = thingName;
|
thing->name = thingName;
|
||||||
// std::cout << "thing name = " << thing->name << " length = " <<
|
|
||||||
// nameLength
|
std::cout << thing->name;
|
||||||
// << "\n";
|
|
||||||
}
|
}
|
||||||
|
std::cout << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
|
||||||
|
std::cout << this->name << ": process model url [" << (int)msg->networkId
|
||||||
|
<< "/" << (int)msg->thingId << "]\n";
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
|
void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
|
||||||
@ -364,18 +384,15 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
|
|||||||
|
|
||||||
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
|
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
|
||||||
std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/"
|
std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/"
|
||||||
<< (int)msg->thingId << "]\n";
|
<< (int)msg->thingId << "] ";
|
||||||
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
Thing* thing = sender->Get(msg->thingId);
|
||||||
if (thing != nullptr)
|
if (thing != nullptr)
|
||||||
thing->ProcessBinary(msg->bytes);
|
thing->ProcessBinary(msg->data);
|
||||||
else {
|
else {
|
||||||
thing = this->Get(msg->networkId, msg->thingId);
|
std::cout << " unknown thing [" << (int)msg->networkId << "/"
|
||||||
if (thing != nullptr)
|
<< (int)msg->thingId << "]";
|
||||||
thing->ProcessBinary(msg->bytes);
|
|
||||||
else
|
|
||||||
std::cout << "custom msg for unknown thing [" << (int)msg->networkId
|
|
||||||
<< "/" << (int)msg->thingId << "]\n";
|
|
||||||
}
|
}
|
||||||
|
std::cout << std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Receive
|
// Receive
|
||||||
|
@ -136,6 +136,7 @@ class ParticipantUDP : public Participant {
|
|||||||
virtual void Process(Participant* sender, InvestigateMsg* msg);
|
virtual void Process(Participant* sender, InvestigateMsg* msg);
|
||||||
virtual void Process(Participant* sender, ThingMsg* msg);
|
virtual void Process(Participant* sender, ThingMsg* msg);
|
||||||
virtual void Process(Participant* sender, NameMsg* msg);
|
virtual void Process(Participant* sender, NameMsg* msg);
|
||||||
|
virtual void Process(Participant* sender, ModelUrlMsg* msg);
|
||||||
virtual void Process(Participant* sender, PoseMsg* msg);
|
virtual void Process(Participant* sender, PoseMsg* msg);
|
||||||
virtual void Process(Participant* sender, BinaryMsg* msg);
|
virtual void Process(Participant* sender, BinaryMsg* msg);
|
||||||
|
|
||||||
|
@ -25,7 +25,7 @@ SiteServer::SiteServer(int port) {
|
|||||||
SetupUDP(port, ipAddress, 0);
|
SetupUDP(port, ipAddress, 0);
|
||||||
|
|
||||||
#if !defined(NO_STD)
|
#if !defined(NO_STD)
|
||||||
//Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
|
// Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -43,22 +43,23 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
|
|||||||
void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
|
void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
|
||||||
|
|
||||||
void SiteServer::Process(Participant* sender, ThingMsg* msg) {
|
void SiteServer::Process(Participant* sender, ThingMsg* msg) {
|
||||||
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
Thing* thing = sender->Get(msg->thingId);
|
||||||
if (thing == nullptr) {
|
if (thing == nullptr) {
|
||||||
// #if defined(NO_STD)
|
// #if defined(NO_STD)
|
||||||
new Thing(sender, msg->networkId, msg->thingId,
|
new Thing(sender, (Thing::Type)msg->thingType,
|
||||||
(Thing::Type)msg->thingType);
|
msg->thingId);
|
||||||
// #else
|
// #else
|
||||||
// auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
|
// auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
|
||||||
// //Thing* newThing;
|
// //Thing* newThing;
|
||||||
// if (thingMsgProcessor != thingMsgProcessors.end()) // found item
|
// if (thingMsgProcessor != thingMsgProcessors.end()) // found item
|
||||||
// //newThing =
|
// //newThing =
|
||||||
// thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
|
// thingMsgProcessor->second(sender, msg->networkId,
|
||||||
// else
|
// msg->thingId);
|
||||||
// //newThing =
|
// else
|
||||||
// new Thing(sender, msg->networkId, msg->thingId,
|
// //newThing =
|
||||||
// (Thing::Type)msg->thingType);
|
// new Thing(sender, msg->networkId, msg->thingId,
|
||||||
// #endif
|
// (Thing::Type)msg->thingType);
|
||||||
|
// #endif
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
52
Thing.cpp
52
Thing.cpp
@ -1,8 +1,8 @@
|
|||||||
#include "Thing.h"
|
#include "Thing.h"
|
||||||
|
|
||||||
|
#include "Messages/PoseMsg.h"
|
||||||
#include "Participant.h"
|
#include "Participant.h"
|
||||||
#include "Participants/IsolatedParticipant.h"
|
#include "Participants/IsolatedParticipant.h"
|
||||||
#include "Messages/PoseMsg.h"
|
|
||||||
|
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
@ -23,14 +23,15 @@ namespace RoboidControl {
|
|||||||
// return isolatedParticipant;
|
// return isolatedParticipant;
|
||||||
// }
|
// }
|
||||||
|
|
||||||
Thing::Thing(int thingType) : Thing(IsolatedParticipant::Isolated(), thingType) {}
|
Thing::Thing(int thingType)
|
||||||
|
: Thing(IsolatedParticipant::Isolated(), thingType) {}
|
||||||
|
|
||||||
Thing::Thing(Participant* owner, Type thingType)
|
Thing::Thing(Participant* owner, Type thingType, unsigned char thingId)
|
||||||
: Thing(owner, (unsigned char)thingType) {}
|
: Thing(owner, (unsigned char)thingType, thingId) {}
|
||||||
|
|
||||||
Thing::Thing(Participant* owner, int thingType) {
|
Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
|
||||||
this->owner = owner;
|
this->owner = owner;
|
||||||
this->id = 0;
|
this->id = thingId;
|
||||||
this->type = thingType;
|
this->type = thingType;
|
||||||
this->networkId = 0;
|
this->networkId = 0;
|
||||||
|
|
||||||
@ -40,27 +41,27 @@ Thing::Thing(Participant* owner, int thingType) {
|
|||||||
this->linearVelocity = Spherical::zero;
|
this->linearVelocity = Spherical::zero;
|
||||||
this->angularVelocity = Spherical::zero;
|
this->angularVelocity = Spherical::zero;
|
||||||
|
|
||||||
// std::cout << "add thing to participant\n";
|
// std::cout << "add thing [" << (int)this->id << "] to owner "
|
||||||
owner->Add(this);
|
// << this->owner->ipAddress << ":" << this->owner->port << std::endl;
|
||||||
|
this->owner->Add(this, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
Thing::Thing(Participant* owner,
|
// Thing::Thing(Participant* owner,
|
||||||
unsigned char networkId,
|
// Type thingType,
|
||||||
unsigned char thingId,
|
// int thingId) {
|
||||||
Type thingType) {
|
// // no participant reference yet..
|
||||||
// no participant reference yet..
|
// this->owner = owner;
|
||||||
this->owner = owner;
|
// this->networkId = networkId;
|
||||||
this->networkId = networkId;
|
// this->id = thingId;
|
||||||
this->id = thingId;
|
// this->type = (unsigned char)thingType;
|
||||||
this->type = (unsigned char)thingType;
|
|
||||||
|
|
||||||
this->linearVelocity = Spherical::zero;
|
// this->linearVelocity = Spherical::zero;
|
||||||
this->angularVelocity = Spherical::zero;
|
// this->angularVelocity = Spherical::zero;
|
||||||
// std::cout << "Created thing " << (int)this->networkId << "/" <<
|
// // std::cout << "Created thing " << (int)this->networkId << "/" <<
|
||||||
// (int)this->id
|
// // (int)this->id
|
||||||
// << "\n";
|
// // << "\n";
|
||||||
owner->Add(this, false);
|
// owner->Add(this, false);
|
||||||
}
|
// }
|
||||||
|
|
||||||
void Thing::Terminate() {
|
void Thing::Terminate() {
|
||||||
// Thing::Remove(this);
|
// Thing::Remove(this);
|
||||||
@ -210,9 +211,10 @@ void Thing::UpdateThings(unsigned long currentTimeMs) {
|
|||||||
IsolatedParticipant::Isolated()->Update(currentTimeMs);
|
IsolatedParticipant::Isolated()->Update(currentTimeMs);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Thing::GenerateBinary(char* buffer, unsigned char* ix) {
|
int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
|
||||||
(void)buffer;
|
(void)buffer;
|
||||||
(void)ix;
|
(void)ix;
|
||||||
|
return 0;
|
||||||
}
|
}
|
||||||
void Thing::ProcessBinary(char* bytes) {
|
void Thing::ProcessBinary(char* bytes) {
|
||||||
(void)bytes;
|
(void)bytes;
|
||||||
|
17
Thing.h
17
Thing.h
@ -43,19 +43,21 @@ class Thing {
|
|||||||
Thing(int thingType = Type::Undetermined);
|
Thing(int thingType = Type::Undetermined);
|
||||||
/// @brief Create a new thing of the given type
|
/// @brief Create a new thing of the given type
|
||||||
/// @param thingType The predefined type of thing
|
/// @param thingType The predefined type of thing
|
||||||
Thing(Participant* participant, Type thingType = Type::Undetermined);
|
Thing(Participant* participant,
|
||||||
|
Type thingType = Type::Undetermined,
|
||||||
|
unsigned char thingId = 0);
|
||||||
/// @brief Create a new thing of the give type
|
/// @brief Create a new thing of the give type
|
||||||
/// @param thingType The custom type of the thing
|
/// @param thingType The custom type of the thing
|
||||||
Thing(Participant* participant, int thingType);
|
Thing(Participant* participant, int thingType, unsigned char thingId = 0);
|
||||||
/// @brief Create a new thing for the given participant
|
/// @brief Create a new thing for the given participant
|
||||||
/// @param participant The participant for which this thing is created
|
/// @param participant The participant for which this thing is created
|
||||||
/// @param networkId The network ID of the thing
|
/// @param networkId The network ID of the thing
|
||||||
/// @param thingId The ID of the thing
|
/// @param thingId The ID of the thing
|
||||||
/// @param thingType The type of thing
|
/// @param thingType The type of thing
|
||||||
Thing(Participant* participant,
|
// Thing(Participant* participant,
|
||||||
unsigned char networkId,
|
// unsigned char networkId,
|
||||||
unsigned char thingId,
|
// unsigned char thingId,
|
||||||
Type thingType = Type::Undetermined);
|
// Type thingType = Type::Undetermined);
|
||||||
|
|
||||||
/// @brief The participant managing this thing
|
/// @brief The participant managing this thing
|
||||||
Participant* owner;
|
Participant* owner;
|
||||||
@ -192,7 +194,8 @@ class Thing {
|
|||||||
/// @brief Function used to generate binary data for this thing
|
/// @brief Function used to generate binary data for this thing
|
||||||
/// @param buffer The byte array for thw binary data
|
/// @param buffer The byte array for thw binary data
|
||||||
/// @param ix The starting position for writing the binary data
|
/// @param ix The starting position for writing the binary data
|
||||||
virtual void GenerateBinary(char* buffer, unsigned char* ix);
|
/// @returns The size of the binary data
|
||||||
|
virtual int GenerateBinary(char* buffer, unsigned char* ix);
|
||||||
// /// @brief FUnction used to process binary data received for this thing
|
// /// @brief FUnction used to process binary data received for this thing
|
||||||
/// @param bytes The binary data
|
/// @param bytes The binary data
|
||||||
virtual void ProcessBinary(char* bytes);
|
virtual void ProcessBinary(char* bytes);
|
||||||
|
@ -5,17 +5,18 @@
|
|||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
TemperatureSensor::TemperatureSensor(Participant* participant,
|
TemperatureSensor::TemperatureSensor(Participant* participant,
|
||||||
unsigned char networkId,
|
|
||||||
unsigned char thingId)
|
unsigned char thingId)
|
||||||
: Thing(participant, networkId, thingId, Type::TemperatureSensor) {}
|
: Thing(participant, Type::TemperatureSensor, thingId) {}
|
||||||
|
|
||||||
void TemperatureSensor::SetTemperature(float temp) {
|
void TemperatureSensor::SetTemperature(float temp) {
|
||||||
this->temperature = temp;
|
this->temperature = temp;
|
||||||
}
|
}
|
||||||
|
|
||||||
void TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
|
int TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
|
||||||
|
unsigned char startIx = *ix;
|
||||||
// std::cout << "Send temperature: " << this->temperature << "\n";
|
// std::cout << "Send temperature: " << this->temperature << "\n";
|
||||||
LowLevelMessages::SendFloat16(buffer, ix, this->temperature);
|
LowLevelMessages::SendFloat16(buffer, ix, this->temperature);
|
||||||
|
return *ix - startIx;
|
||||||
}
|
}
|
||||||
|
|
||||||
void TemperatureSensor::ProcessBinary(char* bytes) {
|
void TemperatureSensor::ProcessBinary(char* bytes) {
|
||||||
|
@ -15,7 +15,7 @@ class TemperatureSensor : public Thing {
|
|||||||
/// @brief Create a temperature sensor with the given ID
|
/// @brief Create a temperature sensor with the given ID
|
||||||
/// @param networkId The network ID of the sensor
|
/// @param networkId The network ID of the sensor
|
||||||
/// @param thingId The ID of the thing
|
/// @param thingId The ID of the thing
|
||||||
TemperatureSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
|
TemperatureSensor(Participant* participant, unsigned char thingId);
|
||||||
|
|
||||||
/// @brief Manually override the measured temperature
|
/// @brief Manually override the measured temperature
|
||||||
/// @param temperature The new temperature
|
/// @param temperature The new temperature
|
||||||
@ -24,7 +24,7 @@ class TemperatureSensor : public Thing {
|
|||||||
/// @brief Function to create a binary message with the temperature
|
/// @brief Function to create a binary message with the temperature
|
||||||
/// @param buffer The byte array for thw binary data
|
/// @param buffer The byte array for thw binary data
|
||||||
/// @param ix The starting position for writing the binary data
|
/// @param ix The starting position for writing the binary data
|
||||||
void GenerateBinary(char* bytes, unsigned char* ix) override;
|
int GenerateBinary(char* bytes, unsigned char* ix) override;
|
||||||
/// @brief Function to extract the temperature received in the binary message
|
/// @brief Function to extract the temperature received in the binary message
|
||||||
/// @param bytes The binary data
|
/// @param bytes The binary data
|
||||||
virtual void ProcessBinary(char* bytes) override;
|
virtual void ProcessBinary(char* bytes) override;
|
||||||
|
@ -10,7 +10,9 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
|
|||||||
this->SetParent(parent);
|
this->SetParent(parent);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
|
int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
void TouchSensor::ProcessBinary(char* bytes) {
|
void TouchSensor::ProcessBinary(char* bytes) {
|
||||||
// if (bytes[0] == 1)
|
// if (bytes[0] == 1)
|
||||||
|
@ -25,7 +25,7 @@ class TouchSensor : public Thing {
|
|||||||
/// @brief Function to create a binary message with the temperature
|
/// @brief Function to create a binary message with the temperature
|
||||||
/// @param buffer The byte array for thw binary data
|
/// @param buffer The byte array for thw binary data
|
||||||
/// @param ix The starting position for writing the binary data
|
/// @param ix The starting position for writing the binary data
|
||||||
void GenerateBinary(char* bytes, unsigned char* ix) override;
|
int GenerateBinary(char* bytes, unsigned char* ix) override;
|
||||||
/// @brief Function to extract the temperature received in the binary message
|
/// @brief Function to extract the temperature received in the binary message
|
||||||
/// @param bytes The binary data
|
/// @param bytes The binary data
|
||||||
virtual void ProcessBinary(char* bytes) override;
|
virtual void ProcessBinary(char* bytes) override;
|
||||||
|
Loading…
x
Reference in New Issue
Block a user