diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index a787d5a..4897569 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -1,4 +1,5 @@ #include "ArduinoParticipant.h" +#if defined(ARDUINO) #if !defined(NO_STD) #include @@ -21,7 +22,6 @@ #endif namespace RoboidControl { -namespace Arduino { #if defined(ARDUINO) && defined(HAS_WIFI) WiFiUDP* udp; @@ -88,7 +88,7 @@ void ParticipantUDP::Receive() { #endif } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) { #if defined(ARDUINO) && defined(HAS_WIFI) // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" // << remoteParticipant->port << "\n"; @@ -131,5 +131,5 @@ bool ParticipantUDP::Publish(IMessage* msg) { return true; }; -} // namespace Arduino } // namespace RoboidControl +#endif \ No newline at end of file diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index 5a9396f..7531302 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -1,15 +1,15 @@ #pragma once +#if defined(ARDUINO) #include "Participants/ParticipantUDP.h" namespace RoboidControl { -namespace Arduino { -class ParticipantUDP : public RoboidControl::ParticipantUDP { +class ParticipantUDP : public ParticipantUDPGeneric { public: void Setup(); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: @@ -18,5 +18,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { void GetBroadcastAddress(); }; -} // namespace Arduino } // namespace RoboidControl +#endif \ No newline at end of file diff --git a/Arduino/Examples/BB2A/BB2A.cpp b/Arduino/Examples/BB2A/BB2A.cpp new file mode 100644 index 0000000..ada5a24 --- /dev/null +++ b/Arduino/Examples/BB2A/BB2A.cpp @@ -0,0 +1,95 @@ +#include "Arduino.h" + +#include "Things/ControlledMotor.h" +#include "Things/DifferentialDrive.h" +#include "Things/RelativeEncoder.h" +#include "Things/TouchSensor.h" + +#include "Arduino/Things/DRV8833.h" +#include "Arduino/Things/DigitalInput.h" +#include "Arduino/Things/UltrasonicSensor.h" +#include "Arduino/ArduinoUtils.h" + +#include "Participants/ParticipantUDP.h" + +#include "configuration.h" + +#include + +using namespace RoboidControl; +using namespace RoboidControl::Arduino; + +ParticipantUDPGeneric* localParticipant; + +DifferentialDrive* bb2b; +TouchSensor* touchLeft; +TouchSensor* touchRight; + +// RelativeEncoder* encoderLeft; +// RelativeEncoder* encoderRight; + +void setup() { + Serial.begin(115200); + delay(3000); + Serial.println("started"); + + StartWifi("serrarens", "192.168.76.44"); + localParticipant = new ParticipantUDPGeneric("192.168.77.76"); + + bb2b = new DifferentialDrive(); + touchLeft = new TouchSensor(bb2b); + touchRight = new TouchSensor(bb2b); + + // bb2b = new DRV8833::DifferentialDrive(driveConfig); + // touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b); + // touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b); + + touchLeft->name = "Left Touch Sensor"; + touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0)); + touchRight->name = "Right Touch Sensor"; + touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0)); + + // encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig); + // encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig); + + // bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b); + // bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b); +} + +void loop() { + + // std::cout << touchLeft->touchedSomething << " | " + // << touchRight->touchedSomething << std::endl; + // std::cout << encoderLeft->rotationSpeed << " : " + // << encoderRight->rotationSpeed << std::endl; + // std::cout << bb2b->leftWheel->encoder->rotationSpeed + // << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl; + + + // The left wheel turns forward when nothing is touched on the right side + // and turn backward when the roboid hits something on the right + float leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f; + // The right wheel does the same, but instead is controlled by + // touches on the left side + float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f; + // When both sides are touching something, both wheels will turn backward + // and the roboid will move backwards + + bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity); + bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity); + // std::cout << " " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl; + + // float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f; + // float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f; + // bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity); + // std::cout << " " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl; + + // std::cout << leftMotor->actualVelocity << std::endl; + + //Serial.println("."); + // Update the roboid state + localParticipant->Update(); + + // and sleep for 100ms + delay(10); +} diff --git a/Arduino/Examples/BB2A/configuration.h b/Arduino/Examples/BB2A/configuration.h new file mode 100644 index 0000000..dc4e4e1 --- /dev/null +++ b/Arduino/Examples/BB2A/configuration.h @@ -0,0 +1,52 @@ +#pragma once + +#include "Arduino/Things/UltrasonicSensor.h" +#include "Arduino/Things/DRV8833.h" +#include "Arduino/Things/DigitalInput.h" + +using namespace RoboidControl::Arduino; + +#if defined(ESP32) +static constexpr DRV8833::Configuration driveConfig = { + 17, // AIn1 + 16, // AIn2 + 14, // BIn1 + 27 // BIn2 +}; +static constexpr UltrasonicSensor::Configuration leftTouchConfig = { + 25, // Trigger + 33 // Echo +}; +static constexpr UltrasonicSensor::Configuration rightTouchConfig = { + 15, // Trigger + 5 // Echo +}; + +#elif defined(UNO) || defined(UNO_R4) +static constexpr DRV8833::Configuration driveConfig = { + 5, // AIn1 + 6, // AIn2 + 7, // BIn1 + 10 // BIn2 +}; + +static constexpr UltrasonicSensor::Configuration leftTouchConfig = { + A0, // Trigger + 12 // Echo +}; +static constexpr UltrasonicSensor::Configuration rightTouchConfig = { + A1, // Trigger + 11 // Echo +}; + +static constexpr DigitalInput::RelativeEncoder::Configuration + leftEncoderConfig = { + 2, // Input pin + 80 // Pulses per revolution +}; +static constexpr DigitalInput::RelativeEncoder::Configuration + rightEncoderConfig = { + 3, // Input pin + 80 // Pulses per revolution +}; +#endif \ No newline at end of file diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index ed04d79..9ab9839 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -26,14 +26,15 @@ DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, Thing* parent) - : RoboidControl::DifferentialDrive(this->drv8833.motorA, - this->drv8833.motorB, - parent), - drv8833(config, this) {} + : RoboidControl::DifferentialDrive(nullptr, nullptr, parent) { + this->drv8833 = new DRV8833(config, this); + this->leftWheel = this->drv8833->motorA; + this->rightWheel = this->drv8833->motorB; +} void DRV8833::DifferentialDrive::Update(bool recurse) { RoboidControl::DifferentialDrive::Update(recurse); - this->drv8833.Update(false); + this->drv8833->Update(false); } #pragma endregion Differential drive @@ -48,8 +49,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver, unsigned char pinIn1, unsigned char pinIn2, bool reverse) - : Motor() { - this->SetParent(driver); + : Motor(driver) { this->pinIn1 = pinIn1; this->pinIn2 = pinIn2; diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index 9762980..9cc7d1a 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -19,6 +19,8 @@ class DRV8833 : public Thing { int BIn1; int BIn2; int standby = 255; + + constexpr Configuration(int a1, int a2, int b1, int b2, int standby = 255) : AIn1(a1), AIn2(a2), BIn1(b1), BIn2(b2), standby(standby) {} }; /// @brief Setup a DRV8833 motor controller @@ -43,7 +45,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive { virtual void Update(bool recurse = false) override; protected: - DRV8833 drv8833; + DRV8833* drv8833 = nullptr; }; #pragma endregion Differential drive diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 338353e..8fa940d 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -8,7 +8,7 @@ namespace Arduino { UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) : Thing(parent) { - this->type = Type::DistanceSensor; + this->type = Type::DistanceSensor; this->name = "Ultrasonic sensor"; this->pinTrigger = config.trigger; this->pinEcho = config.echo; @@ -57,6 +57,28 @@ void UltrasonicSensor::Update(bool recursive) { Thing::Update(recursive); } +#pragma region Distance sensor + +UltrasonicSensor::DistanceSensor::DistanceSensor( + UltrasonicSensor::Configuration config, + Thing* parent) + : RoboidControl::DistanceSensor(parent), ultrasonic(config, this) {} + +void UltrasonicSensor::DistanceSensor::Update(bool recursive) { + RoboidControl::DistanceSensor::Update(recursive); + this->ultrasonic.Update(false); + if (this->ultrasonic.distance > 0) + this->internalDistance = this->ultrasonic.distance; + else +#if ARDUNIO + this->internalDistance = INFINITY; +#else + this->internalDistance = std::numeric_limits::infinity(); +#endif +} + +#pragma endregion Distance sensor + #pragma region Touch sensor UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent) @@ -65,11 +87,11 @@ UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent) void UltrasonicSensor::TouchSensor::Update(bool recursive) { RoboidControl::TouchSensor::Update(recursive); this->ultrasonic.Update(false); - this->internalTouch |= (this->ultrasonic.distance > 0 && - this->ultrasonic.distance <= this->touchDistance); + this->internalTouch = (this->ultrasonic.distance > 0 && + this->ultrasonic.distance <= this->touchDistance); } -#pragma region Touch sensor +#pragma endregion Touch sensor } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h index 8e72887..c991e23 100644 --- a/Arduino/Things/UltrasonicSensor.h +++ b/Arduino/Things/UltrasonicSensor.h @@ -1,5 +1,6 @@ #pragma once +#include "Things/DistanceSensor.h" #include "Things/TouchSensor.h" namespace RoboidControl { @@ -39,9 +40,26 @@ class UltrasonicSensor : Thing { unsigned char pinEcho = 0; public: + class DistanceSensor; class TouchSensor; }; +#pragma region Distance sensor + +class UltrasonicSensor::DistanceSensor : public RoboidControl::DistanceSensor { + public: + DistanceSensor(UltrasonicSensor::Configuration config, + Thing* parent = Thing::LocalRoot()); + + /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs) + virtual void Update(bool recursive = false) override; + + protected: + UltrasonicSensor ultrasonic; +}; + +#pragma endregion Distance sensor + #pragma region Touch sensor class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { @@ -58,7 +76,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { UltrasonicSensor ultrasonic; }; -#pragma region Touch sensor +#pragma endregion Touch sensor } // namespace Arduino } // namespace RoboidControl \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index a15671c..653da5b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,42 +22,23 @@ else() set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard set(CMAKE_POSITION_INDEPENDENT_CODE ON) - project(RoboidControl) - add_subdirectory(LinearAlgebra) - add_subdirectory(Examples) - - add_compile_definitions(GTEST) - include(FetchContent) - FetchContent_Declare( - googletest - DOWNLOAD_EXTRACT_TIMESTAMP ON - URL https://github.com/google/googletest/archive/refs/heads/main.zip - ) - - # For Windows: Prevent overriding the parent project's compiler/linker settings - set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) - FetchContent_MakeAvailable(googletest) - include_directories( . LinearAlgebra ) add_library(RoboidControl STATIC ${srcs}) - + target_include_directories(RoboidControl PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + ) + + project(RoboidControl) + add_subdirectory(LinearAlgebra) + + # Examples + option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF) + add_subdirectory(examples) + enable_testing() + add_subdirectory(test) - file(GLOB_RECURSE test_srcs test/*_test.cc) - add_executable( - RoboidControlTest - ${test_srcs} - ) - target_link_libraries( - RoboidControlTest - gtest_main - RoboidControl - LinearAlgebra - ) - - include(GoogleTest) - gtest_discover_tests(RoboidControlTest) endif() diff --git a/DoxyGen/Doxyfile b/DoxyGen/Doxyfile index 853c02a..d551ef0 100644 --- a/DoxyGen/Doxyfile +++ b/DoxyGen/Doxyfile @@ -48,7 +48,7 @@ PROJECT_NAME = "Roboid Control for C++" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = +PROJECT_NUMBER = 0.4 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 22e8160..f4e0c90 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -2,15 +2,13 @@ #if defined(IDF_VER) #include "esp_wifi.h" -#endif +#include namespace RoboidControl { -namespace EspIdf { -void ParticipantUDP::Setup(int localPort, +void ParticipantUDP::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) { -#if defined(IDF_VER) std::cout << "Set up UDP\n"; GetBroadcastAddress(); @@ -22,42 +20,57 @@ void ParticipantUDP::Setup(int localPort, } // Create a UDP socket - this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (this->sockfd < 0) { + this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (this->sock < 0) { std::cout << "Unable to create UDP socket: errno " << errno << "\n"; vTaskDelete(NULL); return; } - // Set up the server address structure + /* + // Set up the receiving(local) address struct sockaddr_in local_addr; memset(&local_addr, 0, sizeof(local_addr)); local_addr.sin_family = AF_INET; - local_addr.sin_port = htons(this->port); + local_addr.sin_port = htons(localPort); local_addr.sin_addr.s_addr = htonl(INADDR_ANY); // Listen on all available network interfaces - // Bind the socket to the address and port - if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < - 0) { + // Bind the socket to the receiving address + if (bind(this->sock, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) { std::cout << "Unable to bind UDP socket: errno " << errno << "\n"; - close(sockfd); + close(sock); vTaskDelete(NULL); return; } + */ - // Initialize the dest_addr structure + // Initialize the destination(remote) address memset(&this->dest_addr, 0, sizeof(this->dest_addr)); // Clear the entire structure this->dest_addr.sin_family = AF_INET; this->dest_addr.sin_port = htons(this->remoteSite->port); - inet_pton(AF_INET, this->remoteSite->ipAddress, - &this->dest_addr.sin_addr.s_addr); + this->dest_addr.sin_addr.s_addr = inet_addr(this->remoteSite->ipAddress); + // inet_pton(AF_INET, this->remoteSite->ipAddress, + // &this->dest_addr.sin_addr.s_addr); - std::cout << "Wifi sync started local " << this->port << ", remote " + this->connected = true; + + std::cout << "Wifi sync started local " << localPort << ", remote " << this->remoteSite->ipAddress << ":" << this->remoteSite->port << "\n"; -#endif // IDF_VER + + // std::cout << "socket: " << (int)this->sock << std::endl; + // ParticipantMsg* msg = new ParticipantMsg(this->networkId); + // int bufferSize = msg->Serialize(this->buffer); + // int err = sendto(this->sock, buffer, bufferSize, 0, + // (struct sockaddr*)&dest_addr, sizeof(dest_addr)); + // if (errno != 0) + // std::cout << "AASend error " << err << " or " << errno << "\n"; + // else + // std::cout << "AASend SUCCESS\n"; + + //SendTest(); } void ParticipantUDP::GetBroadcastAddress() { @@ -83,10 +96,11 @@ void ParticipantUDP::GetBroadcastAddress() { #endif // IDF_VER } -void ParticipantUDP::Receive() { +void ParticipantUDP::ReceiveUDP() { #if defined(IDF_VER) +/* struct pollfd fds[1]; - fds[0].fd = sockfd; + fds[0].fd = sock; fds[0].events = POLLIN; // We're looking for data available to read // Use poll() with a timeout of 0 to return immediately @@ -103,7 +117,7 @@ void ParticipantUDP::Receive() { char sender_ipAddress[INET_ADDRSTRLEN]; while (ret > 0 && fds[0].revents & POLLIN) { - int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0, + int packetSize = recvfrom(this->sock, buffer, sizeof(buffer) - 1, 0, (struct sockaddr*)&source_addr, &addr_len); if (packetSize < 0) { std::cout << "recvfrom() error\n"; @@ -126,18 +140,66 @@ void ParticipantUDP::Receive() { } } // std::cout << "no more messages\n"; - +*/ #endif // IDF_VER } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { -#if defined(IDF_VER) - // std::cout << "Sending to " << remoteParticipant->ipAddress << ":" - // << remoteParticipant->port << "\n"; +ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {} - int err = sendto(this->sockfd, buffer, bufferSize, 0, - (struct sockaddr*)&dest_addr, sizeof(dest_addr)); - if (errno != 0) +ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) + : ParticipantUDPGeneric(ipAddress, port, localPort) {} + +// bool ParticipantUDP::SendTest() { +// #if defined(IDF_VER) +// // std::cout << "socket: " << (int)this->sock << std::endl; +// // UBaseType_t stack_size = uxTaskGetStackHighWaterMark(NULL); // NULL to check the main task +// // size_t free_heap = xPortGetFreeHeapSize(); +// // std::cout << "Stack High Water Mark: " << stack_size << " heap " << free_heap << std::endl; + +// ParticipantMsg* msg = new ParticipantMsg(this->networkId); +// int bSize = msg->Serialize(this->buffer); +// // std::cout << "buffer size " << bSize << std::endl; +// int err = sendto(this->sock, buffer, bSize, 0, (struct sockaddr*)&dest_addr, +// sizeof(dest_addr)); +// if (errno != 0) +// std::cout << "BBSend error " << err << " or " << errno << "\n"; +// else +// std::cout << "BBSend SUCCESS\n"; + +// #endif +// return true; +// } + +bool ParticipantUDP::Send(IMessage* msg) { + int bufferSize = msg->Serialize(this->buffer); + if (bufferSize <= 0) + return true; + + std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) + << " to " << this->remoteSite->ipAddress << std::endl; + + return this->SendTo(this->remoteSite, bufferSize); +} + +bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, + int bufferSize) { +#if defined(IDF_VER) + uint16_t port = ntohs(dest_addr.sin_port); + + char ip_str[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &dest_addr.sin_addr, ip_str, sizeof(ip_str)); + std::cout << "Sending " << bufferSize << " bytes to " << ip_str << ":" << port << "\n"; + + // Print the IP address and port + // printf("IP Address: %s\n", ip_str); + // printf("Port: %d\n", port); + + this->dest_addr.sin_port = htons(remoteParticipant->port); + this->dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress); + + int err = sendto(this->sock, buffer, bufferSize, 0, + (struct sockaddr*)&this->dest_addr, sizeof(this->dest_addr)); + if (errno < 0) std::cout << "Send error " << err << " or " << errno << "\n"; #endif @@ -154,7 +216,7 @@ bool ParticipantUDP::Publish(IMessage* msg) { dest_addr.sin_family = AF_INET; dest_addr.sin_port = htons(this->port); inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr); - int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, + int err = sendto(sock, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr)); if (err != 0) std::cout << "Publish error\n"; @@ -162,5 +224,6 @@ bool ParticipantUDP::Publish(IMessage* msg) { return true; }; -} // namespace EspIdf } // namespace RoboidControl + +#endif \ No newline at end of file diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index cf1c6bf..1c74ebf 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -1,26 +1,47 @@ #pragma once +#if defined(IDF_VER) #include "Participants/ParticipantUDP.h" -#if defined(IDF_VER) #include "lwip/sockets.h" -#endif namespace RoboidControl { -namespace EspIdf { -class ParticipantUDP : public RoboidControl::ParticipantUDP { +class ParticipantUDP : public ParticipantUDPGeneric { public: + /// @brief Create a participant without connecting to a site + /// @param port The port on which the participant communicates + /// These participant typically broadcast Participant messages to let site + /// servers on the local network know their presence. Alternatively they can + /// broadcast information which can be used directly by other participants. + ParticipantUDP(int port = 7681); + /// @brief Create a participant which will try to connect to a site. + /// @param ipAddress The IP address of the site + /// @param port The port used by the site + /// @param localPort The port used by the local participant + ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); + void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); + // bool SendTest(); + + /// @brief Sens a message to the remote site (if set) + /// @param msg The message to send + /// @return True if a message could be sent. + bool Send(IMessage* msg) override; + void SetupUDP(int localPort, + const char* remoteIpAddress, + int remotePort) override; + void ReceiveUDP() override; + protected: #if defined(IDF_VER) char broadcastIpAddress[INET_ADDRSTRLEN]; - int sockfd; + int sock; struct sockaddr_in dest_addr; // struct sockaddr_in src_addr; #endif @@ -28,5 +49,6 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { void GetBroadcastAddress(); }; -} // namespace EspIdf } // namespace RoboidControl + +#endif diff --git a/Messages/PoseMsg.cpp b/Messages/PoseMsg.cpp index aca128e..d72e453 100644 --- a/Messages/PoseMsg.cpp +++ b/Messages/PoseMsg.cpp @@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) { this->thingId = thing->id; this->poseType = 0; - if (thing->positionUpdated || (force && thing->IsRoot())) { + if (thing->positionUpdated || (force || thing->IsRoot())) { this->position = thing->GetPosition(); this->poseType |= Pose_Position; } - if (thing->orientationUpdated || (force && thing->IsRoot())) { + if (thing->orientationUpdated || (force || thing->IsRoot())) { this->orientation = thing->GetOrientation(); this->poseType |= Pose_Orientation; } diff --git a/Participant.cpp b/Participant.cpp index dc02a15..ba3c46e 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -10,8 +10,6 @@ namespace RoboidControl { #pragma region Participant -ParticipantRegistry Participant::registry; - Participant* Participant::LocalParticipant = new Participant(); void Participant::ReplaceLocalParticipant(Participant& newParticipant) { @@ -24,6 +22,7 @@ Participant::Participant() { //this->Add(this->root); } +/* Participant::Participant(const char* ipAddress, int port) { Thing::CreateRoot(this); //this->Add(this->root); @@ -43,19 +42,26 @@ Participant::Participant(const char* ipAddress, int port) { this->ipAddress = addressString; this->port = port; } - +*/ Participant::~Participant() { // registry.Remove(this); - delete[] this->ipAddress; + // delete[] this->ipAddress; } -void Participant::Update() { +void Participant::Update(bool recurse) { for (Thing* thing : this->things) { if (thing != nullptr) - thing->Update(true); + thing->Update(); } } +bool Participant::Send(IMessage* msg) { + std::cout << "sending message " << (static_cast(this->buffer[0]) & 0xff) + << " to base Participant without communcation support " << std::endl; + + return true; +} +/* bool Participant::Send(IMessage* msg) { int bufferSize = msg->Serialize(this->buffer); if (bufferSize <= 0) @@ -67,31 +73,31 @@ bool Participant::Send(IMessage* msg) { #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); - return thisWindows->Send(this, bufferSize); + return thisWindows->SendTo(this, bufferSize); #elif defined(__unix__) || defined(__APPLE__) Posix::ParticipantUDP* thisPosix = static_cast(this); - return thisPosix->Send(this, bufferSize); + return thisPosix->SendTo(this, bufferSize); #elif defined(ARDUINO) Arduino::ParticipantUDP* thisArduino = static_cast(this); - return thisArduino->Send(this, bufferSize); + return thisArduino->SendTo(this, bufferSize); #elif defined(IDF_VER) EspIdf::ParticipantUDP* thisEspIdf = static_cast(this); - return thisEspIdf->Send(remoteParticipant, bufferSize); + return thisEspIdf->SendTo(this, bufferSize); #else return false; #endif } +*/ -Thing* Participant::Get(unsigned char thingId) { +Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { for (Thing* thing : this->things) { - if (thing->id == thingId) + if (thing->owner->networkId == networkId && thing->id == thingId) return thing; } - // std::cout << "Could not find thing " << this->ipAddress << ":" << - // this->port - // << "[" << (int)thingId << "]\n"; + std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port + << "[" << (int)thingId << "]\n"; return nullptr; } @@ -113,22 +119,24 @@ void Participant::Add(Thing* thing, bool checkId) { thing->id = highestIx + 1; this->things.push_back(thing); #endif - // std::cout << "Add thing with generated ID " << this->ipAddress << ":" - // << this->port << "[" << (int)thing->id << "]\n"; + std::cout << "Add thing with generated ID " + //<< this->ipAddress << ":" << this->port + << "[" << (int)thing->id << "]\n"; } else { - Thing* foundThing = Get(thing->id); + Thing* foundThing = Get(thing->owner->networkId, 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->id << "]\n"; + 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"; + std::cout << "Did not add, existing thing " + //<< this->ipAddress << ":" << this->port + << "[" << (int)thing->id << "]\n"; } } } @@ -156,86 +164,88 @@ void Participant::Remove(Thing* thing) { #pragma endregion -#pragma region ParticipantRegistry +// #pragma region ParticipantRegistry -Participant* ParticipantRegistry::Get(const char* ipAddress, - unsigned int port) { -#if !defined(NO_STD) - for (Participant* participant : ParticipantRegistry::participants) { - if (participant == nullptr) - continue; - if (strcmp(participant->ipAddress, ipAddress) == 0 && - participant->port == port) { - // std::cout << "found participant " << participant->ipAddress << ":" - // << (int)participant->port << std::endl; - return participant; - } - } - std::cout << "Could not find participant " << ipAddress << ":" << (int)port - << std::endl; -#endif - return nullptr; -} +// /* +// Participant* ParticipantRegistry::Get(const char* ipAddress, +// unsigned int port) { +// #if !defined(NO_STD) +// for (Participant* participant : ParticipantRegistry::participants) { +// if (participant == nullptr) +// continue; +// if (strcmp(participant->ipAddress, ipAddress) == 0 && +// participant->port == port) { +// // std::cout << "found participant " << participant->ipAddress << ":" +// // << (int)participant->port << std::endl; +// return participant; +// } +// } +// std::cout << "Could not find participant " << ipAddress << ":" << (int)port +// << std::endl; +// #endif +// return nullptr; +// } +// */ +// Participant* ParticipantRegistry::Get(unsigned char participantId) { +// #if !defined(NO_STD) +// for (Participant* participant : ParticipantRegistry::participants) { +// if (participant == nullptr) +// continue; +// if (participant->networkId == participantId) +// return participant; +// } +// std::cout << "Could not find participant " << (int)participantId << std::endl; +// #endif +// return nullptr; +// } -Participant* ParticipantRegistry::Get(unsigned char participantId) { -#if !defined(NO_STD) - for (Participant* participant : ParticipantRegistry::participants) { - if (participant == nullptr) - continue; - if (participant->networkId == participantId) - return participant; - } - std::cout << "Could not find participant " << (int)participantId << std::endl; -#endif - return nullptr; -} +// // Participant* ParticipantRegistry::Add(const char* ipAddress, +// // unsigned int port) { +// // Participant* participant = new Participant(ipAddress, port); +// // Add(participant); +// // return participant; +// // } -Participant* ParticipantRegistry::Add(const char* ipAddress, - unsigned int port) { - Participant* participant = new Participant(ipAddress, port); - Add(participant); - return participant; -} +// void ParticipantRegistry::Add(Participant* participant) { +// Participant* foundParticipant = +// Get(participant->networkId); +// //Get(participant->ipAddress, participant->port); -void ParticipantRegistry::Add(Participant* participant) { - Participant* foundParticipant = - Get(participant->ipAddress, participant->port); +// if (foundParticipant == nullptr) { +// #if defined(NO_STD) +// // this->things[this->thingCount++] = thing; +// #else +// ParticipantRegistry::participants.push_back(participant); +// #endif +// // std::cout << "Add participant " << participant->ipAddress << ":" +// // << participant->port << "[" << (int)participant->networkId +// // << "]\n"; +// // std::cout << "participants " << +// // ParticipantRegistry::participants.size() +// // << "\n"; +// // } else { +// // std::cout << "Did not add, existing participant " << +// // participant->ipAddress +// // << ":" << participant->port << "[" << +// // (int)participant->networkId +// // << "]\n"; +// } +// } - if (foundParticipant == nullptr) { -#if defined(NO_STD) - // this->things[this->thingCount++] = thing; -#else - ParticipantRegistry::participants.push_back(participant); -#endif - // std::cout << "Add participant " << participant->ipAddress << ":" - // << participant->port << "[" << (int)participant->networkId - // << "]\n"; - // std::cout << "participants " << - // ParticipantRegistry::participants.size() - // << "\n"; - // } else { - // std::cout << "Did not add, existing participant " << - // participant->ipAddress - // << ":" << participant->port << "[" << - // (int)participant->networkId - // << "]\n"; - } -} +// void ParticipantRegistry::Remove(Participant* participant) { +// // participants.remove(participant); +// } -void ParticipantRegistry::Remove(Participant* participant) { - // participants.remove(participant); -} +// #if defined(NO_STD) +// Participant** ParticipantRegistry::GetAll() const { +// return ParticipantRegistry::participants; +// } +// #else +// const std::list& ParticipantRegistry::GetAll() const { +// return ParticipantRegistry::participants; +// } +// #endif -#if defined(NO_STD) -Participant** ParticipantRegistry::GetAll() const { - return ParticipantRegistry::participants; -} -#else -const std::list& ParticipantRegistry::GetAll() const { - return ParticipantRegistry::participants; -} -#endif - -#pragma endregion ParticipantRegistry +// #pragma endregion ParticipantRegistry } // namespace RoboidControl diff --git a/Participant.h b/Participant.h index 6e1217d..d3797dc 100644 --- a/Participant.h +++ b/Participant.h @@ -7,6 +7,7 @@ namespace RoboidControl { constexpr int MAX_THING_COUNT = 256; +/* /// @brief class which manages all known participants class ParticipantRegistry { public: @@ -14,7 +15,7 @@ class ParticipantRegistry { /// @param ipAddress The IP address of the participant /// @param port The port number of the participant /// @return The participant or a nullptr when it could not be found - Participant* Get(const char* ipAddress, unsigned int port); + //Participant* Get(const char* ipAddress, unsigned int port); /// @brief Retrieve a participant by its network ID /// @param networkID The network ID of the participant /// @return The participant or a nullptr when it could not be found @@ -24,7 +25,8 @@ class ParticipantRegistry { /// @param ipAddress The IP address of the participant /// @param port The port number of the participant /// @return The added participant - Participant* Add(const char* ipAddress, unsigned int port); + //Participant* Add(const char* ipAddress, unsigned int port); + /// @brief Add a participant /// @param participant The participant to add void Add(Participant* participant); @@ -52,6 +54,7 @@ class ParticipantRegistry { std::list participants; #endif }; +*/ /// @brief A participant is a device which manages things. /// It can communicate with other participant to synchronise the state of @@ -88,15 +91,6 @@ class Participant { /// @brief The name of the participant const char* name = "Participant"; - /// @brief The Ip Address of a participant. - /// @remarks This does not belong here, it should move to ParticipantUDP or - /// something like that in the future - const char* ipAddress = "0.0.0.0"; - /// @brief The port number for UDP communication with the participant. - /// @remarks This does not belong here, it should move to ParticipantUDP or - /// something like that in the future - unsigned int port = 0; - /// @brief The network Id to identify the participant unsigned char networkId = 0; @@ -112,9 +106,10 @@ class Participant { std::list things; #endif /// @brief Find a thing managed by this participant + /// @param networkId The network ID of the thing /// @param thingId The ID of the thing /// @return The thing if found, nullptr when no thing has been found - Thing* Get(unsigned char thingId); + Thing* Get(unsigned char networkId, unsigned char thingId); /// @brief Add a new thing for this participant. /// @param thing The thing to add /// @param checkId If true, the thing.id is regenerated if it is zero @@ -129,7 +124,7 @@ class Participant { public: /// @brief Update all things for this participant - virtual void Update(); + virtual void Update(bool recurse = true); #pragma endregion Update @@ -142,12 +137,12 @@ class Participant { #pragma endregion Send -#pragma region Participant Registry +// #pragma region Participant Registry - public: - static ParticipantRegistry registry; +// public: +// static ParticipantRegistry registry; -#pragma endregion Participant Registry +// #pragma endregion Participant Registry }; } // namespace RoboidControl diff --git a/Participants/IsolatedParticipant.cpp b/Participants/IsolatedParticipant.cpp index a89eab4..2045651 100644 --- a/Participants/IsolatedParticipant.cpp +++ b/Participants/IsolatedParticipant.cpp @@ -1,14 +1,16 @@ +/* #include "IsolatedParticipant.h" #include "ParticipantUDP.h" namespace RoboidControl { -static ParticipantUDP* isolatedParticipant = nullptr; +static ParticipantUDPGeneric* isolatedParticipant = nullptr; Participant* IsolatedParticipant::Isolated() { if (isolatedParticipant == nullptr) - isolatedParticipant = new ParticipantUDP(0); + isolatedParticipant = new ParticipantUDPGeneric(0); return isolatedParticipant; } -} // namespace RoboidControl \ No newline at end of file +} // namespace RoboidControl + */ \ No newline at end of file diff --git a/Participants/IsolatedParticipant.h b/Participants/IsolatedParticipant.h index 27e9e80..5b44f5a 100644 --- a/Participants/IsolatedParticipant.h +++ b/Participants/IsolatedParticipant.h @@ -1,3 +1,4 @@ +/* #include "Participant.h" namespace RoboidControl { @@ -10,4 +11,5 @@ class IsolatedParticipant { static Participant* Isolated(); }; -} \ No newline at end of file +} +*/ \ No newline at end of file diff --git a/Participants/ParticipantMQTT.cpp b/Participants/ParticipantMQTT.cpp index 26e3675..f23a1dc 100644 --- a/Participants/ParticipantMQTT.cpp +++ b/Participants/ParticipantMQTT.cpp @@ -8,19 +8,15 @@ #include #endif +#include "Participants/ParticipantUDP.h" + namespace RoboidControl { -#if defined(_WIN32) || defined(_WIN64) -Windows::ParticipantUDP thisWindows = Windows::ParticipantUDP(); -#elif defined(__unix__) || defined(__APPLE__) -Posix::ParticipantUDP thisWindows = Posix::ParticipantUDP(); -#endif - -ParticipantMQTT::ParticipantMQTT(const char* ipAddress, int port) - : Participant("127.0.0.1", port) { +MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port) + : RemoteParticipantUDP("127.0.0.1", port) { this->name = "ParticipantUDP"; - this->remoteSite = new Participant(ipAddress, port); - Participant::registry.Add(this); + this->remoteSite = new RemoteParticipantUDP(ipAddress, port); + // Participant::registry.Add(this); this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; this->root->owner = this; @@ -30,22 +26,27 @@ ParticipantMQTT::ParticipantMQTT(const char* ipAddress, int port) Participant::ReplaceLocalParticipant(*this); std::cout << "Setup TCP connection" << std::endl; - thisWindows.SetupTCP(ipAddress, port); + // thisWindows.SetupTCP(ipAddress, port); - send_mqtt_connect("RoboidControl"); - sendSubscribe("domoticz/out"); + // send_mqtt_connect("RoboidControl"); + // sendSubscribe("domoticz/out"); // #endif } +void MQTTParticipantBase::SetupTCP() { + this->connected = false; +} + #define MQTT_CONNECT 0x10 #define MQTT_CONNECT_ACK 0x20 -void ParticipantMQTT::send_mqtt_connect(const char* client_id) { +void MQTTParticipantBase::send_mqtt_connect(const char* client_id) { MQTTConnectPacket packet; packet.fixed_header = MQTT_CONNECT; - packet.remaining_length = //14; - 2 + 4 + 2+ strlen(client_id) + 3; // Protocol name + protocol level + connect - // flags + keep alive + client ID + packet.remaining_length = // 14; + 2 + 4 + 2 + strlen(client_id) + + 3; // Protocol name + protocol level + connect + // flags + keep alive + client ID packet.protocol_name_length = 4; // "MQTT" packet.protocol_name = "MQTT"; packet.protocol_level = 4; // MQTT version 3.1.1 @@ -66,25 +67,25 @@ void ParticipantMQTT::send_mqtt_connect(const char* client_id) { this->buffer[index++] = packet.connect_flags; this->buffer[index++] = packet.keep_alive & 0xFF; // LSB this->buffer[index++] = (packet.keep_alive >> 8) & 0xFF; // MSB -// this->buffer[index++] = 0; // MSB: no client ID -// this->buffer[index++] = 0; // LSB: no client ID - size_t client_id_length = strlen(client_id); - this->buffer[index++] = (client_id_length >> 8) & 0xFF; // MSB - this->buffer[index++] = client_id_length & 0xFF; // LSB - memcpy(&this->buffer[index], client_id, client_id_length); - index += client_id_length; + // this->buffer[index++] = 0; // MSB: no + // client ID this->buffer[index++] = 0; // + // LSB: no client ID + size_t client_id_length = strlen(client_id); + this->buffer[index++] = (client_id_length >> 8) & 0xFF; // MSB + this->buffer[index++] = client_id_length & 0xFF; // LSB + memcpy(&this->buffer[index], client_id, client_id_length); + index += client_id_length; for (int ix = 0; ix < index; ix++) std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n"; // Send the MQTT connect packet - thisWindows.SendTCP(index); - - std::cout << "Send connect, client ID = " << client_id << std::endl; + SendTCP(index); + std::cout << "Send connect, client ID = " << client_id << std::endl; } -void ParticipantMQTT::sendSubscribe(const char* topic) { +void MQTTParticipantBase::sendSubscribe(const char* topic) { // Packet Identifier (2 bytes) static unsigned short packetId = 1; // Increment this for each new subscription @@ -112,27 +113,28 @@ void ParticipantMQTT::sendSubscribe(const char* topic) { this->buffer[6 + topicLength] = 0x00; // QoS level (0) int index = 7 + topicLength; -// for (int ix = 0; ix < index; ix++) -// std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n"; + // for (int ix = 0; ix < index; ix++) + // std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n"; // Send the SUBSCRIBE packet - thisWindows.SendTCP(7 + topicLength); + SendTCP(7 + topicLength); - std::cout << "Send subscribe to topic: " << topic << std::endl; + std::cout << "Send subscribe to topic: " << topic << std::endl; } -void ParticipantMQTT::Update() { +void MQTTParticipantBase::Update(bool recurse) { int packetSize = ReceiveTCP(); if (packetSize > 0) { ReceiveData(packetSize); } } +void MQTTParticipantBase::SendTCP(int bufferSize) {} -int ParticipantMQTT::ReceiveTCP() { - return thisWindows.ReceiveTCP(); +int MQTTParticipantBase::ReceiveTCP() { + return false; } -void ParticipantMQTT::ReceiveData(unsigned char bufferSize) { +void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) { std::cout << " receivemsg\n"; // std::cout << "receive msg " << (int)msgId << "\n"; // std::cout << " buffer size = " <<(int) bufferSize << "\n"; diff --git a/Participants/ParticipantMQTT.h b/Participants/ParticipantMQTT.h index c8e992d..2ed7fbb 100644 --- a/Participants/ParticipantMQTT.h +++ b/Participants/ParticipantMQTT.h @@ -11,6 +11,7 @@ #include "Messages/TextMsg.h" #include "Messages/ThingMsg.h" #include "Participant.h" +#include "Participants/ParticipantUDP.h" #if !defined(NO_STD) #include @@ -18,15 +19,14 @@ // #include #endif - #if defined(_WIN32) || defined(_WIN64) #include "Windows/WindowsParticipant.h" #elif defined(__unix__) || defined(__APPLE__) -#include "Posix/PosixParticipant.h" #include #include #include #include +#include "Posix/PosixParticipant.h" #endif namespace RoboidControl { @@ -43,14 +43,16 @@ namespace RoboidControl { /// participant is created which can be obtained using /// RoboidControl::IsolatedParticipant::Isolated(). /// @sa RoboidControl::Thing::Thing() -class ParticipantMQTT : public Participant { +class MQTTParticipantBase : public RemoteParticipantUDP { #pragma region Init public: /// @brief Create a participant which will try to connect to a MQTT server. /// @param ipAddress The IP address of the MQTT server /// @param port The port used by the MQTT server - ParticipantMQTT(const char* ipAddress, int port = 1883); + MQTTParticipantBase(const char* ipAddress, int port = 1883); +protected: + virtual void SetupTCP(); #pragma endregion Init @@ -58,7 +60,7 @@ class ParticipantMQTT : public Participant { public: /// @brief The remote site when this participant is connected to a site - Participant* remoteSite = nullptr; + RemoteParticipantUDP* remoteSite = nullptr; protected: #if defined(__unix__) || defined(__APPLE__) @@ -75,7 +77,7 @@ class ParticipantMQTT : public Participant { #pragma region Update public: - virtual void Update() override; + virtual void Update(bool recurse = true) override; #pragma endregion Update @@ -96,13 +98,15 @@ class ParticipantMQTT : public Participant { void send_mqtt_connect(const char* client_id); void sendSubscribe(const char* topic); + virtual void SendTCP(int bufferSize); + #pragma endregion Send #pragma region Receive protected: int ReceiveTCP(); - void ReceiveData(unsigned char bufferSize); + void ReceiveData(unsigned char bufferSize); // void ReceiveData(unsigned char bufferSize, Participant* // remoteParticipant); diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 3289e79..4f30b80 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -8,73 +8,167 @@ #include "Posix/PosixParticipant.h" #include "Windows/WindowsParticipant.h" +#include "Things/DifferentialDrive.h" +#include "Things/DistanceSensor.h" +#include "Things/TouchSensor.h" + #include namespace RoboidControl { +#pragma region ParticipantRegistry + +ParticipantRegistry ParticipantUDPGeneric::registry; + +RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress, + unsigned int port) { +#if !defined(NO_STD) + for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) { + if (participant == nullptr) + continue; + if (strcmp(participant->ipAddress, ipAddress) == 0 && + participant->port == port) { + // std::cout << "found participant " << participant->ipAddress << ":" + // << (int)participant->port << std::endl; + return participant; + } + } + std::cout << "Could not find participant " << ipAddress << ":" << (int)port + << std::endl; +#endif + return nullptr; +} + +RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) { +#if !defined(NO_STD) + for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) { + if (participant == nullptr) + continue; + if (participant->networkId == participantId) + return participant; + } + std::cout << "Could not find participant " << (int)participantId << std::endl; +#endif + return nullptr; +} + +RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress, + unsigned int port) { + RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port); + Add(participant); + return participant; +} + +void ParticipantRegistry::Add(RemoteParticipantUDP* participant) { + Participant* foundParticipant = Get(participant->networkId); + // Get(participant->ipAddress, participant->port); + + if (foundParticipant == nullptr) { +#if defined(NO_STD) + // this->things[this->thingCount++] = thing; +#else + ParticipantRegistry::participants.push_back(participant); +#endif + // std::cout << "Add participant " << participant->ipAddress << ":" + // << participant->port << "[" << (int)participant->networkId + // << "]\n"; + // std::cout << "participants " << + // ParticipantRegistry::participants.size() + // << "\n"; + // } else { + // std::cout << "Did not add, existing participant " << + // participant->ipAddress + // << ":" << participant->port << "[" << + // (int)participant->networkId + // << "]\n"; + } +} + +void ParticipantRegistry::Remove(RemoteParticipantUDP* participant) { + // participants.remove(participant); +} + +#if defined(NO_STD) +RemoteParticipantUDP** ParticipantRegistry::GetAll() const { + return ParticipantRegistry::participants; +} +#else +const std::list& ParticipantRegistry::GetAll() const { + return ParticipantRegistry::participants; +} +#endif + +#pragma endregion ParticipantRegistry + +RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) { + // make a copy of the ip address string + int addressLength = (int)strlen(ipAddress); + int stringLength = addressLength + 1; + char* addressString = new char[stringLength]; +#if defined(_WIN32) || defined(_WIN64) + strncpy_s(addressString, stringLength, ipAddress, + addressLength); // Leave space for null terminator +#else + strncpy(addressString, ipAddress, addressLength); +#endif + addressString[addressLength] = '\0'; + + this->ipAddress = addressString; + this->port = port; +} + +bool RemoteParticipantUDP::Send(IMessage* msg) { + // No message is actually sent, because this class has no networking + // implementation + return false; +} + #pragma region Init -ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) { +ParticipantUDPGeneric::ParticipantUDPGeneric(int port) + : RemoteParticipantUDP("127.0.0.1", port) { this->name = "ParticipantUDP"; this->remoteSite = nullptr; if (this->port == 0) this->isIsolated = true; - Participant::registry.Add(this); + registry.Add(this); this->root = Thing::LocalRoot(); //::LocalParticipant->root; this->root->owner = this; this->root->name = "UDP Root"; + std::cout << "P2 " << (long)this->root << std::endl; this->Add(this->root); Participant::ReplaceLocalParticipant(*this); } -ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) - : Participant("127.0.0.1", localPort) { +ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress, + int port, + int localPort) + : RemoteParticipantUDP("127.0.0.1", localPort) { this->name = "ParticipantUDP"; if (this->port == 0) this->isIsolated = true; else - this->remoteSite = new Participant(ipAddress, port); - Participant::registry.Add(this); + this->remoteSite = new RemoteParticipantUDP(ipAddress, port); + registry.Add(this); this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; this->root->owner = this; this->root->name = "UDP Root"; + std::cout << "P1 " << (long)this->root << std::endl; this->Add(this->root); Participant::ReplaceLocalParticipant(*this); } -void ParticipantUDP::begin() { +void ParticipantUDPGeneric::begin() { if (this->isIsolated || this->remoteSite == nullptr) return; SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port); } -void ParticipantUDP::SetupUDP(int localPort, - 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); - thisPosix->Setup(localPort, remoteIpAddress, remotePort); -#elif defined(ARDUINO) - Arduino::ParticipantUDP* thisArduino = - static_cast(this); - thisArduino->Setup(); -#elif defined(IDF_VER) - EspIdf::ParticipantUDP* thisEspIdf = - static_cast(this); - thisEspIdf->Setup(localPort, remoteIpAddress, remotePort); -#endif - this->connected = true; -} - #pragma endregion Init #pragma region Update @@ -83,106 +177,55 @@ void ParticipantUDP::SetupUDP(int localPort, // 1. receive external messages // 2. update the state // 3. send out the updated messages -void ParticipantUDP::Update() { +void ParticipantUDPGeneric::Update(bool recurse) { unsigned long currentTimeMs = Thing::GetTimeMs(); - PrepMyThings(); - if (this->isIsolated == false) { if (this->connected == false) begin(); if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { ParticipantMsg* msg = new ParticipantMsg(this->networkId); + if (this->remoteSite == nullptr) this->Publish(msg); else this->Send(msg); delete msg; - this->nextPublishMe = currentTimeMs + this->publishInterval; } - this->ReceiveUDP(); + //this->ReceiveUDP(); } UpdateMyThings(); UpdateOtherThings(); } -void ParticipantUDP::PrepMyThings() { - for (Thing* thing : this->things) { - if (thing == nullptr) - continue; - - thing->PrepareForUpdate(); - } -} - -void ParticipantUDP::UpdateMyThings() { - // std::cout << this->things.size() << std::endl; +void ParticipantUDPGeneric::UpdateMyThings() { for (Thing* thing : this->things) { if (thing == nullptr) // || thing->GetParent() != nullptr) continue; - // std::cout << thing->name << "\n"; - // if (thing->hierarchyChanged) { - // if (!(this->isIsolated || this->networkId == 0)) { - // ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); - // this->remoteSite->Send(thingMsg); - // delete thingMsg; - - // if (thing->nameChanged) { - // NameMsg* nameMsg = new NameMsg(this->networkId, thing); - // this->remoteSite->Send(nameMsg); - // delete nameMsg; - // } - // } - // } - - // std::cout << "B\n"; - // Why don't we do recursive? + // Why don't we do recursive? // Because when a thing creates a thing in the update, // that new thing is not sent out (because of hierarchyChanged) // before it is updated itself: it is immediatedly updated! thing->Update(false); - // std::cout << "C\n"; - // if (!(this->isIsolated || this->networkId == 0)) { - // if (thing->terminate) { - // DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing); - // this->remoteSite->Send(destroyMsg); - // delete destroyMsg; - // } else { - // // Send to remote site - // if (thing->nameChanged) { - // NameMsg* nameMsg = new NameMsg(this->networkId, thing); - // this->remoteSite->Send(nameMsg); - // delete nameMsg; - // } - // PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); - // this->remoteSite->Send(poseMsg); - // delete poseMsg; - // BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing); - // this->remoteSite->Send(binaryMsg); - // delete binaryMsg; - // } - // } - // std::cout << "D\n"; if (thing->terminate) this->Remove(thing); - // std::cout << "E\n"; } } -void ParticipantUDP::UpdateOtherThings() { +void ParticipantUDPGeneric::UpdateOtherThings() { #if defined(NO_STD) Participant** participants = Participant::registry.GetAll(); for (int ix = 0; ix < Participant::registry.count; ix++) { Participant* participant = participants[ix]; #else - for (Participant* participant : Participant::registry.GetAll()) { + for (Participant* participant : registry.GetAll()) { #endif if (participant == nullptr || participant == this) continue; @@ -210,8 +253,8 @@ void ParticipantUDP::UpdateOtherThings() { #pragma region Send -void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, - Thing* thing) { +void ParticipantUDPGeneric::SendThingInfo(Participant* remoteParticipant, + Thing* thing) { // std::cout << "Send thing info [" << (int)thing->id << "] \n"; ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); remoteParticipant->Send(thingMsg); @@ -230,14 +273,14 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, delete binaryMsg; } -bool ParticipantUDP::Send(IMessage* msg) { +bool ParticipantUDPGeneric::Send(IMessage* msg) { if (this->remoteSite != nullptr) return this->remoteSite->Send(msg); - return false; + return true; } -void ParticipantUDP::PublishThingInfo(Thing* thing) { +void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) { // std::cout << "Publish thing info" << thing->networkId << "\n"; // Strange, when publishing, the network id is irrelevant, because it is // connected to a specific site... @@ -258,71 +301,31 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) { delete customMsg; } -bool ParticipantUDP::Publish(IMessage* msg) { - // std::cout << "publish msg\n"; -#if defined(_WIN32) || defined(_WIN64) - Windows::ParticipantUDP* thisWindows = - static_cast(this); - return thisWindows->Publish(msg); -#elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = static_cast(this); - return thisPosix->Publish(msg); -#elif defined(ARDUINO) - Arduino::ParticipantUDP* thisArduino = - static_cast(this); - return thisArduino->Publish(msg); -#elif defined(IDF_VER) - EspIdf::ParticipantUDP* thisEspIdf = - static_cast(this); - return thisEspIdf->Publish(msg); -#else - return false; -#endif -} - // Send #pragma endregion #pragma region Receive -void ParticipantUDP::ReceiveUDP() { -#if defined(_WIN32) || defined(_WIN64) - Windows::ParticipantUDP* thisWindows = - static_cast(this); - thisWindows->Receive(); -#elif defined(__unix__) || defined(__APPLE__) - Posix::ParticipantUDP* thisPosix = static_cast(this); - thisPosix->Receive(); -#elif defined(ARDUINO) - Arduino::ParticipantUDP* thisArduino = - static_cast(this); - thisArduino->Receive(); -#elif defined(IDF_VER) - EspIdf::ParticipantUDP* thisEspIdf = - static_cast(this); - thisEspIdf->Receive(); -#endif -} - -void ParticipantUDP::ReceiveData(unsigned char packetSize, - char* senderIpAddress, - unsigned int senderPort) { +void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize, + char* senderIpAddress, + unsigned int senderPort) { // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort // << std::endl; - Participant* sender = this->registry.Get(senderIpAddress, senderPort); + RemoteParticipantUDP* sender = + this->registry.Get(senderIpAddress, senderPort); if (sender == nullptr) { sender = this->registry.Add(senderIpAddress, senderPort); #if !defined(NO_STD) - std::cout << "New remote participant " << sender->ipAddress << ":" - << sender->port << std::endl; + // std::cout << "New remote participant " << sender->ipAddress << ":" + // << sender->port << std::endl; #endif } ReceiveData(packetSize, sender); } -void ParticipantUDP::ReceiveData(unsigned char bufferSize, - Participant* sender) { +void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize, + RemoteParticipantUDP* sender) { unsigned char msgId = this->buffer[0]; // std::cout << "receive msg " << (int)msgId << "\n"; // std::cout << " buffer size = " <<(int) bufferSize << "\n"; @@ -395,14 +398,16 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize, #endif } -void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + ParticipantMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId << "\n"; #endif } -void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + NetworkIdMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId << " -> " << (int)msg->networkId << "\n"; @@ -417,28 +422,68 @@ void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) { } } -void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + InvestigateMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n"; #endif } -void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + ThingMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " << (int)msg->parentId << "\n"; #endif + RemoteParticipantUDP* owner = registry.Get(msg->networkId); + if (owner == nullptr) { + owner = new RemoteParticipantUDP(sender->ipAddress, sender->port); + owner->networkId = msg->networkId; + registry.Add(owner); + } + + Thing* thing = owner->Get(msg->networkId, msg->thingId); + if (thing == nullptr) { + bool isRemote = (sender->networkId != owner->networkId); + thing = ProcessNewThing(owner, msg, isRemote); + thing->id = msg->thingId; + thing->type = msg->thingType; + thing->isRemote = isRemote; + } + + if (msg->parentId != 0) { + thing->SetParent(owner->Get(msg->networkId, msg->parentId)); + if (thing->GetParent() == nullptr) + std::cout << "Could not find parent" << std::endl; + } else + thing->SetParent(nullptr); } -void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { +Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner, + ThingMsg* msg, + bool isRemote) { + switch (msg->thingType) { + case Thing::Type::DistanceSensor: + return new DistanceSensor(owner->root); + case Thing::Type::TouchSensor: + return new TouchSensor(owner->root); + case Thing::Type::DifferentialDrive: + return new DifferentialDrive(owner->root); + default: + return new Thing(owner->root); + } +} + +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + NameMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "] "; #endif - Thing* thing = sender->Get(msg->thingId); + Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) { int nameLength = msg->nameLength; int stringLen = nameLength + 1; @@ -464,23 +509,25 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { #endif } -void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + ModelUrlMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n"; #endif } -void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + PoseMsg* msg) { #if !defined(DEBUG) && !defined(NO_STD) std::cout << this->name << ": process PoseMsg [" << (int)this->networkId << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; #endif - Participant* owner = Participant::registry.Get(msg->networkId); + Participant* owner = registry.Get(msg->networkId); if (owner == nullptr) return; - Thing* thing = owner->Get(msg->thingId); + Thing* thing = owner->Get(msg->networkId, msg->thingId); if (thing == nullptr) return; @@ -494,15 +541,16 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { thing->SetAngularVelocity(msg->angularVelocity); } -void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + BinaryMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n"; #endif - Participant* owner = Participant::registry.Get(msg->networkId); + Participant* owner = registry.Get(msg->networkId); if (owner != nullptr) { - Thing* thing = owner->Get(msg->thingId); + Thing* thing = owner->Get(msg->networkId, msg->thingId); if (thing != nullptr) thing->ProcessBinary(msg->data); #if !defined(NO_STD) @@ -516,20 +564,22 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { } } -void ParticipantUDP::Process(Participant* sender, TextMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + TextMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " " << (int)msg->text << "\n"; #endif } -void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + DestroyMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n"; #endif - Thing* thing = sender->Get(msg->thingId); + Thing* thing = sender->Get(msg->networkId, msg->thingId); if (thing != nullptr) this->Remove(thing); } diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 8b4b769..86eba25 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -5,11 +5,11 @@ #include "Messages/InvestigateMsg.h" #include "Messages/ModelUrlMsg.h" #include "Messages/NameMsg.h" +#include "Messages/NetworkIdMsg.h" #include "Messages/ParticipantMsg.h" #include "Messages/PoseMsg.h" -#include "Messages/NetworkIdMsg.h" -#include "Messages/ThingMsg.h" #include "Messages/TextMsg.h" +#include "Messages/ThingMsg.h" #include "Participant.h" #if !defined(NO_STD) @@ -31,6 +31,74 @@ namespace RoboidControl { constexpr int MAX_SENDER_COUNT = 256; +class RemoteParticipantUDP : public Participant { + public: + /// @brief Create a new participant with the given communcation info + /// @param ipAddress The IP address of the participant + /// @param port The UDP port of the participant + /// @remarks This does not belong here, it should move to ParticipantUDP or + /// something like that in the future + RemoteParticipantUDP(const char* ipAddress, int port); + + /// @brief The Ip Address of a participant. + /// @remarks This does not belong here, it should move to ParticipantUDP or + /// something like that in the future + const char* ipAddress = "0.0.0.0"; + /// @brief The port number for UDP communication with the participant. + /// @remarks This does not belong here, it should move to ParticipantUDP or + /// something like that in the future + unsigned int port = 0; + + bool Send(IMessage* msg) override; +}; + +/// @brief class which manages all known participants +class ParticipantRegistry { + public: + /// @brief Retrieve a participant by its address + /// @param ipAddress The IP address of the participant + /// @param port The port number of the participant + /// @return The participant or a nullptr when it could not be found + RemoteParticipantUDP* Get(const char* ipAddress, unsigned int port); + /// @brief Retrieve a participant by its network ID + /// @param networkID The network ID of the participant + /// @return The participant or a nullptr when it could not be found + RemoteParticipantUDP* Get(unsigned char networkID); + + /// @brief Add a participant with the given details + /// @param ipAddress The IP address of the participant + /// @param port The port number of the participant + /// @return The added participant + RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port); + + /// @brief Add a participant + /// @param participant The participant to add + void Add(RemoteParticipantUDP* participant); + + /// @brief Remove a participant + /// @param participant The participant to remove + void Remove(RemoteParticipantUDP* participant); + + private: +#if defined(NO_STD) + public: + RemoteParticipantUDP** GetAll() const; + int count = 0; + + private: + RemoteParticipantUDP** participants; +#else + public: + /// @brief Get all participants + /// @return All participants + const std::list& GetAll() const; + + private: + /// @brief The list of known participants + std::list participants; +#endif +}; + /// @brief A participant using UDP communication /// A local participant is the local device which can communicate with /// other participants It manages all local things and communcation with other @@ -43,8 +111,7 @@ constexpr int MAX_SENDER_COUNT = 256; /// participant is created which can be obtained using /// RoboidControl::IsolatedParticipant::Isolated(). /// @sa RoboidControl::Thing::Thing() -class ParticipantUDP : public Participant { - +class ParticipantUDPGeneric : public RemoteParticipantUDP { #pragma region Init public: @@ -53,31 +120,32 @@ class ParticipantUDP : public Participant { /// These participant typically broadcast Participant messages to let site /// servers on the local network know their presence. Alternatively they can /// broadcast information which can be used directly by other participants. - ParticipantUDP(int port = 7681); + ParticipantUDPGeneric(int port = 7681); /// @brief Create a participant which will try to connect to a site. /// @param ipAddress The IP address of the site /// @param port The port used by the site /// @param localPort The port used by the local participant - ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); + ParticipantUDPGeneric(const char* ipAddress, + int port = 7681, + int localPort = 7681); #pragma endregion Init #pragma region Properties -public: + public: /// @brief True if the participant is running isolated. /// Isolated participants do not communicate with other participants bool isIsolated = false; /// @brief The remote site when this participant is connected to a site - Participant* remoteSite = nullptr; + RemoteParticipantUDP* remoteSite = nullptr; /// The interval in milliseconds for publishing (broadcasting) data on the /// local network long publishInterval = 3000; // 3 seconds protected: - #if !defined(ARDUINO) #if defined(__unix__) || defined(__APPLE__) int sock; @@ -96,13 +164,13 @@ public: #pragma region Update public: - virtual void Update() override; + virtual void Update(bool recurse = true) override; protected: unsigned long nextPublishMe = 0; /// @brief Prepare the local things for the next update - virtual void PrepMyThings(); + // virtual void PrepMyThings(); virtual void UpdateMyThings(); virtual void UpdateOtherThings(); @@ -114,35 +182,46 @@ public: void PublishThingInfo(Thing* thing); virtual bool Send(IMessage* msg) override; - bool Publish(IMessage* msg); + virtual bool Publish(IMessage* msg) = 0; #pragma endregion Send #pragma region Receive -protected: + protected: void ReceiveData(unsigned char bufferSize, char* senderIpAddress, unsigned int senderPort); - void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant); + void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant); - void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort); + virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0; - void ReceiveUDP(); + virtual void ReceiveUDP() = 0; - virtual void Process(Participant* sender, ParticipantMsg* msg); - virtual void Process(Participant* sender, NetworkIdMsg* msg); - 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); - virtual void Process(Participant* sender, TextMsg* msg); - virtual void Process(Participant* sender, DestroyMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg); + + virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg); + virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender, + ThingMsg* msg, + bool isRemote); + + virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg); #pragma endregion Receive +public: + static ParticipantRegistry registry; + }; } // namespace RoboidControl + +#include "EspIdf/EspIdfParticipant.h" +#include "Posix/PosixParticipant.h" diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index cc61639..42f5da9 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -15,7 +15,7 @@ SiteServer::SiteServer(int port) : ParticipantUDP(port) { this->name = "Site Server"; this->publishInterval = 0; - SetupUDP(port, ipAddress, 0); + //SetupUDP(port, ipAddress, 0); } #pragma endregion Init @@ -36,7 +36,7 @@ void SiteServer::UpdateMyThings() { for (int ix = 0; ix < Participant::registry.count; ix++) { Participant* participant = participants[ix]; #else - for (Participant* participant : Participant::registry.GetAll()) { + for (Participant* participant : registry.GetAll()) { #endif if (participant == nullptr || participant == this) continue; @@ -56,7 +56,7 @@ void SiteServer::UpdateMyThings() { #pragma region Receive -void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { +void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) { if (msg->networkId != sender->networkId) { // std::cout << this->name << " received New Client -> " << // sender->ipAddress @@ -67,19 +67,19 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { } } -void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {} +void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {} -void SiteServer::Process(Participant* sender, ThingMsg* msg) { - Thing* thing = sender->Get(msg->thingId); - if (thing == nullptr) +void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) { + Thing* thing = sender->Get(msg->networkId, msg->thingId); + if (thing == nullptr) { // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); // Thing::Reconstruct(sender, msg->thingType, msg->thingId); //thing = new Thing(msg->thingType, sender->root); - ; + } thing->id = msg->thingId; if (msg->parentId != 0) { - thing->SetParent(Get(msg->parentId)); + thing->SetParent(Get(msg->networkId, msg->parentId)); if (thing->IsRoot()) // if (thing->GetParent() != nullptr) #if defined(NO_STD) diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index 87029c5..1532843 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -33,9 +33,9 @@ class SiteServer : public ParticipantUDP { protected: unsigned long nextPublishMe = 0; - virtual void Process(Participant* sender, ParticipantMsg* msg) override; - virtual void Process(Participant* sender, NetworkIdMsg* msg) override; - virtual void Process(Participant* sender, ThingMsg* msg) override; + virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) override; + virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override; + virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override; #pragma endregion Receive diff --git a/Posix/PosixMQTT.cpp b/Posix/PosixMQTT.cpp new file mode 100644 index 0000000..5b1b2c6 --- /dev/null +++ b/Posix/PosixMQTT.cpp @@ -0,0 +1,2 @@ +#include "PosixMQTT.h" + diff --git a/Posix/PosixParticipant.cpp b/Posix/PosixParticipant.cpp index 27e3e4d..89b7425 100644 --- a/Posix/PosixParticipant.cpp +++ b/Posix/PosixParticipant.cpp @@ -1,4 +1,5 @@ #include "PosixParticipant.h" +#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__) #include @@ -9,11 +10,15 @@ #endif namespace RoboidControl { -namespace Posix { -void ParticipantUDP::Setup(int localPort, - const char* remoteIpAddress, - int remotePort) { +ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {} + +ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) + : ParticipantUDPGeneric(ipAddress, port, localPort) {} + +void ParticipantUDP::SetupUDP(int localPort, + const char* remoteIpAddress, + int remotePort) { #if defined(__unix__) || defined(__APPLE__) // Create a UDP socket @@ -92,7 +97,7 @@ void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) { #endif } -void ParticipantUDP::Receive() { +void ParticipantUDP::ReceiveUDP() { #if defined(__unix__) || defined(__APPLE__) sockaddr_in client_addr; socklen_t len = sizeof(client_addr); @@ -147,7 +152,8 @@ int ParticipantUDP::ReceiveTCP() { return 0; } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, + int bufferSize) { #if defined(__unix__) || defined(__APPLE__) // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << // ntohs(remoteParticipant->port) @@ -173,18 +179,18 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { return true; } -bool ParticipantUDP::SendTCP(int bufferSize) { -#if defined(__unix__) || defined(__APPLE__) - // send(sock, this->buffer, bufferSize, 0); - ssize_t sent_bytes = send(this->sock, this->buffer, bufferSize, 0); - if (sent_bytes < 0) { - std::cerr << "Failed to send packet" << strerror(errno) << std::endl; - close(sock); - return -1; - } -#endif - return false; -} +// bool ParticipantUDP::SendTCP(int bufferSize) { +// #if defined(__unix__) || defined(__APPLE__) +// // send(sock, this->buffer, bufferSize, 0); +// ssize_t sent_bytes = send(this->sock, this->buffer, bufferSize, 0); +// if (sent_bytes < 0) { +// std::cerr << "Failed to send packet" << strerror(errno) << std::endl; +// close(sock); +// return -1; +// } +// #endif +// return false; +// } bool ParticipantUDP::Publish(IMessage* msg) { #if defined(__unix__) || defined(__APPLE__) @@ -208,5 +214,6 @@ bool ParticipantUDP::Publish(IMessage* msg) { return true; } -} // namespace Posix } // namespace RoboidControl + +#endif \ No newline at end of file diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h index 7624e5d..8002ea0 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -1,20 +1,36 @@ #pragma once +#if defined(__unix__) || defined(__APPLE__) #include "Participants/ParticipantUDP.h" namespace RoboidControl { -namespace Posix { -class ParticipantUDP : public RoboidControl::ParticipantUDP { +class ParticipantUDP : public ParticipantUDPGeneric { public: - void Setup(int localPort, const char* remoteIpAddress, int remotePort); + /// @brief Create a participant without connecting to a site + /// @param port The port on which the participant communicates + /// These participant typically broadcast Participant messages to let site + /// servers on the local network know their presence. Alternatively they can + /// broadcast information which can be used directly by other participants. + ParticipantUDP(int port = 7681); + /// @brief Create a participant which will try to connect to a site. + /// @param ipAddress The IP address of the site + /// @param port The port used by the site + /// @param localPort The port used by the local participant + ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); + + void SetupUDP(int localPort, + const char* remoteIpAddress, + int remotePort) override; + void ReceiveUDP() override; + + //void Setup(int localPort, const char* remoteIpAddress, int remotePort); void SetupTCP(const char* remoteIpAddress, int remotePort); - void Receive(); + // void Receive(); int ReceiveTCP(); - bool Send(Participant* remoteParticipant, int bufferSize); - bool SendTCP(int bufferSize); - bool Publish(IMessage* msg); + bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); + bool Publish(IMessage* msg) override; protected: #if defined(__unix__) || defined(__APPLE__) @@ -24,5 +40,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { #endif }; -} // namespace Posix } // namespace RoboidControl +#endif diff --git a/Posix/PoxisMQTT.cpp b/Posix/PoxisMQTT.cpp new file mode 100644 index 0000000..bd62531 --- /dev/null +++ b/Posix/PoxisMQTT.cpp @@ -0,0 +1,47 @@ +#include "PoxisMQTT.h" +#if defined(__unix__) || defined(__APPLE__) + +#include +#include // For fcntl +#include +#include +#include + +namespace RoboidControl { + +RoboidControl::MQTTParticipant::MQTTParticipant(const char* remoteIpAddress, + int port) + : MQTTParticipantBase(remoteIpAddress, port) {} + +void MQTTParticipant::SetupTCP() { + // Create a TCP socket + this->sock = socket(AF_INET, SOCK_STREAM, 0); + if (this->sock < 0) { + std::cerr << "Error creating socket" << std::endl; + return; + } + + this->remote_addr.sin_family = AF_INET; + this->remote_addr.sin_port = htons(this->remoteSite->port); + inet_pton(AF_INET, this->remoteSite->ipAddress, &remote_addr.sin_addr); + + int connect_result = + connect(this->sock, (struct sockaddr*)&remote_addr, sizeof(remote_addr)); + if (connect_result < 0) { //} && errno != EINPROGRESS) { + std::cerr << "Error connecting to server" << std::endl; + close(this->sock); + } + + std::cout << "TCP connection to " << this->remoteSite->ipAddress << ":" << this->remoteSite->port + << "\n"; + + // Set the socket to non-blocking mode + int flags = fcntl(this->sock, F_GETFL, 0); + fcntl(this->sock, F_SETFL, flags | O_NONBLOCK); +} +void MQTTParticipant::SendTCP(int bufferSize) { + send(this->sock, this->buffer, bufferSize, 0); +} + +} // namespace RoboidControl +#endif \ No newline at end of file diff --git a/Posix/PoxisMQTT.h b/Posix/PoxisMQTT.h new file mode 100644 index 0000000..05c447b --- /dev/null +++ b/Posix/PoxisMQTT.h @@ -0,0 +1,23 @@ +#pragma once +#if defined(__unix__) || defined(__APPLE__) + +#include "Participants/ParticipantMQTT.h" + +namespace RoboidControl { + +class MQTTParticipant : public MQTTParticipantBase { +public: + MQTTParticipant(const char* ipAddress, int port = 1883); + protected: + void SetupTCP() override; + + void SendTCP(int bufferSize) override; + +#if defined(__unix__) || defined(__APPLE__) + sockaddr_in remote_addr; +#endif + +}; + +} +#endif \ No newline at end of file diff --git a/README.md b/README.md index e0a3c1a..77862b9 100644 --- a/README.md +++ b/README.md @@ -14,4 +14,60 @@ Supporting: # Basic components - RoboidControl::Thing -- RoboidControl::Participant \ No newline at end of file +- RoboidControl::Participant + +# Installation + +## Core code + +The repository uses cmake for building. You can place it in a subfolder of your project and include it in you `CMakeLists.txt`. +For example if the library is placed in the subfolder `roboidcontrol`: +``` +# Add the path to Roboid Control +add_subdirectory(roboidcontrol) + +# Your source files/executable +add_executable(my_executable main.cpp) + +# Link against RoboidControl +target_link_libraries(my_executable RoboidControl) +``` + +## Arduino (PlatformIO) + +Arduino is only supported in combination with PlatformIO. The Arduino IDE is not (yet?) supported. + +The best way to include support for Roboid Control in PlatformIO is +to clone the Roboid Control for C++ repository into a subfolder of the /lib folder. +Alternatively you can download the zip file and unpack it as a subfolder of the /lib folder. + +## ESP-IDF + +The best way to include support for Roboid Control in PlatformIO is +to clone the Roboid Control for C++ repository into a subfolder of the /components folder. +Alternatively you can download the zip file and unpack it as a subfolder of the /components folder. + +Make sure you have included RoboidControl as a component in your top-level CMakeLists.txt, for example: +``` +list(APPEND EXTRA_COMPONENT_DIRS + components/RoboidControl + ) +``` + +# Get Started + +## Core C++ Examples + +This repository contains examples in the `examples` folder. You can build these using cmake. + +For example, to build the BB2A example: +``` +cmake -B build -D BUILD_EXAMPLE_BB2A=ON +cmake --build build +``` +The resulting executable is then `build/examples/Debug/BB2A.exe` + +## Arduino (PlatformIO) Examples + +Specific examples for the Arduino platform are found in the `Arduino\examples` folder. +To use them you should create a new project in PlatformIO and then copy the example code to your project. \ No newline at end of file diff --git a/Testing/Temporary/LastTest.log b/Testing/Temporary/LastTest.log new file mode 100644 index 0000000..3ab3dd5 --- /dev/null +++ b/Testing/Temporary/LastTest.log @@ -0,0 +1,3 @@ +Start testing: Jun 17 17:17 W. Europe Summer Time +---------------------------------------------------------- +End testing: Jun 17 17:17 W. Europe Summer Time diff --git a/Thing.cpp b/Thing.cpp index 5f9694c..e1e80f7 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -42,7 +42,7 @@ Thing::Thing(Participant* owner) { this->owner = owner; this->owner->Add(this); - // std::cout << this->owner->name << ": New root thing " << std::endl; + std::cout << this->owner->name << ": New root thing " << std::endl; } void Thing::CreateRoot(Participant* owner) { @@ -253,12 +253,6 @@ unsigned long Thing::GetTimeMs() { #endif } -// void Thing::Update(bool recursive) { -// Update(GetTimeMs(), recursive); -// } - -void Thing::PrepareForUpdate() {} - void Thing::Update(bool recursive) { this->positionUpdated = false; this->orientationUpdated = false; @@ -268,7 +262,7 @@ void Thing::Update(bool recursive) { this->nameChanged = false; if (recursive) { - // std::cout << "# children: " << (int)this->childCount << std::endl; + std::cout << "# children: " << (int)this->childCount << std::endl; for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { Thing* child = this->children[childIx]; if (child == nullptr) diff --git a/Thing.h b/Thing.h index e425f9b..ad57b31 100644 --- a/Thing.h +++ b/Thing.h @@ -10,7 +10,7 @@ namespace RoboidControl { class Participant; -class ParticipantUDP; +class ParticipantUDPGeneric; #define THING_STORE_SIZE 256 // IMPORTANT: values higher than 256 will need to change the Thing::id type @@ -88,6 +88,11 @@ class Thing { /// This can be either a Thing::Type of a byte value for custom types unsigned char type = Type::Undetermined; + /// @brief Is this a remote thing? + /// A remote thing is owned by other participant + /// and is not simulated by the local participant + bool isRemote = false; + /// @brief The participant owning this thing Participant* owner = nullptr; @@ -223,7 +228,7 @@ class Thing { #pragma region Update public: - virtual void PrepareForUpdate(); + //virtual void PrepareForUpdate(); /// @brief Updates the state of the thing /// @param recurse When true, this will Update the descendants recursively diff --git a/Things/DistanceSensor.cpp b/Things/DistanceSensor.cpp new file mode 100644 index 0000000..c33d64a --- /dev/null +++ b/Things/DistanceSensor.cpp @@ -0,0 +1,29 @@ +#include "DistanceSensor.h" + +#include "Messages/LowLevelMessages.h" + +namespace RoboidControl { + +DistanceSensor::DistanceSensor(Thing* parent) : Thing(parent) { + this->type = Type::DistanceSensor; + this->name = "Distance sensor"; +} + +float DistanceSensor::GetDistance() { + if (this->externalDistance < this->internalDistance) + return this->externalDistance; + else + return this->internalDistance; +} + +int DistanceSensor::GenerateBinary(char* bytes, unsigned char* ix) { + LowLevelMessages::SendFloat16(bytes, ix, this->internalDistance); + return *ix; +} + +void DistanceSensor::ProcessBinary(char* bytes) { + unsigned char ix = 0; + this->externalDistance = LowLevelMessages::ReceiveFloat16(bytes, &ix); +} + +} // namespace RoboidControl \ No newline at end of file diff --git a/Things/DistanceSensor.h b/Things/DistanceSensor.h new file mode 100644 index 0000000..08e324d --- /dev/null +++ b/Things/DistanceSensor.h @@ -0,0 +1,41 @@ +#pragma once + +#if !NO_STD +#include +#endif + +#include "Thing.h" + +namespace RoboidControl { + +/// @brief A sensor measuring distance +class DistanceSensor : public Thing { + public: + /// @brief Create a new child touch sensor + /// @param parent The parent thing + /// @param thingId The ID of the thing, leave out or set to zero to generate + /// an ID + DistanceSensor(Thing* parent = Thing::LocalRoot()); + + /// @brief Get the current distance + float GetDistance(); + + /// @brief Function used to generate binary data for this sensor + /// @param buffer The byte array for thw binary data + /// @param ix The starting position for writing the binary data + int GenerateBinary(char* bytes, unsigned char* ix) override; + /// @brief Function used to process binary data received for this sensor + /// @param bytes The binary data to process + virtual void ProcessBinary(char* bytes) override; + + protected: +#if ARDUNIO + float internalDistance = INFINITY; + float externalDistance = INFINITY; +#else + float internalDistance = std::numeric_limits::infinity(); + float externalDistance = std::numeric_limits::infinity(); +#endif +}; + +} // namespace RoboidControl diff --git a/Things/TouchSensor.cpp b/Things/TouchSensor.cpp index 9d347db..e5a593b 100644 --- a/Things/TouchSensor.cpp +++ b/Things/TouchSensor.cpp @@ -11,9 +11,9 @@ bool TouchSensor::IsTouching() { return this->internalTouch || this->externalTouch; } -void TouchSensor::PrepareForUpdate() { - //this->internalTouch = this->externalTouch; -} +// void TouchSensor::PrepareForUpdate() { +// //this->internalTouch = this->externalTouch; +// } void TouchSensor::Update(bool recursive) { Thing::Update(recursive); diff --git a/Things/TouchSensor.h b/Things/TouchSensor.h index e8eb2bc..bdc5279 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -6,7 +6,7 @@ namespace RoboidControl { /// @brief A sensor which can detect touches class TouchSensor : public Thing { -// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor +// When finishing this release (0.3), I notice that this is equivalent to a digital sensor public: /// @brief Create a new child touch sensor /// @param parent The parent thing @@ -18,7 +18,7 @@ class TouchSensor : public Thing { /// otherwise bool IsTouching(); - virtual void PrepareForUpdate() override; + //virtual void PrepareForUpdate() override; virtual void Update(bool recursive) override; /// @brief Function used to generate binary data for this touch sensor diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index ff26ce5..b2b8cdd 100644 --- a/Windows/WindowsParticipant.cpp +++ b/Windows/WindowsParticipant.cpp @@ -1,4 +1,5 @@ #include "WindowsParticipant.h" +#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64) #include @@ -7,7 +8,6 @@ #endif namespace RoboidControl { -namespace Windows { ParticipantUDP::ParticipantUDP() {} @@ -195,7 +195,7 @@ int ParticipantUDP::ReceiveTCP() { return 0; } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) { #if defined(_WIN32) || defined(_WIN64) char ip_str[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN); @@ -247,5 +247,6 @@ bool ParticipantUDP::Publish(IMessage* msg) { return true; } -} // namespace Windows } // namespace RoboidControl + +#endif \ No newline at end of file diff --git a/Windows/WindowsParticipant.h b/Windows/WindowsParticipant.h index 9405c36..c0caf2d 100644 --- a/Windows/WindowsParticipant.h +++ b/Windows/WindowsParticipant.h @@ -1,16 +1,16 @@ #pragma once +#if defined(_WIN32) || defined(_WIN64) #include "Participants/ParticipantUDP.h" namespace RoboidControl { -namespace Windows { -class ParticipantUDP : public RoboidControl::ParticipantUDP { +class ParticipantUDP : public ParticipantUDPGeneric { public: ParticipantUDP(); void Setup(int localPort, const char* remoteIpAddress, int remotePort); void SetupTCP(const char* remoteIpAddres, int remotePort); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool SendTCP(int bufferSize); bool Publish(IMessage* msg); @@ -23,5 +23,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { #endif }; -} // namespace Windows } // namespace RoboidControl +#endif \ No newline at end of file diff --git a/examples/BB2B.cpp b/examples/BB2A/main.cpp similarity index 96% rename from examples/BB2B.cpp rename to examples/BB2A/main.cpp index 05d80f4..ef8f269 100644 --- a/examples/BB2B.cpp +++ b/examples/BB2A/main.cpp @@ -40,9 +40,9 @@ int main() { // and sleep for 100ms #if defined(ARDUINO) - delay(100); + delay(10); #else - sleep_for(milliseconds(100)); + sleep_for(milliseconds(10)); #endif } diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 00282a2..0cd9d2a 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,25 +1,8 @@ # examples/CMakeLists.txt -# Specify the minimum CMake version -cmake_minimum_required(VERSION 3.10) +# Check if the options are enabled and add the corresponding examples +if(BUILD_EXAMPLE_BB2A) + add_executable(BB2A BB2A/main.cpp) # Adjust the path as necessary -# Specify the path to the main project directory -set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..") - -# Set the project name -project(Examples) - -include_directories(..) - -# Add the executable for the main project -#add_executable(MainExecutable ${SOURCES}) -# Find the main project library (assuming it's defined in the root CMakeLists.txt) -#find_package(RoboidControl REQUIRED) # Replace MyLibrary with your actual library name - -# Add example executables -add_executable(BB2B BB2B.cpp) -target_link_libraries( - BB2B - RoboidControl - LinearAlgebra -) + target_link_libraries(BB2A RoboidControl) +endif() diff --git a/examples/README.md b/examples/README.md index b694fe9..bfba240 100644 --- a/examples/README.md +++ b/examples/README.md @@ -1 +1 @@ -Important: this folder has to be names 'examples' exactly to maintain compatibility with Arduino \ No newline at end of file +Important: this folder has to be named 'examples' exactly to maintain compatibility with Arduino \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..99ee89c --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,30 @@ +# Unit test configuration + +add_compile_definitions(GTEST) +include(FetchContent) +FetchContent_Declare( + googletest + DOWNLOAD_EXTRACT_TIMESTAMP ON + URL https://github.com/google/googletest/archive/refs/heads/main.zip +) + +# For Windows: Prevent overriding the parent project's compiler/linker settings +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) +FetchContent_MakeAvailable(googletest) + +file(GLOB_RECURSE test_srcs *.cc) +message(STATUS "Test sources: ${test_srcs}") +add_executable( + RoboidControlTest + ${test_srcs} +) +message(STATUS "RoboidControlTest target created") +target_link_libraries( + RoboidControlTest + gtest_main + RoboidControl + LinearAlgebra +) + +include(GoogleTest) +gtest_discover_tests(RoboidControlTest) \ No newline at end of file diff --git a/test/participant_test.cc b/test/participant_test.cc index cf9513c..bf5bf35 100644 --- a/test/participant_test.cc +++ b/test/participant_test.cc @@ -77,7 +77,7 @@ TEST(Participant, ThingMsg) { } TEST(Participant, MQTT) { - ParticipantMQTT* participant = new ParticipantMQTT("192.168.77.11"); + MQTTParticipantBase* participant = new MQTTParticipantBase("192.168.77.11"); unsigned long milliseconds = Thing::GetTimeMs(); unsigned long startTime = milliseconds;