diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 5dd9ca0..f479a18 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -1,8 +1,6 @@ #include "EspIdfParticipant.h" -#if defined(IDF_VER) #include "esp_wifi.h" -#endif namespace RoboidControl { namespace EspIdf { @@ -11,6 +9,7 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(IDF_VER) +std::cout << "Set up UDP\n"; GetBroadcastAddress(); wifi_ap_record_t ap_info; @@ -21,8 +20,8 @@ void ParticipantUDP::Setup(int localPort, } // Create a UDP socket - sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (sockfd < 0) { + this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (this->sockfd < 0) { std::cout << "Unable to create UDP socket: errno " << errno << "\n"; vTaskDelete(NULL); return; @@ -38,7 +37,7 @@ void ParticipantUDP::Setup(int localPort, // Bind the socket to the address and port - if (bind(sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) { + if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) { std::cout << "Unable to bind UDP socket: errno " << errno << "\n"; close(sockfd); vTaskDelete(NULL); @@ -61,59 +60,66 @@ void ParticipantUDP::Setup(int localPort, void ParticipantUDP::GetBroadcastAddress() { // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION... - // esp_netif_ip_info_t ip_info; - // esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); + esp_netif_ip_info_t ip_info; + esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); - // // Get IP information (IP address, netmask, gateway) - // if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) { - // std::cout << "Failed to get IP info\n"; - // return; - // } + // Get IP information (IP address, netmask, gateway) + if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) { + std::cout << "Failed to get IP info\n"; + return; + } - // ip_addr_t broadcast_addr = {}; - // broadcast_addr.u_addr.ip4.addr = - // (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr; + ip_addr_t broadcast_addr = {}; + broadcast_addr.u_addr.ip4.addr = + (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr; - // snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR, - // IP2STR(&broadcast_addr.u_addr.ip4)); - // std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n"; + snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR, + IP2STR(&broadcast_addr.u_addr.ip4)); + std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n"; } void ParticipantUDP::Receive() { #if defined(IDF_VER) - struct pollfd fds; - fds.fd = sockfd; - fds.events = POLLIN; // We're looking for data available to read + struct pollfd fds[1]; + fds[0].fd = sockfd; + fds[0].events = POLLIN; // We're looking for data available to read // Use poll() with a timeout of 0 to return immediately - int ret = poll(&fds, 1, 0); + int ret = poll(fds, 1, 0); if (ret == -1) { std::cout << "poll() error\n"; return; } - socklen_t addr_len = sizeof(this->src_addr); + + //char buffer[1024]; + struct sockaddr_in source_addr; + + socklen_t addr_len = sizeof(source_addr); char sender_ipAddress[INET_ADDRSTRLEN]; - while (ret > 0 && fds.revents & POLLIN) { + while (ret > 0 && fds[0].revents & POLLIN) { int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0, - (struct sockaddr*)&this->src_addr, &addr_len); + (struct sockaddr*)&source_addr, &addr_len); if (packetSize < 0) { std::cout << "recvfrom() error\n"; return; + } else if (packetSize == 0) { + break; } - std::cout << "receiving " << packetSize << " bytes\n"; - // inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); - // unsigned int sender_port = ntohs(this->src_addr.sin_port); + // std::cout << "receiving " << packetSize << " bytes, msgId " << (int)this->buffer[0] << "\n"; + inet_ntoa_r(source_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN); + unsigned int sender_port = ntohs(source_addr.sin_port); - // ReceiveData(packetSize, sender_ipAddress, sender_port); + ReceiveData(packetSize, sender_ipAddress, sender_port); - int ret = poll(&fds, 1, 0); + ret = poll(fds, 1, 0); if (ret == -1) { std::cout << "poll() error\n"; return; } } + //std::cout << "no more messages\n"; #endif // IDF_VER } @@ -146,12 +152,6 @@ bool ParticipantUDP::Publish(IMessage* msg) { sizeof(dest_addr)); if (err != 0) std::cout << "Publish error\n"; - // udp.beginPacket(this->broadcastIpAddress, this->remotePort); - // udp.write((unsigned char*)buffer, bufferSize); - // udp.endPacket(); - - // std::cout << "Publish to " << this->broadcastIpAddress << ":" - // << this->remotePort << "\n"; #endif return true; }; diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index ecdad07..1e80f0e 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -2,9 +2,7 @@ #include "Participants/ParticipantUDP.h" -#if defined(IDF_VER) #include "lwip/sockets.h" -#endif namespace RoboidControl { namespace EspIdf { @@ -17,13 +15,12 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { bool Publish(IMessage* msg); protected: -#if defined(IDF_VER) - char broadcastIpAddress[INET_ADDRSTRLEN]; + char broadcastIpAddress[INET_ADDRSTRLEN]; // IPv4 address can have a max of 15 characters + // + null terminator int sockfd; struct sockaddr_in dest_addr; - struct sockaddr_in src_addr; -#endif + //struct sockaddr_in src_addr; void GetBroadcastAddress(); }; diff --git a/LinearAlgebra/Matrix.cpp b/LinearAlgebra/Matrix.cpp index fca4360..b725f55 100644 --- a/LinearAlgebra/Matrix.cpp +++ b/LinearAlgebra/Matrix.cpp @@ -1,6 +1,8 @@ #include "Matrix.h" #include +namespace LinearAlgebra { + #pragma region Matrix1 Matrix1::Matrix1(int size) : size(size) { @@ -12,8 +14,7 @@ Matrix1::Matrix1(int size) : size(size) { } } -Matrix1::Matrix1(float* data, int size) - : data(data), size(size) { +Matrix1::Matrix1(float* data, int size) : data(data), size(size) { this->externalData = true; } @@ -28,12 +29,7 @@ Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) { } Quaternion LinearAlgebra::Matrix1::ToQuaternion() { - return Quaternion( - this->data[0], - this->data[1], - this->data[2], - this->data[3] - ); + return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]); } // Matrix1 @@ -46,9 +42,9 @@ Matrix2::Matrix2() {} Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) { this->nValues = nRows * nCols; if (this->nValues == 0) - data = nullptr; + this->data = nullptr; else { - this->data = new float[nValues](); + this->data = new float[this->nValues]; this->externalData = false; } } @@ -59,11 +55,44 @@ Matrix2::Matrix2(float* data, int nRows, int nCols) this->externalData = true; } +Matrix2::Matrix2(const Matrix2& m) + : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) { + if (this->nValues == 0) + this->data = nullptr; + else { + this->data = new float[this->nValues]; + std::copy(m.data, m.data + nValues, this->data); + } +} + +Matrix2& Matrix2::operator=(const Matrix2& m) { + if (this != &m) { + delete[] this->data; // Free the current memory + + this->nRows = m.nRows; + this->nCols = m.nCols; + this->nValues = m.nValues; + if (this->nValues == 0) + this->data = nullptr; + else { + this->data = new float[this->nValues]; + std::copy(m.data, m.data + this->nValues, this->data); + } + } + return *this; +} + Matrix2::~Matrix2() { - if (data != nullptr && !this->externalData) + if (!this->externalData) delete[] data; } +Matrix2 Matrix2::Clone() const { + Matrix2 r = Matrix2(this->nRows, this->nCols); + std::copy(this->data, this->data + this->nValues, r.data); + return r; +} + // Move constructor Matrix2::Matrix2(Matrix2&& other) noexcept : nRows(other.nRows), @@ -94,6 +123,11 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) { return r; } +void Matrix2::Clear() { + for (int ix = 0; ix < this->nValues; ix++) + this->data[ix] = 0; +} + Matrix2 Matrix2::Identity(int size) { return Diagonal(1, size); } @@ -121,6 +155,17 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) { return r; } +Matrix2 Matrix2::Transpose() const { + Matrix2 r = Matrix2(this->nCols, this->nRows); + + for (uint rowIx = 0; rowIx < this->nRows; rowIx++) { + for (uint colIx = 0; colIx < this->nCols; colIx++) + r.data[colIx * this->nCols + rowIx] = + this->data[rowIx * this->nCols + colIx]; + } + return r; +} + Matrix2 LinearAlgebra::Matrix2::operator-() const { Matrix2 r = Matrix2(this->nRows, this->nCols); for (int ix = 0; ix < r.nValues; ix++) @@ -135,6 +180,12 @@ Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const { return r; } +Matrix2 Matrix2::operator+=(const Matrix2& v) { + for (int ix = 0; ix < this->nValues; ix++) + this->data[ix] += v.data[ix]; + return *this; +} + Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { Matrix2 r = Matrix2(this->nRows, B.nCols); @@ -149,30 +200,54 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const { int BColOffset = i * BCols; // BColOffset is constant for each row of B for (int j = 0; j < BCols; ++j) { float sum = 0; - std::cout << " 0"; + // std::cout << " 0"; int BIndex = j; for (int k = 0; k < ACols; ++k) { - std::cout << " + " << this->data[ARowOffset + k] << " * " - << B.data[BIndex]; + // std::cout << " + " << this->data[ARowOffset + k] << " * " + // << B.data[BIndex]; sum += this->data[ARowOffset + k] * B.data[BIndex]; BIndex += BCols; } r.data[BColOffset + j] = sum; - std::cout << " = " << sum << " ix: " << BColOffset + j << "\n"; + // std::cout << " = " << sum << " ix: " << BColOffset + j << "\n"; } } return r; } -void LinearAlgebra::Matrix2::SetSlice(int rowStart, - int rowStop, - int colStart, - int colStop, - const Matrix2& m) const { +Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) { + Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart); + + int resultRowIx = 0; + int resultColIx = 0; for (int i = rowStart; i < rowStop; i++) { for (int j = colStart; j < colStop; j++) - this->data[i * this->nCols + j] = - m.data[(i - rowStart) * m.nCols + (j - colStart)]; + r.data[resultRowIx * r.nCols + resultColIx] = + this->data[i * this->nCols + j]; + } + return r; +} + +void Matrix2::UpdateSlice(int rowStart, + int rowStop, + int colStart, + int colStop, + const Matrix2& m) const { + // for (int i = rowStart; i < rowStop; i++) { + // for (int j = colStart; j < colStop; j++) + // this->data[i * this->nCols + j] = + // m.data[(i - rowStart) * m.nCols + (j - colStart)]; + // } + + int rRowDataIx = rowStart * this->nCols; + int mRowDataIx = 0; + for (int rowIx = rowStart; rowIx < rowStop; rowIx++) { + rRowDataIx = rowIx * this->nCols; + // rRowDataIx += this->nCols; + mRowDataIx += m.nCols; + for (int colIx = colStart; colIx < colStop; colIx++) { + this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)]; + } } } @@ -181,7 +256,7 @@ void LinearAlgebra::Matrix2::SetSlice(int rowStart, /// @return 4x4 Omega matrix Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) { Matrix2 r = Matrix2::Zero(4, 4); - r.SetSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v)); + r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v)); // set last row to -v int ix = 3 * 4; @@ -200,6 +275,8 @@ Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) { // Matrix2 #pragma endregion +} // namespace LinearAlgebra + template <> MatrixOf::MatrixOf(unsigned int rows, unsigned int cols) { if (rows <= 0 || cols <= 0) { diff --git a/LinearAlgebra/Matrix.h b/LinearAlgebra/Matrix.h index a269bee..ef72922 100644 --- a/LinearAlgebra/Matrix.h +++ b/LinearAlgebra/Matrix.h @@ -33,10 +33,15 @@ class Matrix2 { Matrix2(); Matrix2(int nRows, int nCols); Matrix2(float* data, int nRows, int nCols); + Matrix2(const Matrix2& m); + Matrix2& operator=(const Matrix2& other); ~Matrix2(); + Matrix2 Clone() const; + static Matrix2 Zero(int nRows, int nCols); + void Clear(); static Matrix2 Identity(int size); @@ -44,12 +49,15 @@ class Matrix2 { static Matrix2 SkewMatrix(const Vector3& v); + Matrix2 Transpose() const; + Matrix2 operator-() const; /// @brief Add a matrix to this matrix /// @param m The matrix to add to this matrix /// @return The result of the addition Matrix2 operator+(const Matrix2& v) const; + Matrix2 operator+=(const Matrix2& v); Matrix2 operator*(const Matrix2& m) const; friend Matrix2 operator*(const Matrix2& m, float f) { @@ -74,14 +82,23 @@ class Matrix2 { } return r; } - // friend Matrix2 operator*(float f, const Matrix2& v) { - // Matrix2 r = Matrix2(v.nRows, v.nCols); - // for (int ix = 0; ix < r.nValues; ix++) - // r.data[ix] = f * v.data[ix]; - // return r; - // } - void SetSlice(int rowStart, + friend Matrix2 operator/(const Matrix2& m, float f) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = m.data[ix] / f; + return r; + } + friend Matrix2 operator/(float f, const Matrix2& m) { + Matrix2 r = Matrix2(m.nRows, m.nCols); + for (int ix = 0; ix < r.nValues; ix++) + r.data[ix] = f / m.data[ix]; + return r; + } + + Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop); + + void UpdateSlice(int rowStart, int rowStop, int colStart, int colStop, @@ -208,6 +225,6 @@ class MatrixOf { }; } // namespace LinearAlgebra -//using namespace LinearAlgebra; +// using namespace LinearAlgebra; #endif \ No newline at end of file diff --git a/Messages/BinaryMsg.cpp b/Messages/BinaryMsg.cpp index 7ab7f5d..7e8f6d8 100644 --- a/Messages/BinaryMsg.cpp +++ b/Messages/BinaryMsg.cpp @@ -2,33 +2,41 @@ 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) { this->networkId = networkId; this->thingId = thing->id; 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 ix = this->length; - this->thing->GenerateBinary(buffer, &ix); - if (ix <= this->length) // in this case, no data is actually sent + // unsigned char ix = this->length; + // this->dataLength = this->thing->GenerateBinary(buffer, &ix); + if (this->dataLength <= 0) // in this case, no data is actually sent return 0; - buffer[0] = this->id; - buffer[1] = this->networkId; - buffer[2] = this->thingId; - return ix; + unsigned char ix = 0; + buffer[ix++] = this->id; + buffer[ix++] = this->networkId; + buffer[ix++] = this->thingId; + buffer[ix++] = this->dataLength; + return this->length + this->dataLength; } - } // namespace RoboidControl diff --git a/Messages/BinaryMsg.h b/Messages/BinaryMsg.h index 0b03111..10adb90 100644 --- a/Messages/BinaryMsg.h +++ b/Messages/BinaryMsg.h @@ -10,7 +10,7 @@ class BinaryMsg : public IMessage { /// @brief The message ID static const unsigned char id = 0xB1; /// @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 unsigned char networkId; @@ -19,8 +19,9 @@ class BinaryMsg : public IMessage { /// @brief The thing for which the binary data is communicated Thing* thing; + unsigned char dataLength; /// @brief The binary data which is communicated - char* bytes = nullptr; + char* data = nullptr; /// @brief Create a new message for sending /// @param networkId The network ID of the thing diff --git a/Messages/Messages.cpp b/Messages/Messages.cpp index e21662f..d9f1a8c 100644 --- a/Messages/Messages.cpp +++ b/Messages/Messages.cpp @@ -1,7 +1,6 @@ #include "Messages.h" #include "LowLevelMessages.h" -//#include "Participant.h" #include "string.h" namespace RoboidControl { @@ -10,26 +9,10 @@ namespace RoboidControl { IMessage::IMessage() {} -// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); } - -// IMessage::IMessage(char* buffer) {} - unsigned char IMessage::Serialize(char* buffer) { 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 #pragma endregion diff --git a/Messages/ModelUrlMsg.h b/Messages/ModelUrlMsg.h index 27e5e25..3c35e68 100644 --- a/Messages/ModelUrlMsg.h +++ b/Messages/ModelUrlMsg.h @@ -8,7 +8,7 @@ class ModelUrlMsg : public IMessage { /// @brief The message ID static const unsigned char id = 0x90; /// @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 unsigned char networkId; diff --git a/Messages/NameMsg.cpp b/Messages/NameMsg.cpp index 31b5d0e..36bd474 100644 --- a/Messages/NameMsg.cpp +++ b/Messages/NameMsg.cpp @@ -25,6 +25,7 @@ NameMsg::NameMsg(const char* buffer) { this->networkId = buffer[ix++]; this->thingId = buffer[ix++]; this->nameLength = buffer[ix++]; + // 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++) diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index 9bafa6f..4bbb25a 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -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(); diff --git a/Participant.cpp b/Participant.cpp index 42a0d94..80b484f 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -27,17 +27,20 @@ Participant::~Participant() { delete[] this->ipAddress; } -void Participant::Update(unsigned long currentTimeMs) {} - -Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { +void Participant::Update(unsigned long currentTimeMs) { + for (Thing* thing : this->things) { + if (thing != nullptr) + thing->Update(currentTimeMs, true); + } +} + +Thing* Participant::Get(unsigned char thingId) { for (Thing* thing : this->things) { - // if (thing->networkId == networkId && thing->id == thingId) if (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)thingId << "]\n"; return nullptr; } @@ -51,25 +54,22 @@ void Participant::Add(Thing* thing, bool checkId) { thing->id = (unsigned char)this->things.size() + 1; this->things.push_back(thing); #endif - // std::cout << "Add thing with generated ID " << this->ipAddress << ":" << - // this->port << "[" << (int)thing->networkId << "/" - // << (int)thing->id << "]\n"; + // std::cout << "Add thing with generated ID " << this->ipAddress << ":" + // << this->port << "[" << (int)thing->id << "]\n"; } else { - Thing* foundThing = Get(thing->networkId, thing->id); + Thing* foundThing = Get(thing->id); if (foundThing == nullptr) { #if defined(NO_STD) this->things[this->thingCount++] = thing; #else this->things.push_back(thing); #endif - // std::cout << "Add thing " << this->ipAddress << ":" << this->port << - // "[" << (int)thing->networkId << "/" + // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "[" // << (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"; } } diff --git a/Participant.h b/Participant.h index c03fdcf..5fdffc4 100644 --- a/Participant.h +++ b/Participant.h @@ -50,7 +50,7 @@ class Participant { /// @param thingId The ID of the thing /// @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. - Thing* Get(unsigned char networkId, unsigned char thingId); + Thing* Get(unsigned char thingId); /// @brief Add a new thing for this participant. /// @param thing The thing to add /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index a443002..a406449 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -28,13 +28,12 @@ namespace RoboidControl { ParticipantUDP::ParticipantUDP(int port) { this->ipAddress = "0.0.0.0"; this->port = port; + this->remoteSite = nullptr; if (this->port == 0) 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 +57,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(this); thisWindows->Setup(localPort, remoteIpAddress, remotePort); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); thisPosix->Setup(localPort, remoteIpAddress, remotePort); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -90,6 +88,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,19 +97,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); } } @@ -120,8 +122,7 @@ void ParticipantUDP::ReceiveUDP() { static_cast(this); thisWindows->Receive(); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); thisPosix->Receive(); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -158,8 +159,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; @@ -187,8 +188,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) { static_cast(this); return thisWindows->Send(remoteParticipant, bufferSize); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); return thisPosix->Send(remoteParticipant, bufferSize); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -230,8 +230,7 @@ bool ParticipantUDP::Publish(IMessage* msg) { static_cast(this); return thisWindows->Publish(msg); #elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = - static_cast(this); + Posix::ParticipantUDP* thisPosix = static_cast(this); return thisPosix->Publish(msg); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = @@ -252,66 +251,83 @@ bool ParticipantUDP::Publish(IMessage* msg) { #pragma region Receive void ParticipantUDP::ReceiveData(unsigned char packetSize, - char* senderIpAddress, - unsigned int senderPort) { - Participant* remoteParticipant = + char* senderIpAddress, + unsigned int senderPort) { + Participant* sender = 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"; + if (sender == nullptr) { + sender = this->AddParticipant(senderIpAddress, senderPort); + std::cout << "New remote participant " << sender->ipAddress + << ":" << sender->port << std::endl; } - ReceiveData(packetSize, remoteParticipant); + ReceiveData(packetSize, sender); } void ParticipantUDP::ReceiveData(unsigned char bufferSize, - Participant* remoteParticipant) { + Participant* sender) { 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) { case ParticipantMsg::id: { ParticipantMsg* msg = new ParticipantMsg(this->buffer); - Process(remoteParticipant, msg); + bufferSize -= msg->length; + Process(sender, msg); delete msg; } break; case SiteMsg::id: { SiteMsg* msg = new SiteMsg(this->buffer); - Process(remoteParticipant, msg); + bufferSize -= msg->length; + Process(sender, msg); delete msg; } break; case InvestigateMsg::id: { InvestigateMsg* msg = new InvestigateMsg(this->buffer); - Process(remoteParticipant, msg); + Process(sender, msg); delete msg; } break; case ThingMsg::id: { ThingMsg* msg = new ThingMsg(this->buffer); - Process(remoteParticipant, msg); + bufferSize -= msg->length; + Process(sender, msg); delete msg; } break; case NameMsg::id: { 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; } break; case PoseMsg::id: { PoseMsg* msg = new PoseMsg(this->buffer); - Process(remoteParticipant, msg); + bufferSize -= msg->length; + Process(sender, msg); delete msg; } break; case BinaryMsg::id: { BinaryMsg* msg = new BinaryMsg(this->buffer); - Process(remoteParticipant, msg); + bufferSize -= msg->length + msg->dataLength; + Process(sender, msg); delete msg; } 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) { + 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 @@ -333,9 +349,9 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) { void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { 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) { int nameLength = msg->nameLength; int stringLen = nameLength + 1; @@ -347,15 +363,19 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { // Use strncpy with bounds checking for other platforms (Arduino, POSIX, // ESP-IDF) strncpy(thingName, msg->name, - stringLen - 1); // Leave space for null terminator - thingName[stringLen - 1] = '\0'; // Ensure null termination + nameLength); // Leave space for null terminator #endif thingName[nameLength] = '\0'; thing->name = thingName; - // std::cout << "thing name = " << thing->name << " length = " << - // nameLength - // << "\n"; + + std::cout << thing->name; } + 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) { @@ -365,18 +385,15 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { std::cout << this->name << ": process Binary [" << (int)msg->networkId << "/" - << (int)msg->thingId << "]\n"; - Thing* thing = sender->Get(msg->networkId, msg->thingId); + << (int)msg->thingId << "] "; + Thing* thing = sender->Get(msg->thingId); if (thing != nullptr) - thing->ProcessBinary(msg->bytes); + thing->ProcessBinary(msg->data); else { - thing = this->Get(msg->networkId, msg->thingId); - if (thing != nullptr) - thing->ProcessBinary(msg->bytes); - // else - // std::cout << "custom msg for unknown thing [" << (int)msg->networkId - // << "/" << (int)msg->thingId << "]\n"; + std::cout << " unknown thing [" << (int)msg->networkId << "/" + << (int)msg->thingId << "]"; } + std::cout << std::endl; } // Receive diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 4d82fb9..ec93af9 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -136,6 +136,7 @@ class ParticipantUDP : public Participant { virtual void Process(Participant* sender, InvestigateMsg* msg); virtual void Process(Participant* sender, ThingMsg* 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, BinaryMsg* msg); diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 4531b5b..8bde494 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -25,7 +25,7 @@ SiteServer::SiteServer(int port) { SetupUDP(port, ipAddress, 0); #if !defined(NO_STD) - //Register((unsigned char)Thing::Type::TemperatureSensor); + // Register((unsigned char)Thing::Type::TemperatureSensor); #endif } @@ -43,22 +43,23 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { void SiteServer::Process(Participant* sender, SiteMsg* 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 defined(NO_STD) - new Thing(sender, msg->networkId, msg->thingId, - (Thing::Type)msg->thingType); -// #else -// auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType); -// //Thing* newThing; -// if (thingMsgProcessor != thingMsgProcessors.end()) // found item -// //newThing = -// thingMsgProcessor->second(sender, msg->networkId, msg->thingId); -// else -// //newThing = -// new Thing(sender, msg->networkId, msg->thingId, -// (Thing::Type)msg->thingType); -// #endif + // #if defined(NO_STD) + new Thing(sender, (Thing::Type)msg->thingType, + msg->thingId); + // #else + // auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType); + // //Thing* newThing; + // if (thingMsgProcessor != thingMsgProcessors.end()) // found item + // //newThing = + // thingMsgProcessor->second(sender, msg->networkId, + // msg->thingId); + // else + // //newThing = + // new Thing(sender, msg->networkId, msg->thingId, + // (Thing::Type)msg->thingType); + // #endif } } diff --git a/Thing.cpp b/Thing.cpp index b57be7f..86e4008 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -1,5 +1,6 @@ #include "Thing.h" +#include "Messages/PoseMsg.h" #include "Participant.h" #include "Participants/IsolatedParticipant.h" @@ -22,14 +23,15 @@ namespace RoboidControl { // 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(owner, (unsigned char)thingType) {} +Thing::Thing(Participant* owner, Type thingType, unsigned char thingId) + : 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->id = 0; + this->id = thingId; this->type = thingType; this->networkId = 0; @@ -39,27 +41,27 @@ Thing::Thing(Participant* owner, int thingType) { this->linearVelocity = Spherical::zero; this->angularVelocity = Spherical::zero; - // std::cout << "add thing to participant\n"; - owner->Add(this); + // std::cout << "add thing [" << (int)this->id << "] to owner " + // << this->owner->ipAddress << ":" << this->owner->port << std::endl; + this->owner->Add(this, false); } -Thing::Thing(Participant* owner, - unsigned char networkId, - unsigned char thingId, - Type thingType) { - // no participant reference yet.. - this->owner = owner; - this->networkId = networkId; - this->id = thingId; - this->type = (unsigned char)thingType; +// Thing::Thing(Participant* owner, +// Type thingType, +// int thingId) { +// // no participant reference yet.. +// this->owner = owner; +// this->networkId = networkId; +// this->id = thingId; +// this->type = (unsigned char)thingType; - this->linearVelocity = Spherical::zero; - this->angularVelocity = Spherical::zero; - // std::cout << "Created thing " << (int)this->networkId << "/" << - // (int)this->id - // << "\n"; - owner->Add(this, false); -} +// this->linearVelocity = Spherical::zero; +// this->angularVelocity = Spherical::zero; +// // std::cout << "Created thing " << (int)this->networkId << "/" << +// // (int)this->id +// // << "\n"; +// owner->Add(this, false); +// } void Thing::Terminate() { // Thing::Remove(this); @@ -190,7 +192,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]; @@ -205,9 +211,10 @@ void Thing::UpdateThings(unsigned long currentTimeMs) { IsolatedParticipant::Isolated()->Update(currentTimeMs); } -void Thing::GenerateBinary(char* buffer, unsigned char* ix) { +int Thing::GenerateBinary(char* buffer, unsigned char* ix) { (void)buffer; (void)ix; + return 0; } void Thing::ProcessBinary(char* bytes) { (void)bytes; diff --git a/Thing.h b/Thing.h index d640d8b..febe7a9 100644 --- a/Thing.h +++ b/Thing.h @@ -43,19 +43,21 @@ class Thing { Thing(int thingType = Type::Undetermined); /// @brief Create a new thing of the given type /// @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 /// @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 /// @param participant The participant for which this thing is created /// @param networkId The network ID of the thing /// @param thingId The ID of the thing /// @param thingType The type of thing - Thing(Participant* participant, - unsigned char networkId, - unsigned char thingId, - Type thingType = Type::Undetermined); + // Thing(Participant* participant, + // unsigned char networkId, + // unsigned char thingId, + // Type thingType = Type::Undetermined); /// @brief The participant managing this thing Participant* owner; @@ -192,7 +194,8 @@ class Thing { /// @brief Function used to generate binary data for this thing /// @param buffer The byte array for thw 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 /// @param bytes The binary data virtual void ProcessBinary(char* bytes); diff --git a/Things/TemperatureSensor.cpp b/Things/TemperatureSensor.cpp index f31044a..45718a5 100644 --- a/Things/TemperatureSensor.cpp +++ b/Things/TemperatureSensor.cpp @@ -5,17 +5,18 @@ namespace RoboidControl { TemperatureSensor::TemperatureSensor(Participant* participant, - unsigned char networkId, unsigned char thingId) - : Thing(participant, networkId, thingId, Type::TemperatureSensor) {} + : Thing(participant, Type::TemperatureSensor, thingId) {} void TemperatureSensor::SetTemperature(float 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"; LowLevelMessages::SendFloat16(buffer, ix, this->temperature); + return *ix - startIx; } void TemperatureSensor::ProcessBinary(char* bytes) { diff --git a/Things/TemperatureSensor.h b/Things/TemperatureSensor.h index e6ea177..8d58d9c 100644 --- a/Things/TemperatureSensor.h +++ b/Things/TemperatureSensor.h @@ -15,7 +15,7 @@ class TemperatureSensor : public Thing { /// @brief Create a temperature sensor with the given ID /// @param networkId The network ID of the sensor /// @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 /// @param temperature The new temperature @@ -24,7 +24,7 @@ class TemperatureSensor : public Thing { /// @brief Function to create a binary message with the temperature /// @param buffer The byte array for thw 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 /// @param bytes The binary data virtual void ProcessBinary(char* bytes) override; diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index f7f65f0..b5743f6 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -10,7 +10,9 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) { 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) { // if (bytes[0] == 1) diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index 83ecc8b..39638e7 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -25,7 +25,7 @@ class TouchSensor : public Thing { /// @brief Function to create a binary message with the temperature /// @param buffer The byte array for thw 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 /// @param bytes The binary data virtual void ProcessBinary(char* bytes) override;