diff --git a/Participant.cpp b/Participant.cpp index a251df0..3699e9f 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -89,7 +89,7 @@ void Participant::Update(unsigned long currentTimeMs) { } this->ReceiveUDP(); - Thing::UpdateAll(currentTimeMs); + this->UpdateAll(currentTimeMs); } void Participant::ReceiveUDP() { @@ -187,6 +187,7 @@ bool Participant::Publish(IMessage *msg) { void Participant::ReceiveData(unsigned char bufferSize, Participant *remoteParticipant) { unsigned char msgId = this->buffer[0]; + std::cout << "receive msg " << (int)msgId << "\n"; switch (msgId) { case ClientMsg::id: { ClientMsg *msg = new ClientMsg(this->buffer); @@ -241,7 +242,7 @@ void Participant::Process(Participant *sender, NetworkIdMsg *msg) { // sender->SendThingInfo(thing); // } - for (Thing *thing : Thing::allThings) { + for (Thing *thing : this->things) { sender->SendThingInfo(thing); } } @@ -252,7 +253,7 @@ void Participant::Process(InvestigateMsg *msg) {} void Participant::Process(ThingMsg *msg) {} void Participant::Process(NameMsg *msg) { - Thing *thing = Thing::Get(msg->networkId, msg->thingId); + Thing *thing = this->Get(msg->networkId, msg->thingId); if (thing != nullptr) { char *thingName = new char[strlen(msg->name)]; strcpy(thingName, msg->name); @@ -264,15 +265,66 @@ void Participant::Process(NameMsg *msg) { void Participant::Process(PoseMsg *msg) {} void Participant::Process(CustomMsg *msg) { - Thing *thing = Thing::Get(msg->networkId, msg->thingId); + Thing *thing = this->Get(msg->networkId, msg->thingId); if (thing != nullptr) thing->ProcessBytes(msg->bytes); else std::cout << "custom msg for unknown thing " << (int)msg->networkId << ":" << (int)msg->thingId << "\n"; + std::cout << "Processed custom msg\n"; } // Receive #pragma endregion + +#pragma region Things + +Thing *Participant::Get(unsigned char networkId, unsigned char thingId) { + std::cout << "Get " << (int)networkId << "/" << (int)thingId << " from " + << this->things.size() << " things\n"; + for (auto &thing : this->things) { + std::cout << " ? " << (int)thing->networkId << "/" << (int)thing->id + << "\n"; + if (thing->networkId == networkId && thing->id == thingId) { + return thing; + } + } + return nullptr; +} + +int Participant::Add(Thing *newThing) { + for (Thing *thing : this->things) { + if (thing == newThing) { + std::cout << "Thing already exists, not adding\n"; + return thing->id; + } + } + std::cout << "Adding " << (int)newThing->networkId << "/" << (int)newThing->id + << "\n"; + this->things.push_back(newThing); + return this->things.size(); +} + +void Participant::Remove(Thing *thing) { + this->things.remove_if([thing](Thing *obj) { return obj == thing; }); + std::cout << "Removing " << thing->networkId << "/" << thing->id + << " list size = " << this->things.size() << "\n"; +} + +void Participant::UpdateAll(unsigned long currentTimeMs) { + // Not very efficient, but it works for now. + + for (Thing *thing : this->things) { + if (thing != nullptr && + thing->GetParent() == nullptr) { // update all root things + // std::cout << " update " << (int)ix << " thingid " << (int)thing->id + // << "\n"; + thing->Update(currentTimeMs); + } + } +} + +#pragma endregion + } // namespace Control } // namespace Passer diff --git a/Participant.h b/Participant.h index 977b960..189fddf 100644 --- a/Participant.h +++ b/Participant.h @@ -61,15 +61,18 @@ public: Participant(); Participant(int port); Participant(const char *ipAddress, int port); - // Bad design, you cannot use constructor in global scope - // i.e. - // Participant p = Participant("127.0.0.1", 8000); void begin(); bool connected = false; virtual void Update(unsigned long currentTimeMs = 0); + std::list things; + Thing *Get(unsigned char networkId, unsigned char thingId); + int Add(Thing *thing); + void Remove(Thing *thing); + void UpdateAll(unsigned long currentTimeMs); + void SendThingInfo(Thing *thing); void PublishThingInfo(Thing *thing); diff --git a/SiteServer.cpp b/SiteServer.cpp index 2fc9c48..f48ec3f 100644 --- a/SiteServer.cpp +++ b/SiteServer.cpp @@ -22,22 +22,6 @@ SiteServer::SiteServer(int port) { Register((unsigned char)Thing::Type::TemperatureSensor); } -// void SiteServer::Update(unsigned long currentTimeMs) { -// if (currentTimeMs == 0) { -// #if defined(ARDUINO) -// currentTimeMs = millis(); -// #elif defined(__unix__) || defined(__APPLE__) -// #elif defined(__unix__) || defined(__APPLE__) -// auto now = std::chrono::steady_clock::now(); -// auto ms = std::chrono::duration_cast( -// now.time_since_epoch()); -// currentTimeMs = static_cast(ms.count()); -// #endif -// } -// this->ReceiveUDP(); -// Thing::UpdateAll(currentTimeMs); -// } - void SiteServer::Process(Participant *sender, ClientMsg *msg) { if (msg->networkId == 0) { std::cout << this->name << " received New Client -> " << sender->ipAddress @@ -52,14 +36,19 @@ void SiteServer::Process(Participant *sender, NetworkIdMsg *msg) {} void SiteServer::Process(ThingMsg *msg) { - Thing *thing = Thing::Get(msg->networkId, msg->thingId); + Thing *thing = this->Get(msg->networkId, msg->thingId); if (thing == nullptr) { + std::cout << "could not find thing " << (int)msg->networkId << "/" + << (int)msg->thingId << "\n"; auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType); - if (thingMsgProcessor != thingMsgProcessors.end()) // found item - thingMsgProcessor->second(msg->networkId, msg->thingId); - else - new Thing(this, msg->networkId, msg->thingId, - (Thing::Type)msg->thingType); + if (thingMsgProcessor != thingMsgProcessors.end()) { // found item + Thing *newThing = thingMsgProcessor->second(msg->networkId, msg->thingId); + this->Add(newThing); + } else { + Thing *newThing = new Thing(this, msg->networkId, msg->thingId, + (Thing::Type)msg->thingType); + this->Add(newThing); + } } } diff --git a/SiteServer.h b/SiteServer.h index dbe6ff3..a87f59e 100644 --- a/SiteServer.h +++ b/SiteServer.h @@ -19,7 +19,7 @@ public: template void Register(unsigned char thingType) { thingMsgProcessors[thingType] = [](unsigned char networkId, unsigned char thingId) { - return std::make_unique(networkId, thingId); + return new ThingClass(networkId, thingId); }; }; @@ -30,8 +30,8 @@ protected: virtual void Process(Participant *sender, NetworkIdMsg *msg) override; virtual void Process(ThingMsg *msg) override; - using ThingConstructor = std::function( - unsigned char networkId, unsigned char thingId)>; + using ThingConstructor = + std::function; std::unordered_map thingMsgProcessors; }; diff --git a/Thing.cpp b/Thing.cpp index 0f59e10..b3237f8 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -12,17 +12,18 @@ Thing::Thing(unsigned char thingType) { // this->position = Spherical16::zero; // this->orientation = SwingTwist16::identity; + this->id = 0; this->type = thingType; this->networkId = 0; this->Init(); - int thingId = Thing::Add(this); + // int thingId = Thing::Add(this); - if (thingId < 0) { - std::cout << "ERROR: Thing store is full\n"; - this->id = 0; // what to do when we cannot store any more things? - } else - this->id = thingId; + // if (thingId < 0) { + // std::cout << "ERROR: Thing store is full\n"; + // this->id = 0; // what to do when we cannot store any more things? + // } else + // this->id = thingId; this->linearVelocity = Spherical16::zero; this->angularVelocity = Spherical16::zero; @@ -36,21 +37,23 @@ Passer::Control::Thing::Thing(Participant *participant, unsigned char networkId, this->type = (unsigned char)thingType; this->Init(); - thingId = Thing::Add(this); + // thingId = Thing::Add(this); - if (thingId < 0) { - std::cout << "ERROR: Thing store is full\n"; - this->id = 0; // what to do when we cannot store any more things? - } else - this->id = thingId; + // if (thingId < 0) { + // std::cout << "ERROR: Thing store is full\n"; + // this->id = 0; // what to do when we cannot store any more things? + // } else + // this->id = thingId; this->linearVelocity = Spherical16::zero; this->angularVelocity = Spherical16::zero; - std::cout << "Added thing " << (int)this->networkId << "/" << (int)this->id + std::cout << "Created thing " << (int)this->networkId << "/" << (int)this->id << "\n"; } -void Thing::Terminate() { Thing::Remove(this); } +void Thing::Terminate() { + // Thing::Remove(this); +} void Thing::Init() {} @@ -178,40 +181,48 @@ Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; } Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; } // All things -std::list Thing::allThings; +// std::list Thing::allThings; -Thing *Thing::Get(unsigned char networkId, unsigned char thingId) { +// Thing *Thing::Get(unsigned char networkId, unsigned char thingId) { +// std::cout << "Get " << (int)networkId << "/" << (int)thingId << " from " +// << allThings.size() << " things\n"; +// for (auto &thing : allThings) { +// std::cout << " ? " << (int)thing->networkId << "/" << (int)thing->id +// << "\n"; +// if (thing->networkId == networkId && thing->id == thingId) { +// return thing; +// } +// } +// return nullptr; +// } - for (auto &thing : allThings) { - if (thing->networkId == networkId && thing->id == thingId) { - return thing; - } - } - return nullptr; -} +// int Thing::Add(Thing *newThing) { +// for (Thing *thing : allThings) { +// if (thing == newThing) +// return thing->id; +// } +// std::cout << "Adding " << (int)newThing->networkId << "/" << +// (int)newThing->id +// << "\n"; +// allThings.push_back(newThing); +// return allThings.size(); +// } -int Thing::Add(Thing *newThing) { - for (Thing *thing : allThings) { - if (thing == newThing) - return thing->id; - } - allThings.push_back(newThing); - return allThings.size(); -} +// void Thing::Remove(Thing *thing) { +// Thing::allThings.remove_if([thing](Thing *obj) { return obj == thing; }); +// std::cout << "Removing " << thing->networkId << "/" << thing->id +// << " list size = " << allThings.size() << "\n"; +// } -void Thing::Remove(Thing *thing) { - Thing::allThings.remove_if([thing](Thing *obj) { return obj == thing; }); -} +// void Thing::UpdateAll(unsigned long currentTimeMs) { +// // Not very efficient, but it works for now. -void Thing::UpdateAll(unsigned long currentTimeMs) { - // Not very efficient, but it works for now. - - for (Thing *thing : Thing::allThings) { - if (thing != nullptr && - thing->parent == nullptr) { // update all root things - // std::cout << " update " << (int)ix << " thingid " << (int)thing->id - // << "\n"; - thing->Update(currentTimeMs); - } - } -} \ No newline at end of file +// for (Thing *thing : Thing::allThings) { +// if (thing != nullptr && +// thing->parent == nullptr) { // update all root things +// // std::cout << " update " << (int)ix << " thingid " << (int)thing->id +// // << "\n"; +// thing->Update(currentTimeMs); +// } +// } +//} \ No newline at end of file diff --git a/Thing.h b/Thing.h index b4b29de..7f5e6c9 100644 --- a/Thing.h +++ b/Thing.h @@ -128,13 +128,13 @@ protected: virtual void Init(); //------------ All things -public: - static Thing *Get(unsigned char networkId, unsigned char thingId); - static int Add(Thing *thing); - static void Remove(Thing *thing); - static void UpdateAll(unsigned long currentTimeMs); + // public: + // static Thing *Get(unsigned char networkId, unsigned char thingId); + // static int Add(Thing *thing); + // static void Remove(Thing *thing); + // static void UpdateAll(unsigned long currentTimeMs); - static std::list allThings; + // static std::list allThings; }; // static std::list allThings; diff --git a/UdpPosix.cpp b/UdpPosix.cpp index 3321f54..a17b27d 100644 --- a/UdpPosix.cpp +++ b/UdpPosix.cpp @@ -16,20 +16,8 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress, #if defined(__unix__) || defined(__APPLE__) // Create a UDP socket -#if defined(_WIN32) || defined(_WIN64) - // Windows-specific Winsock initialization - WSADATA wsaData; - if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) { - std::cerr << "WSAStartup failed" << std::endl; - return; - } -#endif -#if defined(_WIN32) || defined(_WIN64) - this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); -#elif defined(__unix__) || defined(__APPLE__) this->sock = socket(AF_INET, SOCK_DGRAM, 0); -#endif if (this->sock < 0) { std::cerr << "Error creating socket" << std::endl; @@ -52,12 +40,7 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress, remote_addr.sin_port = htons(remotePort); if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) { std::cerr << "Invalid address" << std::endl; -#if defined(_WIN32) || defined(_WIN64) - closesocket(sock); - WSACleanup(); -#elif defined(__unix__) || defined(__APPLE__) close(sock); -#endif return; } } @@ -68,12 +51,7 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress, server_addr.sin_port = htons(localPort); if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) { std::cerr << "Invalid address" << std::endl; -#if defined(_WIN32) || defined(_WIN64) - closesocket(sock); - WSACleanup(); -#elif defined(__unix__) || defined(__APPLE__) close(sock); -#endif return; } @@ -81,12 +59,7 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress, if (bind(this->sock, (const struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { std::cerr << "Bind failed" << std::endl; -#if defined(_WIN32) || defined(_WIN64) - closesocket(sock); - WSACleanup(); -#elif defined(__unix__) || defined(__APPLE__) close(sock); -#endif } #endif @@ -94,29 +67,11 @@ void UdpPosix::Setup(int localPort, const char *remoteIpAddress, void UdpPosix::Receive() { #if defined(__unix__) || defined(__APPLE__) - // char ip_str[INET_ADDRSTRLEN]; - // inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN); - // std::cout << this->name << " Receive on " << ip_str << ":" - // << ntohs(server_addr.sin_port) << "\n"; - sockaddr_in client_addr; -#if defined(_WIN32) || defined(_WIN64) - int len = sizeof(client_addr); -#elif defined(__unix__) || defined(__APPLE__) socklen_t len = sizeof(client_addr); -#endif int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr *)&client_addr, &len); - // std::cout << "received data " << packetSize << "\n"; - if (packetSize < 0) { -#if defined(_WIN32) || defined(_WIN64) - int error_code = WSAGetLastError(); // Get the error code on Windows - if (error_code != WSAEWOULDBLOCK) - std::cerr << "recvfrom failed with error: " << error_code << std::endl; -#else - // std::cerr << "recvfrom failed with error: " << packetSize << std::endl; -#endif - } else if (packetSize > 0) { + if (packetSize > 0) { char sender_ipAddress[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, INET_ADDRSTRLEN); @@ -134,6 +89,7 @@ void UdpPosix::Receive() { } ReceiveData(packetSize, remoteParticipant); + std::cout << "Received data\n"; } #endif } @@ -142,7 +98,6 @@ bool UdpPosix::Send( IMessage *msg) { // Send the message to the specified address and port #if defined(__unix__) || defined(__APPLE__) int bufferSize = msg->Serialize(this->buffer); - // std::cout << "buffer size " << bufferSize << "\n"; if (bufferSize <= 0) return true; @@ -152,21 +107,11 @@ bool UdpPosix::Send( << "\n"; int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr *)&remote_addr, sizeof(remote_addr)); -#if defined(_WIN32) || defined(_WIN64) - if (sent_bytes <= SOCKET_ERROR) { - int error_code = WSAGetLastError(); // Get the error code on Windows - std::cerr << "sendto failed with error: " << error_code << std::endl; - closesocket(sock); - WSACleanup(); - return false; - } -#elif defined(__unix__) || defined(__APPLE__) if (sent_bytes < 0) { std::cerr << "sendto failed with error: " << sent_bytes << std::endl; close(sock); return false; } -#endif #endif return true; } @@ -184,21 +129,11 @@ bool UdpPosix::Publish(IMessage *msg) { int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr *)&broadcast_addr, sizeof(broadcast_addr)); -#if defined(_WIN32) || defined(_WIN64) - if (sent_bytes <= SOCKET_ERROR) { - int error_code = WSAGetLastError(); // Get the error code on Windows - std::cerr << "sendto failed with error: " << error_code << std::endl; - closesocket(sock); - WSACleanup(); - return false; - } -#elif defined(__unix__) || defined(__APPLE__) if (sent_bytes < 0) { std::cerr << "sendto failed with error: " << sent_bytes << std::endl; close(sock); return false; } -#endif #endif return true; } diff --git a/test/second_test.cc b/test/second_test.cc index 0410c4e..6f706ad 100644 --- a/test/second_test.cc +++ b/test/second_test.cc @@ -35,7 +35,8 @@ TEST_F(ControlCoreSuite2, Basic2) { .time_since_epoch() .count(); - Thing::UpdateAll(milliseconds); + // Thing::UpdateAll(milliseconds); + t.Update(milliseconds); } #endif