From df19ecf95381982bdd7426b66bab2ab2f7eef132 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 15 Apr 2025 12:38:02 +0200 Subject: [PATCH 01/18] Attempts to get I2C communcation working --- Participants/ParticipantUDP.cpp | 4 +++- Thing.cpp | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index f643587..8ead868 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -104,6 +104,7 @@ void ParticipantUDP::Update(unsigned long currentTimeMs) { if (thing == nullptr) continue; + std::cout << "Update thing " << (int)thing->id << std::endl; if (this->isIsolated == false) { PoseMsg* poseMsg = new PoseMsg(this->networkId, thing); this->Send(thing->owner, poseMsg); @@ -353,7 +354,8 @@ void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) { void ParticipantUDP::Process(Participant* 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"; + << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " + << (int)msg->parentId << "\n"; #endif } diff --git a/Thing.cpp b/Thing.cpp index 86e4008..96c888f 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -43,7 +43,7 @@ Thing::Thing(Participant* owner, int thingType, unsigned char thingId) { // std::cout << "add thing [" << (int)this->id << "] to owner " // << this->owner->ipAddress << ":" << this->owner->port << std::endl; - this->owner->Add(this, false); + this->owner->Add(this, true); } // Thing::Thing(Participant* owner, From 70c4afca89c208d21a25facbd60eff611805f69f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 17 Jun 2025 15:50:56 +0200 Subject: [PATCH 02/18] Add setup documentation --- CMakeLists.txt | 17 +++++---- DoxyGen/Doxyfile | 2 +- Participant.cpp | 2 +- Participants/ParticipantUDP.cpp | 4 ++- README.md | 54 +++++++++++++++++++++++++++- examples/{BB2B.cpp => BB2A/main.cpp} | 0 examples/CMakeLists.txt | 27 +++----------- examples/README.md | 2 +- 8 files changed, 74 insertions(+), 34 deletions(-) rename examples/{BB2B.cpp => BB2A/main.cpp} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index e4a9a1b..bcec558 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,9 +22,18 @@ else() set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard set(CMAKE_POSITION_INDEPENDENT_CODE ON) + include_directories( + . + LinearAlgebra + ) + add_library(RoboidControl STATIC ${srcs}) + project(RoboidControl) add_subdirectory(LinearAlgebra) - add_subdirectory(Examples) + + # Examples + option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF) + add_subdirectory(examples) add_compile_definitions(GTEST) include(FetchContent) @@ -38,12 +47,6 @@ else() set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) - include_directories( - . - LinearAlgebra - ) - add_library(RoboidControl STATIC ${srcs}) - enable_testing() file(GLOB_RECURSE test_srcs test/*_test.cc) 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/Participant.cpp b/Participant.cpp index b8e0d16..dc02a15 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -67,7 +67,7 @@ bool Participant::Send(IMessage* msg) { #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); - return thisWindows->Send(remoteParticipant, bufferSize); + return thisWindows->Send(this, bufferSize); #elif defined(__unix__) || defined(__APPLE__) Posix::ParticipantUDP* thisPosix = static_cast(this); return thisPosix->Send(this, bufferSize); diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index fc99aae..06d3fed 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -232,7 +232,9 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, bool ParticipantUDP::Send(IMessage* msg) { if (this->remoteSite != nullptr) - this->remoteSite->Send(msg); + return this->remoteSite->Send(msg); + + return true; } void ParticipantUDP::PublishThingInfo(Thing* thing) { diff --git a/README.md b/README.md index e0a3c1a..876fdf2 100644 --- a/README.md +++ b/README.md @@ -14,4 +14,56 @@ 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_subdirectory(roboidcontrol) + +add_executable(my_executable main.cpp) # Your source files/executable +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/examples/BB2B.cpp b/examples/BB2A/main.cpp similarity index 100% rename from examples/BB2B.cpp rename to examples/BB2A/main.cpp 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 From 3bb3c68e9530a24995fade45ce763298aaaf6292 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 17 Jun 2025 17:18:17 +0200 Subject: [PATCH 03/18] Moved unit test cmake commands to test --- CMakeLists.txt | 27 +-------------------------- Testing/Temporary/LastTest.log | 3 +++ test/CMakeLists.txt | 30 ++++++++++++++++++++++++++++++ 3 files changed, 34 insertions(+), 26 deletions(-) create mode 100644 Testing/Temporary/LastTest.log create mode 100644 test/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index bcec558..63a3359 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,33 +35,8 @@ else() option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF) 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) - 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/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/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 From f4d36ad960928f9321bf9351cbcde1ff50cab50b Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 17 Jun 2025 17:55:01 +0200 Subject: [PATCH 04/18] Add target_include_directories --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 63a3359..75b9504 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,9 @@ else() LinearAlgebra ) add_library(RoboidControl STATIC ${srcs}) + target_include_directories(RoboidControl PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + ) project(RoboidControl) add_subdirectory(LinearAlgebra) From 6532d656de66131d41b8b3cc01e99f20d3498d0e Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 17 Jun 2025 17:58:07 +0200 Subject: [PATCH 05/18] Updated core code installation doc. --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 876fdf2..77862b9 100644 --- a/README.md +++ b/README.md @@ -23,9 +23,13 @@ Supporting: 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) -add_executable(my_executable main.cpp) # Your source files/executable +# Your source files/executable +add_executable(my_executable main.cpp) + +# Link against RoboidControl target_link_libraries(my_executable RoboidControl) ``` From e5916466a6c353309c27873b9e35600bc6178508 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 14:00:39 +0200 Subject: [PATCH 06/18] Fixed force sending posmsg --- Messages/PoseMsg.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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; } From e89c2467539019e7fa845c907c35d7fe0744d8e9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 14:04:01 +0200 Subject: [PATCH 07/18] Increased update rate --- examples/BB2A/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/BB2A/main.cpp b/examples/BB2A/main.cpp index 05d80f4..ef8f269 100644 --- a/examples/BB2A/main.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 } From e0b8f3eac3b92898cb07f9ab4f36f8565c0d6cd8 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 14:37:06 +0200 Subject: [PATCH 08/18] Fix touchsensor not resetting --- Arduino/Things/UltrasonicSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 338353e..804c2bc 100644 --- a/Arduino/Things/UltrasonicSensor.cpp +++ b/Arduino/Things/UltrasonicSensor.cpp @@ -65,7 +65,7 @@ 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->internalTouch = (this->ultrasonic.distance > 0 && this->ultrasonic.distance <= this->touchDistance); } From 6eafbe538f168a75c7994e142e00233b1f530a0a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 15:27:08 +0200 Subject: [PATCH 09/18] Fix intialization issues --- Arduino/Things/DRV8833.cpp | 11 +++++++---- Arduino/Things/DRV8833.h | 2 ++ 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index ed04d79..77dbbbc 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -8,6 +8,7 @@ namespace Arduino { #pragma region DRV8833 DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { + std::cout << "DRV8833\n"; this->type = Type::Undetermined; this->name = "DRV8833"; this->pinStandby = config.standby; @@ -26,10 +27,12 @@ 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) {} + : drv8833(config, this), + RoboidControl::DifferentialDrive(nullptr, nullptr, parent) { + this->drv8833 = DRV8833(config, this); + this->leftWheel = this->drv8833.motorA; + this->rightWheel = this->drv8833.motorB; +} void DRV8833::DifferentialDrive::Update(bool recurse) { RoboidControl::DifferentialDrive::Update(recurse); diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index 9762980..a4cbd9f 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 From 634d560ee1b7a596c9e41aa0dd48b03ee594bb3a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 15:38:46 +0200 Subject: [PATCH 10/18] Fix crash when updating DRV8833 Diff Drive --- Arduino/Things/DRV8833.cpp | 12 +++++------- Arduino/Things/DRV8833.h | 2 +- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp index 77dbbbc..7421e73 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -8,7 +8,6 @@ namespace Arduino { #pragma region DRV8833 DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { - std::cout << "DRV8833\n"; this->type = Type::Undetermined; this->name = "DRV8833"; this->pinStandby = config.standby; @@ -27,16 +26,15 @@ DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) { DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, Thing* parent) - : drv8833(config, this), - RoboidControl::DifferentialDrive(nullptr, nullptr, parent) { - this->drv8833 = DRV8833(config, this); - this->leftWheel = this->drv8833.motorA; - this->rightWheel = this->drv8833.motorB; + : 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 diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h index a4cbd9f..9cc7d1a 100644 --- a/Arduino/Things/DRV8833.h +++ b/Arduino/Things/DRV8833.h @@ -45,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 From 17916ae3dbdf608b668357967a1b24307f72fa62 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Jun 2025 17:16:42 +0200 Subject: [PATCH 11/18] Cleanup --- Arduino/Examples/BB2A/BB2A.cpp | 95 +++++++++++++++++++++++++++ Arduino/Examples/BB2A/configuration.h | 52 +++++++++++++++ Arduino/Things/DRV8833.cpp | 3 +- Participants/ParticipantUDP.cpp | 51 -------------- Participants/ParticipantUDP.h | 2 +- Thing.cpp | 6 -- Thing.h | 2 +- Things/TouchSensor.cpp | 6 +- Things/TouchSensor.h | 2 +- 9 files changed, 154 insertions(+), 65 deletions(-) create mode 100644 Arduino/Examples/BB2A/BB2A.cpp create mode 100644 Arduino/Examples/BB2A/configuration.h diff --git a/Arduino/Examples/BB2A/BB2A.cpp b/Arduino/Examples/BB2A/BB2A.cpp new file mode 100644 index 0000000..440fc87 --- /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; + +ParticipantUDP* 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 ParticipantUDP("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 7421e73..9ab9839 100644 --- a/Arduino/Things/DRV8833.cpp +++ b/Arduino/Things/DRV8833.cpp @@ -49,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/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 06d3fed..dc26caf 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -86,8 +86,6 @@ void ParticipantUDP::SetupUDP(int localPort, void ParticipantUDP::Update() { unsigned long currentTimeMs = Thing::GetTimeMs(); - PrepMyThings(); - if (this->isIsolated == false) { if (this->connected == false) begin(); @@ -111,68 +109,19 @@ void ParticipantUDP::Update() { 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; 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? // 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"; } } diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 8b4b769..a23bb6b 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -102,7 +102,7 @@ public: 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(); diff --git a/Thing.cpp b/Thing.cpp index 5f9694c..efb6bfb 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -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; diff --git a/Thing.h b/Thing.h index e425f9b..f538c33 100644 --- a/Thing.h +++ b/Thing.h @@ -223,7 +223,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/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..9890ef8 100644 --- a/Things/TouchSensor.h +++ b/Things/TouchSensor.h @@ -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 From 230a41f1434be09ad0ec299437197fcdce73bf83 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 19 Jun 2025 11:58:25 +0200 Subject: [PATCH 12/18] Add Distance Sensor --- Arduino/Things/UltrasonicSensor.cpp | 28 +++++++++++++++++--- Arduino/Things/UltrasonicSensor.h | 20 +++++++++++++- Things/DistanceSensor.cpp | 29 ++++++++++++++++++++ Things/DistanceSensor.h | 41 +++++++++++++++++++++++++++++ Things/TouchSensor.h | 2 +- 5 files changed, 115 insertions(+), 5 deletions(-) create mode 100644 Things/DistanceSensor.cpp create mode 100644 Things/DistanceSensor.h diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp index 804c2bc..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) @@ -66,10 +88,10 @@ 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->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/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.h b/Things/TouchSensor.h index 9890ef8..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 From 1add0647e1c5517d1faa67f0ad666f1d80e74467 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 26 Jun 2025 16:33:33 +0200 Subject: [PATCH 13/18] Improved new thing processing --- Arduino/ArduinoParticipant.cpp | 2 +- Arduino/ArduinoParticipant.h | 2 +- EspIdf/EspIdfParticipant.cpp | 2 +- EspIdf/EspIdfParticipant.h | 2 +- Participant.cpp | 18 +++++------ Participant.h | 5 ++-- Participants/ParticipantUDP.cpp | 53 +++++++++++++++++++++++++++++---- Participants/ParticipantUDP.h | 5 +++- Participants/SiteServer.cpp | 8 ++--- Posix/PosixParticipant.cpp | 2 +- Posix/PosixParticipant.h | 2 +- Thing.h | 5 ++++ Windows/WindowsParticipant.cpp | 2 +- Windows/WindowsParticipant.h | 2 +- 14 files changed, 80 insertions(+), 30 deletions(-) diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index a787d5a..6d5c7f0 100644 --- a/Arduino/ArduinoParticipant.cpp +++ b/Arduino/ArduinoParticipant.cpp @@ -88,7 +88,7 @@ void ParticipantUDP::Receive() { #endif } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) { #if defined(ARDUINO) && defined(HAS_WIFI) // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" // << remoteParticipant->port << "\n"; diff --git a/Arduino/ArduinoParticipant.h b/Arduino/ArduinoParticipant.h index 5a9396f..5211300 100644 --- a/Arduino/ArduinoParticipant.h +++ b/Arduino/ArduinoParticipant.h @@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 22e8160..ca853a5 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -130,7 +130,7 @@ void ParticipantUDP::Receive() { #endif // IDF_VER } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) // std::cout << "Sending to " << remoteParticipant->ipAddress << ":" // << remoteParticipant->port << "\n"; diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index cf1c6bf..565c0fe 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -13,7 +13,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: diff --git a/Participant.cpp b/Participant.cpp index 3f1adc6..697872c 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -49,10 +49,10 @@ Participant::~Participant() { delete[] this->ipAddress; } -void Participant::Update() { +void Participant::Update(bool recurse) { for (Thing* thing : this->things) { if (thing != nullptr) - thing->Update(true); + thing->Update(); } } @@ -67,26 +67,26 @@ 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 @@ -115,7 +115,7 @@ void Participant::Add(Thing* thing, bool checkId) { // 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; diff --git a/Participant.h b/Participant.h index 6e1217d..f8ecc9e 100644 --- a/Participant.h +++ b/Participant.h @@ -112,9 +112,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 +130,7 @@ class Participant { public: /// @brief Update all things for this participant - virtual void Update(); + virtual void Update(bool recurse = true); #pragma endregion Update diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index dc26caf..0830434 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -8,6 +8,10 @@ #include "Posix/PosixParticipant.h" #include "Windows/WindowsParticipant.h" +#include "Things/DistanceSensor.h" +#include "Things/TouchSensor.h" +#include "Things/DifferentialDrive.h" + #include namespace RoboidControl { @@ -83,7 +87,7 @@ void ParticipantUDP::SetupUDP(int localPort, // 1. receive external messages // 2. update the state // 3. send out the updated messages -void ParticipantUDP::Update() { +void ParticipantUDP::Update(bool recurse) { unsigned long currentTimeMs = Thing::GetTimeMs(); if (this->isIsolated == false) { @@ -182,7 +186,7 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, bool ParticipantUDP::Send(IMessage* msg) { if (this->remoteSite != nullptr) return this->remoteSite->Send(msg); - + return true; } @@ -379,6 +383,43 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) { << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " << (int)msg->parentId << "\n"; #endif + Participant* owner = Participant::registry.Get(msg->networkId); + if (owner == nullptr) { + owner = new Participant(); + owner->networkId = msg->networkId; + Participant::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); +} + +Thing* ParticipantUDP::ProcessNewThing(Participant* 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 ParticipantUDP::Process(Participant* sender, NameMsg* msg) { @@ -387,7 +428,7 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { << (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; @@ -429,7 +470,7 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { if (owner == nullptr) return; - Thing* thing = owner->Get(msg->thingId); + Thing* thing = owner->Get(msg->networkId, msg->thingId); if (thing == nullptr) return; @@ -451,7 +492,7 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { Participant* owner = Participant::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) @@ -478,7 +519,7 @@ void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) { << (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 a23bb6b..0eb205b 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -96,7 +96,7 @@ public: #pragma region Update public: - virtual void Update() override; + virtual void Update(bool recurse = true) override; protected: unsigned long nextPublishMe = 0; @@ -133,7 +133,10 @@ protected: 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 Thing* ProcessNewThing(Participant* sender, ThingMsg* msg, bool isRemote); + virtual void Process(Participant* sender, NameMsg* msg); virtual void Process(Participant* sender, ModelUrlMsg* msg); virtual void Process(Participant* sender, PoseMsg* msg); diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index cc61639..085cd6f 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -70,16 +70,16 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {} void SiteServer::Process(Participant* sender, ThingMsg* msg) { - Thing* thing = sender->Get(msg->thingId); - if (thing == nullptr) + 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/Posix/PosixParticipant.cpp b/Posix/PosixParticipant.cpp index a01a059..9d44299 100644 --- a/Posix/PosixParticipant.cpp +++ b/Posix/PosixParticipant.cpp @@ -90,7 +90,7 @@ void ParticipantUDP::Receive() { #endif } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) { #if defined(__unix__) || defined(__APPLE__) // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port) // << "\n"; diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h index 98302f3..fc20888 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: diff --git a/Thing.h b/Thing.h index f538c33..f556712 100644 --- a/Thing.h +++ b/Thing.h @@ -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; diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index c6e99db..5268787 100644 --- a/Windows/WindowsParticipant.cpp +++ b/Windows/WindowsParticipant.cpp @@ -102,7 +102,7 @@ void ParticipantUDP::Receive() { #endif // _WIN32 || _WIN64 } -bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { +bool ParticipantUDP::SendTo(Participant* 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); diff --git a/Windows/WindowsParticipant.h b/Windows/WindowsParticipant.h index 9a747d9..46cb629 100644 --- a/Windows/WindowsParticipant.h +++ b/Windows/WindowsParticipant.h @@ -9,7 +9,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { public: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); - bool Send(Participant* remoteParticipant, int bufferSize); + bool SendTo(Participant* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: From 9a26e98b0f4353b4daaef44da8bda169a14f401f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 26 Jun 2025 17:25:32 +0200 Subject: [PATCH 14/18] Improved wifi code (but still not working) --- EspIdf/EspIdfParticipant.cpp | 29 ++++++++++++++--------------- EspIdf/EspIdfParticipant.h | 2 +- Participant.cpp | 14 +++++++------- Participants/ParticipantUDP.cpp | 10 +++++++--- Thing.cpp | 4 ++-- 5 files changed, 31 insertions(+), 28 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index ca853a5..096f32c 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -22,26 +22,25 @@ 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 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; } @@ -54,7 +53,7 @@ void ParticipantUDP::Setup(int localPort, inet_pton(AF_INET, this->remoteSite->ipAddress, &this->dest_addr.sin_addr.s_addr); - std::cout << "Wifi sync started local " << this->port << ", remote " + std::cout << "Wifi sync started local " << localPort << ", remote " << this->remoteSite->ipAddress << ":" << this->remoteSite->port << "\n"; #endif // IDF_VER @@ -86,7 +85,7 @@ void ParticipantUDP::GetBroadcastAddress() { void ParticipantUDP::Receive() { #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 +102,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"; @@ -132,10 +131,10 @@ void ParticipantUDP::Receive() { bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) { #if defined(IDF_VER) - // std::cout << "Sending to " << remoteParticipant->ipAddress << ":" - // << remoteParticipant->port << "\n"; + std::cout << "Sending to " << remoteParticipant->ipAddress << ":" + << remoteParticipant->port << "\n"; - int err = sendto(this->sockfd, buffer, bufferSize, 0, + int err = sendto(this->sock, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr)); if (errno != 0) std::cout << "Send error " << err << " or " << errno << "\n"; @@ -154,7 +153,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"; diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index 565c0fe..b744971 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -20,7 +20,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { #if defined(IDF_VER) char broadcastIpAddress[INET_ADDRSTRLEN]; - int sockfd; + int sock; struct sockaddr_in dest_addr; // struct sockaddr_in src_addr; #endif diff --git a/Participant.cpp b/Participant.cpp index 697872c..74d3e60 100644 --- a/Participant.cpp +++ b/Participant.cpp @@ -112,8 +112,8 @@ 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->owner->networkId, thing->id); if (foundThing == nullptr) { @@ -122,12 +122,12 @@ void Participant::Add(Thing* thing, bool checkId) { #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"; } } } diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 0830434..a0623e8 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -8,9 +8,9 @@ #include "Posix/PosixParticipant.h" #include "Windows/WindowsParticipant.h" +#include "Things/DifferentialDrive.h" #include "Things/DistanceSensor.h" #include "Things/TouchSensor.h" -#include "Things/DifferentialDrive.h" #include @@ -28,6 +28,7 @@ ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) { this->root = Thing::LocalRoot(); //::LocalParticipant->root; this->root->owner = this; this->root->name = "UDP Root"; + std::cout << "P2 " << (int)this->root << std::endl; this->Add(this->root); Participant::ReplaceLocalParticipant(*this); @@ -45,6 +46,7 @@ ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; this->root->owner = this; this->root->name = "UDP Root"; + std::cout << "P1 " << (int)this->root << std::endl; this->Add(this->root); Participant::ReplaceLocalParticipant(*this); @@ -102,7 +104,6 @@ void ParticipantUDP::Update(bool recurse) { this->Send(msg); delete msg; - this->nextPublishMe = currentTimeMs + this->publishInterval; } @@ -114,10 +115,13 @@ void ParticipantUDP::Update(bool recurse) { } void ParticipantUDP::UpdateMyThings() { + std::cout << "# things = " << this->things.size() << std::endl; for (Thing* thing : this->things) { + std::cout << ".\n"; if (thing == nullptr) // || thing->GetParent() != nullptr) continue; - + std::cout << (int)this->root << std::endl; + std::cout << thing->name << std::endl; // 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) diff --git a/Thing.cpp b/Thing.cpp index efb6bfb..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) { @@ -262,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) From b5f07f77c29035f8a16cd56be53161a785122fdf Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 27 Jun 2025 12:38:44 +0200 Subject: [PATCH 15/18] Major refactoring participants --- Arduino/ArduinoParticipant.cpp | 6 +- Arduino/ArduinoParticipant.h | 8 +- Arduino/Examples/BB2A/BB2A.cpp | 4 +- EspIdf/EspIdfParticipant.cpp | 77 +++++++-- EspIdf/EspIdfParticipant.h | 32 +++- Participant.cpp | 179 ++++++++++--------- Participant.h | 24 +-- Participants/IsolatedParticipant.cpp | 8 +- Participants/IsolatedParticipant.h | 4 +- Participants/ParticipantUDP.cpp | 250 ++++++++++++++++++++++----- Participants/ParticipantUDP.h | 129 +++++++++++--- Participants/SiteServer.cpp | 12 +- Participants/SiteServer.h | 8 +- Posix/PosixParticipant.cpp | 7 +- Posix/PosixParticipant.h | 8 +- Thing.h | 2 +- Windows/WindowsParticipant.cpp | 7 +- Windows/WindowsParticipant.h | 8 +- 18 files changed, 542 insertions(+), 231 deletions(-) diff --git a/Arduino/ArduinoParticipant.cpp b/Arduino/ArduinoParticipant.cpp index 6d5c7f0..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::SendTo(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 5211300..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 SendTo(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 index 440fc87..ada5a24 100644 --- a/Arduino/Examples/BB2A/BB2A.cpp +++ b/Arduino/Examples/BB2A/BB2A.cpp @@ -19,7 +19,7 @@ using namespace RoboidControl; using namespace RoboidControl::Arduino; -ParticipantUDP* localParticipant; +ParticipantUDPGeneric* localParticipant; DifferentialDrive* bb2b; TouchSensor* touchLeft; @@ -34,7 +34,7 @@ void setup() { Serial.println("started"); StartWifi("serrarens", "192.168.76.44"); - localParticipant = new ParticipantUDP("192.168.77.76"); + localParticipant = new ParticipantUDPGeneric("192.168.77.76"); bb2b = new DifferentialDrive(); touchLeft = new TouchSensor(bb2b); diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 096f32c..774f6a3 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -4,10 +4,10 @@ #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) @@ -29,7 +29,8 @@ void ParticipantUDP::Setup(int localPort, return; } - // Set up the receiving address + /* + // Set up the receiving(local) address struct sockaddr_in local_addr; memset(&local_addr, 0, sizeof(local_addr)); local_addr.sin_family = AF_INET; @@ -44,18 +45,33 @@ void ParticipantUDP::Setup(int localPort, 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 " << localPort << ", remote " << this->remoteSite->ipAddress << ":" << this->remoteSite->port << "\n"; + + 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(); + #endif // IDF_VER } @@ -82,8 +98,9 @@ void ParticipantUDP::GetBroadcastAddress() { #endif // IDF_VER } -void ParticipantUDP::Receive() { +void ParticipantUDP::ReceiveUDP() { #if defined(IDF_VER) +/* struct pollfd fds[1]; fds[0].fd = sock; fds[0].events = POLLIN; // We're looking for data available to read @@ -125,17 +142,50 @@ void ParticipantUDP::Receive() { } } // std::cout << "no more messages\n"; - +*/ #endif // IDF_VER } -bool ParticipantUDP::SendTo(Participant* remoteParticipant, int bufferSize) { -#if defined(IDF_VER) - std::cout << "Sending to " << remoteParticipant->ipAddress << ":" - << remoteParticipant->port << "\n"; +ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {} +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; + ParticipantMsg* msg = new ParticipantMsg(this->networkId); + int bSize = msg->Serialize(this->buffer); + 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::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 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); + + std::cout << "socket: " << (int)this->sock << std::endl; int err = sendto(this->sock, buffer, bufferSize, 0, - (struct sockaddr*)&dest_addr, sizeof(dest_addr)); + (struct sockaddr*)&this->dest_addr, sizeof(this->dest_addr)); if (errno != 0) std::cout << "Send error " << err << " or " << errno << "\n"; @@ -161,5 +211,4 @@ bool ParticipantUDP::Publish(IMessage* msg) { return true; }; -} // namespace EspIdf } // namespace RoboidControl diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index b744971..b465843 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -1,21 +1,39 @@ #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 SendTo(Participant* remoteParticipant, int bufferSize); + bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); + bool SendTest(); + + //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]; @@ -28,5 +46,7 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { void GetBroadcastAddress(); }; -} // namespace EspIdf + } // namespace RoboidControl + +#endif diff --git a/Participant.cpp b/Participant.cpp index 74d3e60..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,10 +42,10 @@ 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(bool recurse) { @@ -56,6 +55,13 @@ void Participant::Update(bool recurse) { } } +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) @@ -83,13 +89,14 @@ bool Participant::Send(IMessage* msg) { return false; #endif } +*/ Thing* Participant::Get(unsigned char networkId, unsigned char thingId) { for (Thing* thing : this->things) { if (thing->owner->networkId == networkId && thing->id == thingId) return thing; } - std::cout << "Could not find thing " << this->ipAddress << ":" << this->port + std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port << "[" << (int)thingId << "]\n"; return nullptr; } @@ -112,8 +119,9 @@ 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->owner->networkId, thing->id); if (foundThing == nullptr) { @@ -122,12 +130,13 @@ void Participant::Add(Thing* thing, bool checkId) { #else this->things.push_back(thing); #endif - std::cout << "Add thing " << this->ipAddress << ":" << this->port << + 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"; } } } @@ -155,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 f8ecc9e..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; @@ -143,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/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index a0623e8..2670107 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -16,14 +16,145 @@ 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) { + int bufferSize = msg->Serialize(this->buffer); + if (bufferSize <= 0) + return true; + + // std::cout << "send msg " << (static_cast(this->buffer[0]) & 0xff) + // << " to " << this->ipAddress << std::endl; + + // #if defined(_WIN32) || defined(_WIN64) + // Windows::ParticipantUDP* thisWindows = + // static_cast(this); + // return thisWindows->SendTo(this, bufferSize); + // #elif defined(__unix__) || defined(__APPLE__) + // Posix::ParticipantUDP* thisPosix = + // static_cast(this); return thisPosix->SendTo(this, + // bufferSize); + // #elif defined(ARDUINO) + // Arduino::ParticipantUDP* thisArduino = + // static_cast(this); + // return thisArduino->SendTo(this, bufferSize); + // #elif defined(IDF_VER) + // EspIdf::ParticipantUDP* thisEspIdf = + // static_cast(this); + // return thisEspIdf->SendTo(this, bufferSize); + // #else + return false; + // #endif +} + #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; @@ -34,14 +165,16 @@ ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) { 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; @@ -52,14 +185,15 @@ ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) 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, +/* +void ParticipantUDPGeneric::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(_WIN32) || defined(_WIN64) @@ -80,6 +214,7 @@ void ParticipantUDP::SetupUDP(int localPort, #endif this->connected = true; } + */ #pragma endregion Init @@ -89,32 +224,38 @@ void ParticipantUDP::SetupUDP(int localPort, // 1. receive external messages // 2. update the state // 3. send out the updated messages -void ParticipantUDP::Update(bool recurse) { +void ParticipantUDPGeneric::Update(bool recurse) { unsigned long currentTimeMs = Thing::GetTimeMs(); if (this->isIsolated == false) { if (this->connected == false) begin(); + // EspIdf::ParticipantUDP* thisEspIdf = + // static_cast(this); + // thisEspIdf->SendTest(); + if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { ParticipantMsg* msg = new ParticipantMsg(this->networkId); + if (this->remoteSite == nullptr) this->Publish(msg); else this->Send(msg); + // thisEspIdf->SendTest(); delete msg; this->nextPublishMe = currentTimeMs + this->publishInterval; } - this->ReceiveUDP(); + // this->ReceiveUDP(); } - UpdateMyThings(); - UpdateOtherThings(); + // UpdateMyThings(); + // UpdateOtherThings(); } -void ParticipantUDP::UpdateMyThings() { +void ParticipantUDPGeneric::UpdateMyThings() { std::cout << "# things = " << this->things.size() << std::endl; for (Thing* thing : this->things) { std::cout << ".\n"; @@ -133,13 +274,13 @@ void ParticipantUDP::UpdateMyThings() { } } -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; @@ -167,8 +308,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); @@ -187,14 +328,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 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... @@ -215,7 +356,8 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) { delete customMsg; } -bool ParticipantUDP::Publish(IMessage* msg) { +/* +bool ParticipantUDPGeneric::Publish(IMessage* msg) { // std::cout << "publish msg\n"; #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = @@ -236,13 +378,15 @@ bool ParticipantUDP::Publish(IMessage* msg) { return false; #endif } +*/ // Send #pragma endregion #pragma region Receive -void ParticipantUDP::ReceiveUDP() { +/* +void ParticipantUDPGeneric::ReceiveUDP() { #if defined(_WIN32) || defined(_WIN64) Windows::ParticipantUDP* thisWindows = static_cast(this); @@ -260,26 +404,28 @@ void ParticipantUDP::ReceiveUDP() { 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"; @@ -352,14 +498,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"; @@ -374,24 +522,26 @@ 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 - Participant* owner = Participant::registry.Get(msg->networkId); + RemoteParticipantUDP* owner = registry.Get(msg->networkId); if (owner == nullptr) { - owner = new Participant(); + owner = new RemoteParticipantUDP(sender->ipAddress, sender->port); owner->networkId = msg->networkId; - Participant::registry.Add(owner); + registry.Add(owner); } Thing* thing = owner->Get(msg->networkId, msg->thingId); @@ -411,9 +561,9 @@ void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) { thing->SetParent(nullptr); } -Thing* ParticipantUDP::ProcessNewThing(Participant* owner, - ThingMsg* msg, - bool isRemote) { +Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner, + ThingMsg* msg, + bool isRemote) { switch (msg->thingType) { case Thing::Type::DistanceSensor: return new DistanceSensor(owner->root); @@ -426,7 +576,8 @@ Thing* ParticipantUDP::ProcessNewThing(Participant* owner, } } -void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { +void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender, + NameMsg* msg) { #if defined(DEBUG) std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/" << (int)msg->thingId << "] "; @@ -458,19 +609,21 @@ 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; @@ -488,13 +641,14 @@ 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->networkId, msg->thingId); if (thing != nullptr) @@ -510,14 +664,16 @@ 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"; diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 0eb205b..3e60a01 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; @@ -102,7 +170,7 @@ public: 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(); @@ -113,39 +181,46 @@ public: void SendThingInfo(Participant* remoteParticipant, Thing* thing); void PublishThingInfo(Thing* thing); - virtual bool Send(IMessage* msg) override; - bool Publish(IMessage* msg); + virtual bool Send(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(RemoteParticipantUDP* sender, ParticipantMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg); + virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg); - virtual void Process(Participant* sender, ThingMsg* msg); - virtual Thing* ProcessNewThing(Participant* sender, ThingMsg* msg, bool isRemote); + virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg); + virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender, + ThingMsg* msg, + bool isRemote); - 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, 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" diff --git a/Participants/SiteServer.cpp b/Participants/SiteServer.cpp index 085cd6f..9baa03e 100644 --- a/Participants/SiteServer.cpp +++ b/Participants/SiteServer.cpp @@ -11,11 +11,11 @@ namespace RoboidControl { #pragma region Init -SiteServer::SiteServer(int port) : ParticipantUDP(port) { +SiteServer::SiteServer(int port) : ParticipantUDPGeneric(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,9 +67,9 @@ 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) { +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); diff --git a/Participants/SiteServer.h b/Participants/SiteServer.h index 87029c5..97de7bf 100644 --- a/Participants/SiteServer.h +++ b/Participants/SiteServer.h @@ -11,7 +11,7 @@ namespace RoboidControl { /// @brief A participant is device which can communicate with other participants -class SiteServer : public ParticipantUDP { +class SiteServer : public ParticipantUDPGeneric { #pragma region Init @@ -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/PosixParticipant.cpp b/Posix/PosixParticipant.cpp index 9d44299..6ae6cef 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,7 +10,6 @@ #endif namespace RoboidControl { -namespace Posix { void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(__unix__) || defined(__APPLE__) @@ -90,7 +90,7 @@ void ParticipantUDP::Receive() { #endif } -bool ParticipantUDP::SendTo(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) // << "\n"; @@ -132,5 +132,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 fc20888..17dd23f 100644 --- a/Posix/PosixParticipant.h +++ b/Posix/PosixParticipant.h @@ -1,15 +1,15 @@ #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); void Receive(); - bool SendTo(Participant* remoteParticipant, int bufferSize); + bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: @@ -20,5 +20,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { #endif }; -} // namespace Posix } // namespace RoboidControl +#endif diff --git a/Thing.h b/Thing.h index f556712..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 diff --git a/Windows/WindowsParticipant.cpp b/Windows/WindowsParticipant.cpp index 5268787..dfaa445 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 { void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) { #if defined(_WIN32) || defined(_WIN64) @@ -102,7 +102,7 @@ void ParticipantUDP::Receive() { #endif // _WIN32 || _WIN64 } -bool ParticipantUDP::SendTo(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); @@ -142,5 +142,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 46cb629..3d241ba 100644 --- a/Windows/WindowsParticipant.h +++ b/Windows/WindowsParticipant.h @@ -1,15 +1,15 @@ #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: void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); - bool SendTo(Participant* remoteParticipant, int bufferSize); + bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); protected: @@ -18,5 +18,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP { #endif }; -} // namespace Windows } // namespace RoboidControl +#endif \ No newline at end of file From 8901e6933a48120460ffcb5a4e9240fe6b5ee432 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 27 Jun 2025 15:00:54 +0200 Subject: [PATCH 16/18] ESP-IDF sending now works --- EspIdf/EspIdfParticipant.cpp | 72 ++++++++++++++++++++------------- EspIdf/EspIdfParticipant.h | 18 +++++---- Participants/ParticipantUDP.cpp | 6 +-- 3 files changed, 56 insertions(+), 40 deletions(-) diff --git a/EspIdf/EspIdfParticipant.cpp b/EspIdf/EspIdfParticipant.cpp index 774f6a3..44dc0e8 100644 --- a/EspIdf/EspIdfParticipant.cpp +++ b/EspIdf/EspIdfParticipant.cpp @@ -10,7 +10,6 @@ namespace RoboidControl { void ParticipantUDP::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) { -#if defined(IDF_VER) std::cout << "Set up UDP\n"; GetBroadcastAddress(); @@ -56,23 +55,23 @@ void ParticipantUDP::SetupUDP(int localPort, // inet_pton(AF_INET, this->remoteSite->ipAddress, // &this->dest_addr.sin_addr.s_addr); + this->connected = true; + std::cout << "Wifi sync started local " << localPort << ", remote " << this->remoteSite->ipAddress << ":" << this->remoteSite->port << "\n"; - 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"; + // 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(); - -#endif // IDF_VER + //SendTest(); } void ParticipantUDP::GetBroadcastAddress() { @@ -151,20 +150,36 @@ ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {} 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; - ParticipantMsg* msg = new ParticipantMsg(this->networkId); - int bSize = msg->Serialize(this->buffer); - 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"; +// 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; -#endif - return true; +// 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, @@ -174,7 +189,7 @@ bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, char ip_str[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &dest_addr.sin_addr, ip_str, sizeof(ip_str)); - std::cout << "Sending to " << ip_str << ":" << port << "\n"; + std::cout << "Sending " << bufferSize << " bytes to " << ip_str << ":" << port << "\n"; // Print the IP address and port // printf("IP Address: %s\n", ip_str); @@ -183,10 +198,9 @@ bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, this->dest_addr.sin_port = htons(remoteParticipant->port); this->dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress); - std::cout << "socket: " << (int)this->sock << std::endl; int err = sendto(this->sock, buffer, bufferSize, 0, (struct sockaddr*)&this->dest_addr, sizeof(this->dest_addr)); - if (errno != 0) + if (errno < 0) std::cout << "Send error " << err << " or " << errno << "\n"; #endif diff --git a/EspIdf/EspIdfParticipant.h b/EspIdf/EspIdfParticipant.h index b465843..1c74ebf 100644 --- a/EspIdf/EspIdfParticipant.h +++ b/EspIdf/EspIdfParticipant.h @@ -9,7 +9,7 @@ namespace RoboidControl { class ParticipantUDP : public ParticipantUDPGeneric { public: - /// @brief Create a participant without connecting to a site + /// @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 @@ -19,19 +19,22 @@ class ParticipantUDP : public ParticipantUDPGeneric { /// @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); + ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681); void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Receive(); bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize); bool Publish(IMessage* msg); - bool SendTest(); + // bool SendTest(); - //bool Send(IMessage* msg) override; - void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) override; + /// @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: @@ -46,7 +49,6 @@ class ParticipantUDP : public ParticipantUDPGeneric { void GetBroadcastAddress(); }; - } // namespace RoboidControl #endif diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 2670107..1fdb695 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -231,18 +231,18 @@ void ParticipantUDPGeneric::Update(bool recurse) { if (this->connected == false) begin(); - // EspIdf::ParticipantUDP* thisEspIdf = - // static_cast(this); + ParticipantUDP* thisEspIdf = + static_cast(this); // thisEspIdf->SendTest(); if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { ParticipantMsg* msg = new ParticipantMsg(this->networkId); + // thisEspIdf->SendTest(); if (this->remoteSite == nullptr) this->Publish(msg); else this->Send(msg); - // thisEspIdf->SendTest(); delete msg; this->nextPublishMe = currentTimeMs + this->publishInterval; From f20ebd7ad2d869903d9ced2e733c10baf673c5d9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 27 Jun 2025 17:12:42 +0200 Subject: [PATCH 17/18] Switch to MPU6050, finally getting good data --- Participants/ParticipantUDP.cpp | 114 ++------------------------------ Participants/ParticipantUDP.h | 2 +- 2 files changed, 8 insertions(+), 108 deletions(-) diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index 1fdb695..e806493 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -118,32 +118,9 @@ RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) { } bool RemoteParticipantUDP::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->ipAddress << std::endl; - - // #if defined(_WIN32) || defined(_WIN64) - // Windows::ParticipantUDP* thisWindows = - // static_cast(this); - // return thisWindows->SendTo(this, bufferSize); - // #elif defined(__unix__) || defined(__APPLE__) - // Posix::ParticipantUDP* thisPosix = - // static_cast(this); return thisPosix->SendTo(this, - // bufferSize); - // #elif defined(ARDUINO) - // Arduino::ParticipantUDP* thisArduino = - // static_cast(this); - // return thisArduino->SendTo(this, bufferSize); - // #elif defined(IDF_VER) - // EspIdf::ParticipantUDP* thisEspIdf = - // static_cast(this); - // return thisEspIdf->SendTo(this, bufferSize); - // #else + // No message is actually sent, because this class has no networking + // implementation return false; - // #endif } #pragma region Init @@ -192,30 +169,6 @@ void ParticipantUDPGeneric::begin() { SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port); } -/* -void ParticipantUDPGeneric::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 @@ -231,14 +184,9 @@ void ParticipantUDPGeneric::Update(bool recurse) { if (this->connected == false) begin(); - ParticipantUDP* thisEspIdf = - static_cast(this); - // thisEspIdf->SendTest(); - if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { ParticipantMsg* msg = new ParticipantMsg(this->networkId); - // thisEspIdf->SendTest(); if (this->remoteSite == nullptr) this->Publish(msg); else @@ -248,22 +196,19 @@ void ParticipantUDPGeneric::Update(bool recurse) { this->nextPublishMe = currentTimeMs + this->publishInterval; } - // this->ReceiveUDP(); + this->ReceiveUDP(); } - // UpdateMyThings(); - // UpdateOtherThings(); + UpdateMyThings(); + UpdateOtherThings(); } void ParticipantUDPGeneric::UpdateMyThings() { - std::cout << "# things = " << this->things.size() << std::endl; for (Thing* thing : this->things) { - std::cout << ".\n"; if (thing == nullptr) // || thing->GetParent() != nullptr) continue; - std::cout << (int)this->root << std::endl; - std::cout << thing->name << std::endl; - // 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! @@ -356,56 +301,11 @@ void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) { delete customMsg; } -/* -bool ParticipantUDPGeneric::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 ParticipantUDPGeneric::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 ParticipantUDPGeneric::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) { diff --git a/Participants/ParticipantUDP.h b/Participants/ParticipantUDP.h index 3e60a01..159062e 100644 --- a/Participants/ParticipantUDP.h +++ b/Participants/ParticipantUDP.h @@ -181,7 +181,7 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP { void SendThingInfo(Participant* remoteParticipant, Thing* thing); void PublishThingInfo(Thing* thing); - virtual bool Send(IMessage* msg); + virtual bool Send(IMessage* msg) override; virtual bool Publish(IMessage* msg) = 0; #pragma endregion Send From 19a5c5d4d580759e0045ddc6ab724da773cfcecb Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 27 Jun 2025 17:45:52 +0200 Subject: [PATCH 18/18] Cleanup, but still crashes after about 20-30 secs. --- Participants/ParticipantUDP.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Participants/ParticipantUDP.cpp b/Participants/ParticipantUDP.cpp index e806493..66adb7c 100644 --- a/Participants/ParticipantUDP.cpp +++ b/Participants/ParticipantUDP.cpp @@ -196,7 +196,7 @@ void ParticipantUDPGeneric::Update(bool recurse) { this->nextPublishMe = currentTimeMs + this->publishInterval; } - this->ReceiveUDP(); + //this->ReceiveUDP(); } UpdateMyThings();