RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Merge branch '0.4_dev' into MQTT and fixed compatibility

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-29 11:42:46 +02:00
commit ff8570e2aa
43 changed files with 1159 additions and 523 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Arduino/ArduinoParticipant.cpp
View File

@ -1,4 +1,5 @@
#include "ArduinoParticipant.h" #include "ArduinoParticipant.h"
#if defined(ARDUINO)
#if !defined(NO_STD) #if !defined(NO_STD)
#include <iostream> #include <iostream>
@ -21,7 +22,6 @@
#endif #endif
namespace RoboidControl { namespace RoboidControl {
namespace Arduino {
#if defined(ARDUINO) && defined(HAS_WIFI) #if defined(ARDUINO) && defined(HAS_WIFI)
WiFiUDP* udp; WiFiUDP* udp;
@ -88,7 +88,7 @@ void ParticipantUDP::Receive() {
#endif #endif
} }
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
#if defined(ARDUINO) && defined(HAS_WIFI) #if defined(ARDUINO) && defined(HAS_WIFI)
// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":" // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
// << remoteParticipant->port << "\n"; // << remoteParticipant->port << "\n";
@ -131,5 +131,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
return true; return true;
}; };
} // namespace Arduino
} // namespace RoboidControl } // namespace RoboidControl
#endif

8
Arduino/ArduinoParticipant.h
View File

@ -1,15 +1,15 @@
#pragma once #pragma once
#if defined(ARDUINO)
#include "Participants/ParticipantUDP.h" #include "Participants/ParticipantUDP.h"
namespace RoboidControl { namespace RoboidControl {
namespace Arduino {
class ParticipantUDP : public RoboidControl::ParticipantUDP { class ParticipantUDP : public ParticipantUDPGeneric {
public: public:
void Setup(); void Setup();
void Receive(); void Receive();
bool Send(Participant* remoteParticipant, int bufferSize); bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg); bool Publish(IMessage* msg);
protected: protected:
@ -18,5 +18,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
void GetBroadcastAddress(); void GetBroadcastAddress();
}; };
} // namespace Arduino
} // namespace RoboidControl } // namespace RoboidControl
#endif

95
Arduino/Examples/BB2A/BB2A.cpp Normal file
View File

@ -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 <iostream>
using namespace RoboidControl;
using namespace RoboidControl::Arduino;
ParticipantUDPGeneric* localParticipant;
DifferentialDrive* bb2b;
TouchSensor* touchLeft;
TouchSensor* touchRight;
// RelativeEncoder* encoderLeft;
// RelativeEncoder* encoderRight;
void setup() {
Serial.begin(115200);
delay(3000);
Serial.println("started");
StartWifi("serrarens", "192.168.76.44");
localParticipant = new ParticipantUDPGeneric("192.168.77.76");
bb2b = new DifferentialDrive();
touchLeft = new TouchSensor(bb2b);
touchRight = new TouchSensor(bb2b);
// bb2b = new DRV8833::DifferentialDrive(driveConfig);
// touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b);
// touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b);
touchLeft->name = "Left Touch Sensor";
touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0));
touchRight->name = "Right Touch Sensor";
touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0));
// encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig);
// encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig);
// bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b);
// bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b);
}
void loop() {
// std::cout << touchLeft->touchedSomething << " | "
// << touchRight->touchedSomething << std::endl;
// std::cout << encoderLeft->rotationSpeed << " : "
// << encoderRight->rotationSpeed << std::endl;
// std::cout << bb2b->leftWheel->encoder->rotationSpeed
// << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl;
// The left wheel turns forward when nothing is touched on the right side
// and turn backward when the roboid hits something on the right
float leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f;
// The right wheel does the same, but instead is controlled by
// touches on the left side
float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity);
bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity);
// std::cout << " " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl;
// float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f;
// float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f;
// bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity);
// std::cout << " " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl;
// std::cout << leftMotor->actualVelocity << std::endl;
//Serial.println(".");
// Update the roboid state
localParticipant->Update();
// and sleep for 100ms
delay(10);
}

52
Arduino/Examples/BB2A/configuration.h Normal file
View File

@ -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

14
Arduino/Things/DRV8833.cpp
View File

@ -26,14 +26,15 @@ DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) {
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config, DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
Thing* parent) Thing* parent)
: RoboidControl::DifferentialDrive(this->drv8833.motorA, : RoboidControl::DifferentialDrive(nullptr, nullptr, parent) {
this->drv8833.motorB, this->drv8833 = new DRV8833(config, this);
parent), this->leftWheel = this->drv8833->motorA;
drv8833(config, this) {} this->rightWheel = this->drv8833->motorB;
}
void DRV8833::DifferentialDrive::Update(bool recurse) { void DRV8833::DifferentialDrive::Update(bool recurse) {
RoboidControl::DifferentialDrive::Update(recurse); RoboidControl::DifferentialDrive::Update(recurse);
this->drv8833.Update(false); this->drv8833->Update(false);
} }
#pragma endregion Differential drive #pragma endregion Differential drive
@ -48,8 +49,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
unsigned char pinIn1, unsigned char pinIn1,
unsigned char pinIn2, unsigned char pinIn2,
bool reverse) bool reverse)
: Motor() { : Motor(driver) {
this->SetParent(driver);
this->pinIn1 = pinIn1; this->pinIn1 = pinIn1;
this->pinIn2 = pinIn2; this->pinIn2 = pinIn2;

4
Arduino/Things/DRV8833.h
View File

@ -19,6 +19,8 @@ class DRV8833 : public Thing {
int BIn1; int BIn1;
int BIn2; int BIn2;
int standby = 255; 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 /// @brief Setup a DRV8833 motor controller
@ -43,7 +45,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
virtual void Update(bool recurse = false) override; virtual void Update(bool recurse = false) override;
protected: protected:
DRV8833 drv8833; DRV8833* drv8833 = nullptr;
}; };
#pragma endregion Differential drive #pragma endregion Differential drive

30
Arduino/Things/UltrasonicSensor.cpp
View File

@ -8,7 +8,7 @@ namespace Arduino {
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent) UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
: Thing(parent) { : Thing(parent) {
this->type = Type::DistanceSensor; this->type = Type::DistanceSensor;
this->name = "Ultrasonic sensor"; this->name = "Ultrasonic sensor";
this->pinTrigger = config.trigger; this->pinTrigger = config.trigger;
this->pinEcho = config.echo; this->pinEcho = config.echo;
@ -57,6 +57,28 @@ void UltrasonicSensor::Update(bool recursive) {
Thing::Update(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<float>::infinity();
#endif
}
#pragma endregion Distance sensor
#pragma region Touch sensor #pragma region Touch sensor
UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent) UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
@ -65,11 +87,11 @@ UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
void UltrasonicSensor::TouchSensor::Update(bool recursive) { void UltrasonicSensor::TouchSensor::Update(bool recursive) {
RoboidControl::TouchSensor::Update(recursive); RoboidControl::TouchSensor::Update(recursive);
this->ultrasonic.Update(false); this->ultrasonic.Update(false);
this->internalTouch |= (this->ultrasonic.distance > 0 && 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 Arduino
} // namespace RoboidControl } // namespace RoboidControl

20
Arduino/Things/UltrasonicSensor.h
View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include "Things/DistanceSensor.h"
#include "Things/TouchSensor.h" #include "Things/TouchSensor.h"
namespace RoboidControl { namespace RoboidControl {
@ -39,9 +40,26 @@ class UltrasonicSensor : Thing {
unsigned char pinEcho = 0; unsigned char pinEcho = 0;
public: public:
class DistanceSensor;
class TouchSensor; 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 #pragma region Touch sensor
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor { class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
@ -58,7 +76,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
UltrasonicSensor ultrasonic; UltrasonicSensor ultrasonic;
}; };
#pragma region Touch sensor #pragma endregion Touch sensor
} // namespace Arduino } // namespace Arduino
} // namespace RoboidControl } // namespace RoboidControl

43
CMakeLists.txt
View File

@ -22,42 +22,23 @@ else()
set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard
set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON)
project(RoboidControl)
add_subdirectory(LinearAlgebra)
add_subdirectory(Examples)
add_compile_definitions(GTEST)
include(FetchContent)
FetchContent_Declare(
googletest
DOWNLOAD_EXTRACT_TIMESTAMP ON
URL https://github.com/google/googletest/archive/refs/heads/main.zip
)
# For Windows: Prevent overriding the parent project's compiler/linker settings
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
FetchContent_MakeAvailable(googletest)
include_directories( include_directories(
. .
LinearAlgebra LinearAlgebra
) )
add_library(RoboidControl STATIC ${srcs}) add_library(RoboidControl STATIC ${srcs})
target_include_directories(RoboidControl PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)
project(RoboidControl)
add_subdirectory(LinearAlgebra)
# Examples
option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF)
add_subdirectory(examples)
enable_testing() 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() endif()

2
DoxyGen/Doxyfile
View File

@ -48,7 +48,7 @@ PROJECT_NAME = "Roboid Control for C++"
# could be handy for archiving the generated documentation or if some version # could be handy for archiving the generated documentation or if some version
# control system is used. # control system is used.
PROJECT_NUMBER = PROJECT_NUMBER = 0.4
# Using the PROJECT_BRIEF tag one can provide an optional one line description # 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 # for a project that appears at the top of each page and should give viewer a

123
EspIdf/EspIdfParticipant.cpp
View File

@ -2,15 +2,13 @@
#if defined(IDF_VER) #if defined(IDF_VER)
#include "esp_wifi.h" #include "esp_wifi.h"
#endif
#include <arpa/inet.h>
namespace RoboidControl { namespace RoboidControl {
namespace EspIdf {
void ParticipantUDP::Setup(int localPort, void ParticipantUDP::SetupUDP(int localPort,
const char* remoteIpAddress, const char* remoteIpAddress,
int remotePort) { int remotePort) {
#if defined(IDF_VER)
std::cout << "Set up UDP\n"; std::cout << "Set up UDP\n";
GetBroadcastAddress(); GetBroadcastAddress();
@ -22,42 +20,57 @@ void ParticipantUDP::Setup(int localPort,
} }
// Create a UDP socket // Create a UDP socket
this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (this->sockfd < 0) { if (this->sock < 0) {
std::cout << "Unable to create UDP socket: errno " << errno << "\n"; std::cout << "Unable to create UDP socket: errno " << errno << "\n";
vTaskDelete(NULL); vTaskDelete(NULL);
return; return;
} }
// Set up the server address structure /*
// Set up the receiving(local) address
struct sockaddr_in local_addr; struct sockaddr_in local_addr;
memset(&local_addr, 0, sizeof(local_addr)); memset(&local_addr, 0, sizeof(local_addr));
local_addr.sin_family = AF_INET; 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 = local_addr.sin_addr.s_addr =
htonl(INADDR_ANY); // Listen on all available network interfaces htonl(INADDR_ANY); // Listen on all available network interfaces
// Bind the socket to the address and port // Bind the socket to the receiving address
if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < if (bind(this->sock, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) {
0) {
std::cout << "Unable to bind UDP socket: errno " << errno << "\n"; std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
close(sockfd); close(sock);
vTaskDelete(NULL); vTaskDelete(NULL);
return; return;
} }
*/
// Initialize the dest_addr structure // Initialize the destination(remote) address
memset(&this->dest_addr, 0, memset(&this->dest_addr, 0,
sizeof(this->dest_addr)); // Clear the entire structure sizeof(this->dest_addr)); // Clear the entire structure
this->dest_addr.sin_family = AF_INET; this->dest_addr.sin_family = AF_INET;
this->dest_addr.sin_port = htons(this->remoteSite->port); this->dest_addr.sin_port = htons(this->remoteSite->port);
inet_pton(AF_INET, this->remoteSite->ipAddress, this->dest_addr.sin_addr.s_addr = inet_addr(this->remoteSite->ipAddress);
&this->dest_addr.sin_addr.s_addr); // inet_pton(AF_INET, this->remoteSite->ipAddress,
// &this->dest_addr.sin_addr.s_addr);
std::cout << "Wifi sync started local " << this->port << ", remote " this->connected = true;
std::cout << "Wifi sync started local " << localPort << ", remote "
<< this->remoteSite->ipAddress << ":" << this->remoteSite->port << this->remoteSite->ipAddress << ":" << this->remoteSite->port
<< "\n"; << "\n";
#endif // IDF_VER
// std::cout << "socket: " << (int)this->sock << std::endl;
// ParticipantMsg* msg = new ParticipantMsg(this->networkId);
// int bufferSize = msg->Serialize(this->buffer);
// int err = sendto(this->sock, buffer, bufferSize, 0,
// (struct sockaddr*)&dest_addr, sizeof(dest_addr));
// if (errno != 0)
// std::cout << "AASend error " << err << " or " << errno << "\n";
// else
// std::cout << "AASend SUCCESS\n";
//SendTest();
} }
void ParticipantUDP::GetBroadcastAddress() { void ParticipantUDP::GetBroadcastAddress() {
@ -83,10 +96,11 @@ void ParticipantUDP::GetBroadcastAddress() {
#endif // IDF_VER #endif // IDF_VER
} }
void ParticipantUDP::Receive() { void ParticipantUDP::ReceiveUDP() {
#if defined(IDF_VER) #if defined(IDF_VER)
/*
struct pollfd fds[1]; struct pollfd fds[1];
fds[0].fd = sockfd; fds[0].fd = sock;
fds[0].events = POLLIN; // We're looking for data available to read fds[0].events = POLLIN; // We're looking for data available to read
// Use poll() with a timeout of 0 to return immediately // Use poll() with a timeout of 0 to return immediately
@ -103,7 +117,7 @@ void ParticipantUDP::Receive() {
char sender_ipAddress[INET_ADDRSTRLEN]; char sender_ipAddress[INET_ADDRSTRLEN];
while (ret > 0 && fds[0].revents & POLLIN) { 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); (struct sockaddr*)&source_addr, &addr_len);
if (packetSize < 0) { if (packetSize < 0) {
std::cout << "recvfrom() error\n"; std::cout << "recvfrom() error\n";
@ -126,18 +140,66 @@ void ParticipantUDP::Receive() {
} }
} }
// std::cout << "no more messages\n"; // std::cout << "no more messages\n";
*/
#endif // IDF_VER #endif // IDF_VER
} }
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}
#if defined(IDF_VER)
// std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
// << remoteParticipant->port << "\n";
int err = sendto(this->sockfd, buffer, bufferSize, 0, ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
(struct sockaddr*)&dest_addr, sizeof(dest_addr)); : ParticipantUDPGeneric(ipAddress, port, localPort) {}
if (errno != 0)
// bool ParticipantUDP::SendTest() {
// #if defined(IDF_VER)
// // std::cout << "socket: " << (int)this->sock << std::endl;
// // UBaseType_t stack_size = uxTaskGetStackHighWaterMark(NULL); // NULL to check the main task
// // size_t free_heap = xPortGetFreeHeapSize();
// // std::cout << "Stack High Water Mark: " << stack_size << " heap " << free_heap << std::endl;
// ParticipantMsg* msg = new ParticipantMsg(this->networkId);
// int bSize = msg->Serialize(this->buffer);
// // std::cout << "buffer size " << bSize << std::endl;
// int err = sendto(this->sock, buffer, bSize, 0, (struct sockaddr*)&dest_addr,
// sizeof(dest_addr));
// if (errno != 0)
// std::cout << "BBSend error " << err << " or " << errno << "\n";
// else
// std::cout << "BBSend SUCCESS\n";
// #endif
// return true;
// }
bool ParticipantUDP::Send(IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
return true;
std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
<< " to " << this->remoteSite->ipAddress << std::endl;
return this->SendTo(this->remoteSite, bufferSize);
}
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
int bufferSize) {
#if defined(IDF_VER)
uint16_t port = ntohs(dest_addr.sin_port);
char ip_str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &dest_addr.sin_addr, ip_str, sizeof(ip_str));
std::cout << "Sending " << bufferSize << " bytes to " << ip_str << ":" << port << "\n";
// Print the IP address and port
// printf("IP Address: %s\n", ip_str);
// printf("Port: %d\n", port);
this->dest_addr.sin_port = htons(remoteParticipant->port);
this->dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);
int err = sendto(this->sock, buffer, bufferSize, 0,
(struct sockaddr*)&this->dest_addr, sizeof(this->dest_addr));
if (errno < 0)
std::cout << "Send error " << err << " or " << errno << "\n"; std::cout << "Send error " << err << " or " << errno << "\n";
#endif #endif
@ -154,7 +216,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
dest_addr.sin_family = AF_INET; dest_addr.sin_family = AF_INET;
dest_addr.sin_port = htons(this->port); dest_addr.sin_port = htons(this->port);
inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr); 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)); sizeof(dest_addr));
if (err != 0) if (err != 0)
std::cout << "Publish error\n"; std::cout << "Publish error\n";
@ -162,5 +224,6 @@ bool ParticipantUDP::Publish(IMessage* msg) {
return true; return true;
}; };
} // namespace EspIdf
} // namespace RoboidControl } // namespace RoboidControl
#endif

36
EspIdf/EspIdfParticipant.h
View File

@ -1,26 +1,47 @@
#pragma once #pragma once
#if defined(IDF_VER)
#include "Participants/ParticipantUDP.h" #include "Participants/ParticipantUDP.h"
#if defined(IDF_VER)
#include "lwip/sockets.h" #include "lwip/sockets.h"
#endif
namespace RoboidControl { namespace RoboidControl {
namespace EspIdf {
class ParticipantUDP : public RoboidControl::ParticipantUDP { class ParticipantUDP : public ParticipantUDPGeneric {
public: 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 Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive(); void Receive();
bool Send(Participant* remoteParticipant, int bufferSize); bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg); bool Publish(IMessage* msg);
// bool SendTest();
/// @brief Sens a message to the remote site (if set)
/// @param msg The message to send
/// @return True if a message could be sent.
bool Send(IMessage* msg) override;
void SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) override;
void ReceiveUDP() override;
protected: protected:
#if defined(IDF_VER) #if defined(IDF_VER)
char broadcastIpAddress[INET_ADDRSTRLEN]; char broadcastIpAddress[INET_ADDRSTRLEN];
int sockfd; int sock;
struct sockaddr_in dest_addr; struct sockaddr_in dest_addr;
// struct sockaddr_in src_addr; // struct sockaddr_in src_addr;
#endif #endif
@ -28,5 +49,6 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
void GetBroadcastAddress(); void GetBroadcastAddress();
}; };
} // namespace EspIdf
} // namespace RoboidControl } // namespace RoboidControl
#endif

4
Messages/PoseMsg.cpp
View File

@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
this->thingId = thing->id; this->thingId = thing->id;
this->poseType = 0; this->poseType = 0;
if (thing->positionUpdated || (force && thing->IsRoot())) { if (thing->positionUpdated || (force || thing->IsRoot())) {
this->position = thing->GetPosition(); this->position = thing->GetPosition();
this->poseType |= Pose_Position; this->poseType |= Pose_Position;
} }
if (thing->orientationUpdated || (force && thing->IsRoot())) { if (thing->orientationUpdated || (force || thing->IsRoot())) {
this->orientation = thing->GetOrientation(); this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation; this->poseType |= Pose_Orientation;
} }

204
Participant.cpp
View File

@ -10,8 +10,6 @@ namespace RoboidControl {
#pragma region Participant #pragma region Participant
ParticipantRegistry Participant::registry;
Participant* Participant::LocalParticipant = new Participant(); Participant* Participant::LocalParticipant = new Participant();
void Participant::ReplaceLocalParticipant(Participant& newParticipant) { void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
@ -24,6 +22,7 @@ Participant::Participant() {
//this->Add(this->root); //this->Add(this->root);
} }
/*
Participant::Participant(const char* ipAddress, int port) { Participant::Participant(const char* ipAddress, int port) {
Thing::CreateRoot(this); Thing::CreateRoot(this);
//this->Add(this->root); //this->Add(this->root);
@ -43,19 +42,26 @@ Participant::Participant(const char* ipAddress, int port) {
this->ipAddress = addressString; this->ipAddress = addressString;
this->port = port; this->port = port;
} }
*/
Participant::~Participant() { Participant::~Participant() {
// registry.Remove(this); // registry.Remove(this);
delete[] this->ipAddress; // delete[] this->ipAddress;
} }
void Participant::Update() { void Participant::Update(bool recurse) {
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
if (thing != nullptr) if (thing != nullptr)
thing->Update(true); thing->Update();
} }
} }
bool Participant::Send(IMessage* msg) {
std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff)
<< " to base Participant without communcation support " << std::endl;
return true;
}
/*
bool Participant::Send(IMessage* msg) { bool Participant::Send(IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer); int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0) if (bufferSize <= 0)
@ -67,31 +73,31 @@ bool Participant::Send(IMessage* msg) {
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows = Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this); static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Send(this, bufferSize); return thisWindows->SendTo(this, bufferSize);
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this); Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Send(this, bufferSize); return thisPosix->SendTo(this, bufferSize);
#elif defined(ARDUINO) #elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino = Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this); static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->Send(this, bufferSize); return thisArduino->SendTo(this, bufferSize);
#elif defined(IDF_VER) #elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf = EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this); static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->Send(remoteParticipant, bufferSize); return thisEspIdf->SendTo(this, bufferSize);
#else #else
return false; return false;
#endif #endif
} }
*/
Thing* Participant::Get(unsigned char thingId) { Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
for (Thing* thing : this->things) { for (Thing* thing : this->things) {
if (thing->id == thingId) if (thing->owner->networkId == networkId && thing->id == thingId)
return thing; return thing;
} }
// std::cout << "Could not find thing " << this->ipAddress << ":" << std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port
// this->port << "[" << (int)thingId << "]\n";
// << "[" << (int)thingId << "]\n";
return nullptr; return nullptr;
} }
@ -113,22 +119,24 @@ void Participant::Add(Thing* thing, bool checkId) {
thing->id = highestIx + 1; thing->id = highestIx + 1;
this->things.push_back(thing); this->things.push_back(thing);
#endif #endif
// std::cout << "Add thing with generated ID " << this->ipAddress << ":" std::cout << "Add thing with generated ID "
// << this->port << "[" << (int)thing->id << "]\n"; //<< this->ipAddress << ":" << this->port
<< "[" << (int)thing->id << "]\n";
} else { } else {
Thing* foundThing = Get(thing->id); Thing* foundThing = Get(thing->owner->networkId, thing->id);
if (foundThing == nullptr) { if (foundThing == nullptr) {
#if defined(NO_STD) #if defined(NO_STD)
this->things[this->thingCount++] = thing; this->things[this->thingCount++] = thing;
#else #else
this->things.push_back(thing); this->things.push_back(thing);
#endif #endif
// std::cout << "Add thing " << this->ipAddress << ":" << this->port << std::cout << "Add thing " << //this->ipAddress << ":" << this->port <<
// "[" "["
// << (int)thing->id << "]\n"; << (int)thing->id << "]\n";
} else { } else {
// std::cout << "Did not add, existing thing " << this->ipAddress << ":" std::cout << "Did not add, existing thing "
// << this->port << "[" << (int)thing->id << "]\n"; //<< this->ipAddress << ":" << this->port
<< "[" << (int)thing->id << "]\n";
} }
} }
} }
@ -156,86 +164,88 @@ void Participant::Remove(Thing* thing) {
#pragma endregion #pragma endregion
#pragma region ParticipantRegistry // #pragma region ParticipantRegistry
Participant* ParticipantRegistry::Get(const char* ipAddress, // /*
unsigned int port) { // Participant* ParticipantRegistry::Get(const char* ipAddress,
#if !defined(NO_STD) // unsigned int port) {
for (Participant* participant : ParticipantRegistry::participants) { // #if !defined(NO_STD)
if (participant == nullptr) // for (Participant* participant : ParticipantRegistry::participants) {
continue; // if (participant == nullptr)
if (strcmp(participant->ipAddress, ipAddress) == 0 && // continue;
participant->port == port) { // if (strcmp(participant->ipAddress, ipAddress) == 0 &&
// std::cout << "found participant " << participant->ipAddress << ":" // participant->port == port) {
// << (int)participant->port << std::endl; // // std::cout << "found participant " << participant->ipAddress << ":"
return participant; // // << (int)participant->port << std::endl;
} // return participant;
} // }
std::cout << "Could not find participant " << ipAddress << ":" << (int)port // }
<< std::endl; // std::cout << "Could not find participant " << ipAddress << ":" << (int)port
#endif // << std::endl;
return nullptr; // #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) { // // Participant* ParticipantRegistry::Add(const char* ipAddress,
#if !defined(NO_STD) // // unsigned int port) {
for (Participant* participant : ParticipantRegistry::participants) { // // Participant* participant = new Participant(ipAddress, port);
if (participant == nullptr) // // Add(participant);
continue; // // return participant;
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, // void ParticipantRegistry::Add(Participant* participant) {
unsigned int port) { // Participant* foundParticipant =
Participant* participant = new Participant(ipAddress, port); // Get(participant->networkId);
Add(participant); // //Get(participant->ipAddress, participant->port);
return participant;
}
void ParticipantRegistry::Add(Participant* participant) { // if (foundParticipant == nullptr) {
Participant* foundParticipant = // #if defined(NO_STD)
Get(participant->ipAddress, participant->port); // // 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) { // void ParticipantRegistry::Remove(Participant* participant) {
#if defined(NO_STD) // // participants.remove(participant);
// 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) { // #if defined(NO_STD)
// participants.remove(participant); // Participant** ParticipantRegistry::GetAll() const {
} // return ParticipantRegistry::participants;
// }
// #else
// const std::list<Participant*>& ParticipantRegistry::GetAll() const {
// return ParticipantRegistry::participants;
// }
// #endif
#if defined(NO_STD) // #pragma endregion ParticipantRegistry
Participant** ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#else
const std::list<Participant*>& ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#endif
#pragma endregion ParticipantRegistry
} // namespace RoboidControl } // namespace RoboidControl

29
Participant.h
View File

@ -7,6 +7,7 @@ namespace RoboidControl {
constexpr int MAX_THING_COUNT = 256; constexpr int MAX_THING_COUNT = 256;
/*
/// @brief class which manages all known participants /// @brief class which manages all known participants
class ParticipantRegistry { class ParticipantRegistry {
public: public:
@ -14,7 +15,7 @@ class ParticipantRegistry {
/// @param ipAddress The IP address of the participant /// @param ipAddress The IP address of the participant
/// @param port The port number of the participant /// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found /// @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 /// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant /// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found /// @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 ipAddress The IP address of the participant
/// @param port The port number of the participant /// @param port The port number of the participant
/// @return The added 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 /// @brief Add a participant
/// @param participant The participant to add /// @param participant The participant to add
void Add(Participant* participant); void Add(Participant* participant);
@ -52,6 +54,7 @@ class ParticipantRegistry {
std::list<Participant*> participants; std::list<Participant*> participants;
#endif #endif
}; };
*/
/// @brief A participant is a device which manages things. /// @brief A participant is a device which manages things.
/// It can communicate with other participant to synchronise the state of /// It can communicate with other participant to synchronise the state of
@ -88,15 +91,6 @@ class Participant {
/// @brief The name of the participant /// @brief The name of the participant
const char* name = "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 /// @brief The network Id to identify the participant
unsigned char networkId = 0; unsigned char networkId = 0;
@ -112,9 +106,10 @@ class Participant {
std::list<Thing*> things; std::list<Thing*> things;
#endif #endif
/// @brief Find a thing managed by this participant /// @brief Find a thing managed by this participant
/// @param networkId The network ID of the thing
/// @param thingId The ID of the thing /// @param thingId The ID of the thing
/// @return The thing if found, nullptr when no thing has been found /// @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. /// @brief Add a new thing for this participant.
/// @param thing The thing to add /// @param thing The thing to add
/// @param checkId If true, the thing.id is regenerated if it is zero /// @param checkId If true, the thing.id is regenerated if it is zero
@ -129,7 +124,7 @@ class Participant {
public: public:
/// @brief Update all things for this participant /// @brief Update all things for this participant
virtual void Update(); virtual void Update(bool recurse = true);
#pragma endregion Update #pragma endregion Update
@ -142,12 +137,12 @@ class Participant {
#pragma endregion Send #pragma endregion Send
#pragma region Participant Registry // #pragma region Participant Registry
public: // public:
static ParticipantRegistry registry; // static ParticipantRegistry registry;
#pragma endregion Participant Registry // #pragma endregion Participant Registry
}; };
} // namespace RoboidControl } // namespace RoboidControl

8
Participants/IsolatedParticipant.cpp
View File

@ -1,14 +1,16 @@
/*
#include "IsolatedParticipant.h" #include "IsolatedParticipant.h"
#include "ParticipantUDP.h" #include "ParticipantUDP.h"
namespace RoboidControl { namespace RoboidControl {
static ParticipantUDP* isolatedParticipant = nullptr; static ParticipantUDPGeneric* isolatedParticipant = nullptr;
Participant* IsolatedParticipant::Isolated() { Participant* IsolatedParticipant::Isolated() {
if (isolatedParticipant == nullptr) if (isolatedParticipant == nullptr)
isolatedParticipant = new ParticipantUDP(0); isolatedParticipant = new ParticipantUDPGeneric(0);
return isolatedParticipant; return isolatedParticipant;
} }
} // namespace RoboidControl } // namespace RoboidControl
*/

4
Participants/IsolatedParticipant.h
View File

@ -1,3 +1,4 @@
/*
#include "Participant.h" #include "Participant.h"
namespace RoboidControl { namespace RoboidControl {
@ -10,4 +11,5 @@ class IsolatedParticipant {
static Participant* Isolated(); static Participant* Isolated();
}; };
} }
*/

74
Participants/ParticipantMQTT.cpp
View File

@ -8,19 +8,15 @@
#include <unistd.h> #include <unistd.h>
#endif #endif
#include "Participants/ParticipantUDP.h"
namespace RoboidControl { namespace RoboidControl {
#if defined(_WIN32) || defined(_WIN64) MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port)
Windows::ParticipantUDP thisWindows = Windows::ParticipantUDP(); : RemoteParticipantUDP("127.0.0.1", port) {
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP thisWindows = Posix::ParticipantUDP();
#endif
ParticipantMQTT::ParticipantMQTT(const char* ipAddress, int port)
: Participant("127.0.0.1", port) {
this->name = "ParticipantUDP"; this->name = "ParticipantUDP";
this->remoteSite = new Participant(ipAddress, port); this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
Participant::registry.Add(this); // Participant::registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
this->root->owner = this; this->root->owner = this;
@ -30,22 +26,27 @@ ParticipantMQTT::ParticipantMQTT(const char* ipAddress, int port)
Participant::ReplaceLocalParticipant(*this); Participant::ReplaceLocalParticipant(*this);
std::cout << "Setup TCP connection" << std::endl; std::cout << "Setup TCP connection" << std::endl;
thisWindows.SetupTCP(ipAddress, port); // thisWindows.SetupTCP(ipAddress, port);
send_mqtt_connect("RoboidControl"); // send_mqtt_connect("RoboidControl");
sendSubscribe("domoticz/out"); // sendSubscribe("domoticz/out");
// #endif // #endif
} }
void MQTTParticipantBase::SetupTCP() {
this->connected = false;
}
#define MQTT_CONNECT 0x10 #define MQTT_CONNECT 0x10
#define MQTT_CONNECT_ACK 0x20 #define MQTT_CONNECT_ACK 0x20
void ParticipantMQTT::send_mqtt_connect(const char* client_id) { void MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
MQTTConnectPacket packet; MQTTConnectPacket packet;
packet.fixed_header = MQTT_CONNECT; packet.fixed_header = MQTT_CONNECT;
packet.remaining_length = //14; packet.remaining_length = // 14;
2 + 4 + 2+ strlen(client_id) + 3; // Protocol name + protocol level + connect 2 + 4 + 2 + strlen(client_id) +
// flags + keep alive + client ID 3; // Protocol name + protocol level + connect
// flags + keep alive + client ID
packet.protocol_name_length = 4; // "MQTT" packet.protocol_name_length = 4; // "MQTT"
packet.protocol_name = "MQTT"; packet.protocol_name = "MQTT";
packet.protocol_level = 4; // MQTT version 3.1.1 packet.protocol_level = 4; // MQTT version 3.1.1
@ -66,25 +67,25 @@ void ParticipantMQTT::send_mqtt_connect(const char* client_id) {
this->buffer[index++] = packet.connect_flags; this->buffer[index++] = packet.connect_flags;
this->buffer[index++] = packet.keep_alive & 0xFF; // LSB this->buffer[index++] = packet.keep_alive & 0xFF; // LSB
this->buffer[index++] = (packet.keep_alive >> 8) & 0xFF; // MSB this->buffer[index++] = (packet.keep_alive >> 8) & 0xFF; // MSB
// this->buffer[index++] = 0; // MSB: no client ID // this->buffer[index++] = 0; // MSB: no
// this->buffer[index++] = 0; // LSB: no client ID // client ID this->buffer[index++] = 0; //
size_t client_id_length = strlen(client_id); // LSB: no client ID
this->buffer[index++] = (client_id_length >> 8) & 0xFF; // MSB size_t client_id_length = strlen(client_id);
this->buffer[index++] = client_id_length & 0xFF; // LSB this->buffer[index++] = (client_id_length >> 8) & 0xFF; // MSB
memcpy(&this->buffer[index], client_id, client_id_length); this->buffer[index++] = client_id_length & 0xFF; // LSB
index += client_id_length; memcpy(&this->buffer[index], client_id, client_id_length);
index += client_id_length;
for (int ix = 0; ix < index; ix++) for (int ix = 0; ix < index; ix++)
std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n"; std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n";
// Send the MQTT connect packet // Send the MQTT connect packet
thisWindows.SendTCP(index); SendTCP(index);
std::cout << "Send connect, client ID = " << client_id << std::endl;
std::cout << "Send connect, client ID = " << client_id << std::endl;
} }
void ParticipantMQTT::sendSubscribe(const char* topic) { void MQTTParticipantBase::sendSubscribe(const char* topic) {
// Packet Identifier (2 bytes) // Packet Identifier (2 bytes)
static unsigned short packetId = static unsigned short packetId =
1; // Increment this for each new subscription 1; // Increment this for each new subscription
@ -112,27 +113,28 @@ void ParticipantMQTT::sendSubscribe(const char* topic) {
this->buffer[6 + topicLength] = 0x00; // QoS level (0) this->buffer[6 + topicLength] = 0x00; // QoS level (0)
int index = 7 + topicLength; int index = 7 + topicLength;
// for (int ix = 0; ix < index; ix++) // for (int ix = 0; ix < index; ix++)
// std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n"; // std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n";
// Send the SUBSCRIBE packet // Send the SUBSCRIBE packet
thisWindows.SendTCP(7 + topicLength); SendTCP(7 + topicLength);
std::cout << "Send subscribe to topic: " << topic << std::endl; std::cout << "Send subscribe to topic: " << topic << std::endl;
} }
void ParticipantMQTT::Update() { void MQTTParticipantBase::Update(bool recurse) {
int packetSize = ReceiveTCP(); int packetSize = ReceiveTCP();
if (packetSize > 0) { if (packetSize > 0) {
ReceiveData(packetSize); ReceiveData(packetSize);
} }
} }
void MQTTParticipantBase::SendTCP(int bufferSize) {}
int ParticipantMQTT::ReceiveTCP() { int MQTTParticipantBase::ReceiveTCP() {
return thisWindows.ReceiveTCP(); return false;
} }
void ParticipantMQTT::ReceiveData(unsigned char bufferSize) { void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) {
std::cout << " receivemsg\n"; std::cout << " receivemsg\n";
// std::cout << "receive msg " << (int)msgId << "\n"; // std::cout << "receive msg " << (int)msgId << "\n";
// std::cout << " buffer size = " <<(int) bufferSize << "\n"; // std::cout << " buffer size = " <<(int) bufferSize << "\n";

18
Participants/ParticipantMQTT.h
View File

@ -11,6 +11,7 @@
#include "Messages/TextMsg.h" #include "Messages/TextMsg.h"
#include "Messages/ThingMsg.h" #include "Messages/ThingMsg.h"
#include "Participant.h" #include "Participant.h"
#include "Participants/ParticipantUDP.h"
#if !defined(NO_STD) #if !defined(NO_STD)
#include <functional> #include <functional>
@ -18,15 +19,14 @@
// #include <unordered_map> // #include <unordered_map>
#endif #endif
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
#include "Windows/WindowsParticipant.h" #include "Windows/WindowsParticipant.h"
#elif defined(__unix__) || defined(__APPLE__) #elif defined(__unix__) || defined(__APPLE__)
#include "Posix/PosixParticipant.h"
#include <arpa/inet.h> #include <arpa/inet.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <sys/socket.h> #include <sys/socket.h>
#include <unistd.h> #include <unistd.h>
#include "Posix/PosixParticipant.h"
#endif #endif
namespace RoboidControl { namespace RoboidControl {
@ -43,14 +43,16 @@ namespace RoboidControl {
/// participant is created which can be obtained using /// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated(). /// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing() /// @sa RoboidControl::Thing::Thing()
class ParticipantMQTT : public Participant { class MQTTParticipantBase : public RemoteParticipantUDP {
#pragma region Init #pragma region Init
public: public:
/// @brief Create a participant which will try to connect to a MQTT server. /// @brief Create a participant which will try to connect to a MQTT server.
/// @param ipAddress The IP address of the MQTT server /// @param ipAddress The IP address of the MQTT server
/// @param port The port used by the MQTT server /// @param port The port used by the MQTT server
ParticipantMQTT(const char* ipAddress, int port = 1883); MQTTParticipantBase(const char* ipAddress, int port = 1883);
protected:
virtual void SetupTCP();
#pragma endregion Init #pragma endregion Init
@ -58,7 +60,7 @@ class ParticipantMQTT : public Participant {
public: public:
/// @brief The remote site when this participant is connected to a site /// @brief The remote site when this participant is connected to a site
Participant* remoteSite = nullptr; RemoteParticipantUDP* remoteSite = nullptr;
protected: protected:
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
@ -75,7 +77,7 @@ class ParticipantMQTT : public Participant {
#pragma region Update #pragma region Update
public: public:
virtual void Update() override; virtual void Update(bool recurse = true) override;
#pragma endregion Update #pragma endregion Update
@ -96,13 +98,15 @@ class ParticipantMQTT : public Participant {
void send_mqtt_connect(const char* client_id); void send_mqtt_connect(const char* client_id);
void sendSubscribe(const char* topic); void sendSubscribe(const char* topic);
virtual void SendTCP(int bufferSize);
#pragma endregion Send #pragma endregion Send
#pragma region Receive #pragma region Receive
protected: protected:
int ReceiveTCP(); int ReceiveTCP();
void ReceiveData(unsigned char bufferSize); void ReceiveData(unsigned char bufferSize);
// void ReceiveData(unsigned char bufferSize, Participant* // void ReceiveData(unsigned char bufferSize, Participant*
// remoteParticipant); // remoteParticipant);

364
Participants/ParticipantUDP.cpp
View File

@ -8,73 +8,167 @@
#include "Posix/PosixParticipant.h" #include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.h" #include "Windows/WindowsParticipant.h"
#include "Things/DifferentialDrive.h"
#include "Things/DistanceSensor.h"
#include "Things/TouchSensor.h"
#include <string.h> #include <string.h>
namespace RoboidControl { 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<RemoteParticipantUDP*>& ParticipantRegistry::GetAll() const {
return ParticipantRegistry::participants;
}
#endif
#pragma endregion ParticipantRegistry
RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) {
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)
strncpy_s(addressString, stringLength, ipAddress,
addressLength); // Leave space for null terminator
#else
strncpy(addressString, ipAddress, addressLength);
#endif
addressString[addressLength] = '\0';
this->ipAddress = addressString;
this->port = port;
}
bool RemoteParticipantUDP::Send(IMessage* msg) {
// No message is actually sent, because this class has no networking
// implementation
return false;
}
#pragma region Init #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->name = "ParticipantUDP";
this->remoteSite = nullptr; this->remoteSite = nullptr;
if (this->port == 0) if (this->port == 0)
this->isIsolated = true; this->isIsolated = true;
Participant::registry.Add(this); registry.Add(this);
this->root = Thing::LocalRoot(); //::LocalParticipant->root; this->root = Thing::LocalRoot(); //::LocalParticipant->root;
this->root->owner = this; this->root->owner = this;
this->root->name = "UDP Root"; this->root->name = "UDP Root";
std::cout << "P2 " << (long)this->root << std::endl;
this->Add(this->root); this->Add(this->root);
Participant::ReplaceLocalParticipant(*this); Participant::ReplaceLocalParticipant(*this);
} }
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort) ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
: Participant("127.0.0.1", localPort) { int port,
int localPort)
: RemoteParticipantUDP("127.0.0.1", localPort) {
this->name = "ParticipantUDP"; this->name = "ParticipantUDP";
if (this->port == 0) if (this->port == 0)
this->isIsolated = true; this->isIsolated = true;
else else
this->remoteSite = new Participant(ipAddress, port); this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
Participant::registry.Add(this); registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root; this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
this->root->owner = this; this->root->owner = this;
this->root->name = "UDP Root"; this->root->name = "UDP Root";
std::cout << "P1 " << (long)this->root << std::endl;
this->Add(this->root); this->Add(this->root);
Participant::ReplaceLocalParticipant(*this); Participant::ReplaceLocalParticipant(*this);
} }
void ParticipantUDP::begin() { void ParticipantUDPGeneric::begin() {
if (this->isIsolated || this->remoteSite == nullptr) if (this->isIsolated || this->remoteSite == nullptr)
return; return;
SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port); SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
} }
void ParticipantUDP::SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) {
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
thisWindows->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
thisPosix->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
thisArduino->Setup();
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
#endif
this->connected = true;
}
#pragma endregion Init #pragma endregion Init
#pragma region Update #pragma region Update
@ -83,106 +177,55 @@ void ParticipantUDP::SetupUDP(int localPort,
// 1. receive external messages // 1. receive external messages
// 2. update the state // 2. update the state
// 3. send out the updated messages // 3. send out the updated messages
void ParticipantUDP::Update() { void ParticipantUDPGeneric::Update(bool recurse) {
unsigned long currentTimeMs = Thing::GetTimeMs(); unsigned long currentTimeMs = Thing::GetTimeMs();
PrepMyThings();
if (this->isIsolated == false) { if (this->isIsolated == false) {
if (this->connected == false) if (this->connected == false)
begin(); begin();
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) { if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
ParticipantMsg* msg = new ParticipantMsg(this->networkId); ParticipantMsg* msg = new ParticipantMsg(this->networkId);
if (this->remoteSite == nullptr) if (this->remoteSite == nullptr)
this->Publish(msg); this->Publish(msg);
else else
this->Send(msg); this->Send(msg);
delete msg; delete msg;
this->nextPublishMe = currentTimeMs + this->publishInterval; this->nextPublishMe = currentTimeMs + this->publishInterval;
} }
this->ReceiveUDP(); //this->ReceiveUDP();
} }
UpdateMyThings(); UpdateMyThings();
UpdateOtherThings(); UpdateOtherThings();
} }
void ParticipantUDP::PrepMyThings() { void ParticipantUDPGeneric::UpdateMyThings() {
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) { for (Thing* thing : this->things) {
if (thing == nullptr) // || thing->GetParent() != nullptr) if (thing == nullptr) // || thing->GetParent() != nullptr)
continue; continue;
// std::cout << thing->name << "\n"; // Why don't we do recursive?
// 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, // Because when a thing creates a thing in the update,
// that new thing is not sent out (because of hierarchyChanged) // that new thing is not sent out (because of hierarchyChanged)
// before it is updated itself: it is immediatedly updated! // before it is updated itself: it is immediatedly updated!
thing->Update(false); 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) if (thing->terminate)
this->Remove(thing); this->Remove(thing);
// std::cout << "E\n";
} }
} }
void ParticipantUDP::UpdateOtherThings() { void ParticipantUDPGeneric::UpdateOtherThings() {
#if defined(NO_STD) #if defined(NO_STD)
Participant** participants = Participant::registry.GetAll(); Participant** participants = Participant::registry.GetAll();
for (int ix = 0; ix < Participant::registry.count; ix++) { for (int ix = 0; ix < Participant::registry.count; ix++) {
Participant* participant = participants[ix]; Participant* participant = participants[ix];
#else #else
for (Participant* participant : Participant::registry.GetAll()) { for (Participant* participant : registry.GetAll()) {
#endif #endif
if (participant == nullptr || participant == this) if (participant == nullptr || participant == this)
continue; continue;
@ -210,8 +253,8 @@ void ParticipantUDP::UpdateOtherThings() {
#pragma region Send #pragma region Send
void ParticipantUDP::SendThingInfo(Participant* remoteParticipant, void ParticipantUDPGeneric::SendThingInfo(Participant* remoteParticipant,
Thing* thing) { Thing* thing) {
// std::cout << "Send thing info [" << (int)thing->id << "] \n"; // std::cout << "Send thing info [" << (int)thing->id << "] \n";
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing); ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
remoteParticipant->Send(thingMsg); remoteParticipant->Send(thingMsg);
@ -230,14 +273,14 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
delete binaryMsg; delete binaryMsg;
} }
bool ParticipantUDP::Send(IMessage* msg) { bool ParticipantUDPGeneric::Send(IMessage* msg) {
if (this->remoteSite != nullptr) if (this->remoteSite != nullptr)
return this->remoteSite->Send(msg); return this->remoteSite->Send(msg);
return false; return true;
} }
void ParticipantUDP::PublishThingInfo(Thing* thing) { void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) {
// std::cout << "Publish thing info" << thing->networkId << "\n"; // std::cout << "Publish thing info" << thing->networkId << "\n";
// Strange, when publishing, the network id is irrelevant, because it is // Strange, when publishing, the network id is irrelevant, because it is
// connected to a specific site... // connected to a specific site...
@ -258,71 +301,31 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) {
delete customMsg; delete customMsg;
} }
bool ParticipantUDP::Publish(IMessage* msg) {
// std::cout << "publish msg\n";
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Publish(msg);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Publish(msg);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->Publish(msg);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->Publish(msg);
#else
return false;
#endif
}
// Send // Send
#pragma endregion #pragma endregion
#pragma region Receive #pragma region Receive
void ParticipantUDP::ReceiveUDP() { void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
#if defined(_WIN32) || defined(_WIN64) char* senderIpAddress,
Windows::ParticipantUDP* thisWindows = unsigned int senderPort) {
static_cast<Windows::ParticipantUDP*>(this);
thisWindows->Receive();
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
thisPosix->Receive();
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
thisArduino->Receive();
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
thisEspIdf->Receive();
#endif
}
void ParticipantUDP::ReceiveData(unsigned char packetSize,
char* senderIpAddress,
unsigned int senderPort) {
// std::cout << "Receive data from " << senderIpAddress << ":" << senderPort // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
// << std::endl; // << std::endl;
Participant* sender = this->registry.Get(senderIpAddress, senderPort); RemoteParticipantUDP* sender =
this->registry.Get(senderIpAddress, senderPort);
if (sender == nullptr) { if (sender == nullptr) {
sender = this->registry.Add(senderIpAddress, senderPort); sender = this->registry.Add(senderIpAddress, senderPort);
#if !defined(NO_STD) #if !defined(NO_STD)
std::cout << "New remote participant " << sender->ipAddress << ":" // std::cout << "New remote participant " << sender->ipAddress << ":"
<< sender->port << std::endl; // << sender->port << std::endl;
#endif #endif
} }
ReceiveData(packetSize, sender); ReceiveData(packetSize, sender);
} }
void ParticipantUDP::ReceiveData(unsigned char bufferSize, void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
Participant* sender) { RemoteParticipantUDP* sender) {
unsigned char msgId = this->buffer[0]; unsigned char msgId = this->buffer[0];
// std::cout << "receive msg " << (int)msgId << "\n"; // std::cout << "receive msg " << (int)msgId << "\n";
// std::cout << " buffer size = " <<(int) bufferSize << "\n"; // std::cout << " buffer size = " <<(int) bufferSize << "\n";
@ -395,14 +398,16 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
#endif #endif
} }
void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) { void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
ParticipantMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
<< "\n"; << "\n";
#endif #endif
} }
void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) { void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
NetworkIdMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
<< " -> " << (int)msg->networkId << "\n"; << " -> " << (int)msg->networkId << "\n";
@ -417,28 +422,68 @@ void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
} }
} }
void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) { void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
InvestigateMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n"; << "/" << (int)msg->thingId << "]\n";
#endif #endif
} }
void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) { void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
ThingMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "] " << (int)msg->thingType << " " << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
<< (int)msg->parentId << "\n"; << (int)msg->parentId << "\n";
#endif #endif
RemoteParticipantUDP* owner = registry.Get(msg->networkId);
if (owner == nullptr) {
owner = new RemoteParticipantUDP(sender->ipAddress, sender->port);
owner->networkId = msg->networkId;
registry.Add(owner);
}
Thing* thing = owner->Get(msg->networkId, msg->thingId);
if (thing == nullptr) {
bool isRemote = (sender->networkId != owner->networkId);
thing = ProcessNewThing(owner, msg, isRemote);
thing->id = msg->thingId;
thing->type = msg->thingType;
thing->isRemote = isRemote;
}
if (msg->parentId != 0) {
thing->SetParent(owner->Get(msg->networkId, msg->parentId));
if (thing->GetParent() == nullptr)
std::cout << "Could not find parent" << std::endl;
} else
thing->SetParent(nullptr);
} }
void ParticipantUDP::Process(Participant* sender, NameMsg* msg) { Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
ThingMsg* msg,
bool isRemote) {
switch (msg->thingType) {
case Thing::Type::DistanceSensor:
return new DistanceSensor(owner->root);
case Thing::Type::TouchSensor:
return new TouchSensor(owner->root);
case Thing::Type::DifferentialDrive:
return new DifferentialDrive(owner->root);
default:
return new Thing(owner->root);
}
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
NameMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/" std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "] "; << (int)msg->thingId << "] ";
#endif #endif
Thing* thing = sender->Get(msg->thingId); Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing != nullptr) { if (thing != nullptr) {
int nameLength = msg->nameLength; int nameLength = msg->nameLength;
int stringLen = nameLength + 1; int stringLen = nameLength + 1;
@ -464,23 +509,25 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
#endif #endif
} }
void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) { void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
ModelUrlMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n"; << "/" << (int)msg->thingId << "]\n";
#endif #endif
} }
void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) { void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
PoseMsg* msg) {
#if !defined(DEBUG) && !defined(NO_STD) #if !defined(DEBUG) && !defined(NO_STD)
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
<< "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n"; << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
#endif #endif
Participant* owner = Participant::registry.Get(msg->networkId); Participant* owner = registry.Get(msg->networkId);
if (owner == nullptr) if (owner == nullptr)
return; return;
Thing* thing = owner->Get(msg->thingId); Thing* thing = owner->Get(msg->networkId, msg->thingId);
if (thing == nullptr) if (thing == nullptr)
return; return;
@ -494,15 +541,16 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
thing->SetAngularVelocity(msg->angularVelocity); thing->SetAngularVelocity(msg->angularVelocity);
} }
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) { void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
BinaryMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n"; << "/" << (int)msg->thingId << "]\n";
#endif #endif
Participant* owner = Participant::registry.Get(msg->networkId); Participant* owner = registry.Get(msg->networkId);
if (owner != nullptr) { if (owner != nullptr) {
Thing* thing = owner->Get(msg->thingId); Thing* thing = owner->Get(msg->networkId, msg->thingId);
if (thing != nullptr) if (thing != nullptr)
thing->ProcessBinary(msg->data); thing->ProcessBinary(msg->data);
#if !defined(NO_STD) #if !defined(NO_STD)
@ -516,20 +564,22 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
} }
} }
void ParticipantUDP::Process(Participant* sender, TextMsg* msg) { void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
TextMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " " std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
<< (int)msg->text << "\n"; << (int)msg->text << "\n";
#endif #endif
} }
void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) { void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
DestroyMsg* msg) {
#if defined(DEBUG) #if defined(DEBUG)
std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/" std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "]\n"; << (int)msg->thingId << "]\n";
#endif #endif
Thing* thing = sender->Get(msg->thingId); Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing != nullptr) if (thing != nullptr)
this->Remove(thing); this->Remove(thing);
} }

131
Participants/ParticipantUDP.h
View File

@ -5,11 +5,11 @@
#include "Messages/InvestigateMsg.h" #include "Messages/InvestigateMsg.h"
#include "Messages/ModelUrlMsg.h" #include "Messages/ModelUrlMsg.h"
#include "Messages/NameMsg.h" #include "Messages/NameMsg.h"
#include "Messages/NetworkIdMsg.h"
#include "Messages/ParticipantMsg.h" #include "Messages/ParticipantMsg.h"
#include "Messages/PoseMsg.h" #include "Messages/PoseMsg.h"
#include "Messages/NetworkIdMsg.h"
#include "Messages/ThingMsg.h"
#include "Messages/TextMsg.h" #include "Messages/TextMsg.h"
#include "Messages/ThingMsg.h"
#include "Participant.h" #include "Participant.h"
#if !defined(NO_STD) #if !defined(NO_STD)
@ -31,6 +31,74 @@ namespace RoboidControl {
constexpr int MAX_SENDER_COUNT = 256; 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<RemoteParticipantUDP*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<RemoteParticipantUDP*> participants;
#endif
};
/// @brief A participant using UDP communication /// @brief A participant using UDP communication
/// A local participant is the local device which can communicate with /// A local participant is the local device which can communicate with
/// other participants It manages all local things and communcation with other /// 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 /// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated(). /// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing() /// @sa RoboidControl::Thing::Thing()
class ParticipantUDP : public Participant { class ParticipantUDPGeneric : public RemoteParticipantUDP {
#pragma region Init #pragma region Init
public: public:
@ -53,31 +120,32 @@ class ParticipantUDP : public Participant {
/// These participant typically broadcast Participant messages to let site /// These participant typically broadcast Participant messages to let site
/// servers on the local network know their presence. Alternatively they can /// servers on the local network know their presence. Alternatively they can
/// broadcast information which can be used directly by other participants. /// 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. /// @brief Create a participant which will try to connect to a site.
/// @param ipAddress The IP address of the site /// @param ipAddress The IP address of the site
/// @param port The port used by the site /// @param port The port used by the site
/// @param localPort The port used by the local participant /// @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 endregion Init
#pragma region Properties #pragma region Properties
public: public:
/// @brief True if the participant is running isolated. /// @brief True if the participant is running isolated.
/// Isolated participants do not communicate with other participants /// Isolated participants do not communicate with other participants
bool isIsolated = false; bool isIsolated = false;
/// @brief The remote site when this participant is connected to a site /// @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 /// The interval in milliseconds for publishing (broadcasting) data on the
/// local network /// local network
long publishInterval = 3000; // 3 seconds long publishInterval = 3000; // 3 seconds
protected: protected:
#if !defined(ARDUINO) #if !defined(ARDUINO)
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
int sock; int sock;
@ -96,13 +164,13 @@ public:
#pragma region Update #pragma region Update
public: public:
virtual void Update() override; virtual void Update(bool recurse = true) override;
protected: protected:
unsigned long nextPublishMe = 0; unsigned long nextPublishMe = 0;
/// @brief Prepare the local things for the next update /// @brief Prepare the local things for the next update
virtual void PrepMyThings(); // virtual void PrepMyThings();
virtual void UpdateMyThings(); virtual void UpdateMyThings();
virtual void UpdateOtherThings(); virtual void UpdateOtherThings();
@ -114,35 +182,46 @@ public:
void PublishThingInfo(Thing* thing); void PublishThingInfo(Thing* thing);
virtual bool Send(IMessage* msg) override; virtual bool Send(IMessage* msg) override;
bool Publish(IMessage* msg); virtual bool Publish(IMessage* msg) = 0;
#pragma endregion Send #pragma endregion Send
#pragma region Receive #pragma region Receive
protected: protected:
void ReceiveData(unsigned char bufferSize, void ReceiveData(unsigned char bufferSize,
char* senderIpAddress, char* senderIpAddress,
unsigned int senderPort); 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(RemoteParticipantUDP* sender, ParticipantMsg* msg);
virtual void Process(Participant* sender, NetworkIdMsg* msg); virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg);
virtual void Process(Participant* sender, InvestigateMsg* msg); virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg);
virtual void Process(Participant* sender, ThingMsg* msg);
virtual void Process(Participant* sender, NameMsg* msg); virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg);
virtual void Process(Participant* sender, ModelUrlMsg* msg); virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender,
virtual void Process(Participant* sender, PoseMsg* msg); ThingMsg* msg,
virtual void Process(Participant* sender, BinaryMsg* msg); bool isRemote);
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 #pragma endregion Receive
public:
static ParticipantRegistry registry;
}; };
} // namespace RoboidControl } // namespace RoboidControl
#include "EspIdf/EspIdfParticipant.h"
#include "Posix/PosixParticipant.h"

18
Participants/SiteServer.cpp
View File

@ -15,7 +15,7 @@ SiteServer::SiteServer(int port) : ParticipantUDP(port) {
this->name = "Site Server"; this->name = "Site Server";
this->publishInterval = 0; this->publishInterval = 0;
SetupUDP(port, ipAddress, 0); //SetupUDP(port, ipAddress, 0);
} }
#pragma endregion Init #pragma endregion Init
@ -36,7 +36,7 @@ void SiteServer::UpdateMyThings() {
for (int ix = 0; ix < Participant::registry.count; ix++) { for (int ix = 0; ix < Participant::registry.count; ix++) {
Participant* participant = participants[ix]; Participant* participant = participants[ix];
#else #else
for (Participant* participant : Participant::registry.GetAll()) { for (Participant* participant : registry.GetAll()) {
#endif #endif
if (participant == nullptr || participant == this) if (participant == nullptr || participant == this)
continue; continue;
@ -56,7 +56,7 @@ void SiteServer::UpdateMyThings() {
#pragma region Receive #pragma region Receive
void SiteServer::Process(Participant* sender, ParticipantMsg* msg) { void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
if (msg->networkId != sender->networkId) { if (msg->networkId != sender->networkId) {
// std::cout << this->name << " received New Client -> " << // std::cout << this->name << " received New Client -> " <<
// sender->ipAddress // sender->ipAddress
@ -67,19 +67,19 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
} }
} }
void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {} void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(Participant* sender, ThingMsg* msg) { void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->thingId); Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing == nullptr) if (thing == nullptr) {
// new Thing(sender, (Thing::Type)msg->thingType, msg->thingId); // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
// Thing::Reconstruct(sender, msg->thingType, msg->thingId); // Thing::Reconstruct(sender, msg->thingType, msg->thingId);
//thing = new Thing(msg->thingType, sender->root); //thing = new Thing(msg->thingType, sender->root);
; }
thing->id = msg->thingId; thing->id = msg->thingId;
if (msg->parentId != 0) { if (msg->parentId != 0) {
thing->SetParent(Get(msg->parentId)); thing->SetParent(Get(msg->networkId, msg->parentId));
if (thing->IsRoot()) if (thing->IsRoot())
// if (thing->GetParent() != nullptr) // if (thing->GetParent() != nullptr)
#if defined(NO_STD) #if defined(NO_STD)

6
Participants/SiteServer.h
View File

@ -33,9 +33,9 @@ class SiteServer : public ParticipantUDP {
protected: protected:
unsigned long nextPublishMe = 0; unsigned long nextPublishMe = 0;
virtual void Process(Participant* sender, ParticipantMsg* msg) override; virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) override;
virtual void Process(Participant* sender, NetworkIdMsg* msg) override; virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override;
virtual void Process(Participant* sender, ThingMsg* msg) override; virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override;
#pragma endregion Receive #pragma endregion Receive

2
Posix/PosixMQTT.cpp Normal file
View File

@ -0,0 +1,2 @@
#include "PosixMQTT.h"

45
Posix/PosixParticipant.cpp
View File

@ -1,4 +1,5 @@
#include "PosixParticipant.h" #include "PosixParticipant.h"
#if defined(__unix__) || defined(__APPLE__)
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h> #include <arpa/inet.h>
@ -9,11 +10,15 @@
#endif #endif
namespace RoboidControl { namespace RoboidControl {
namespace Posix {
void ParticipantUDP::Setup(int localPort, ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}
const char* remoteIpAddress,
int remotePort) { ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
: ParticipantUDPGeneric(ipAddress, port, localPort) {}
void ParticipantUDP::SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) {
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
// Create a UDP socket // Create a UDP socket
@ -92,7 +97,7 @@ void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) {
#endif #endif
} }
void ParticipantUDP::Receive() { void ParticipantUDP::ReceiveUDP() {
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
sockaddr_in client_addr; sockaddr_in client_addr;
socklen_t len = sizeof(client_addr); socklen_t len = sizeof(client_addr);
@ -147,7 +152,8 @@ int ParticipantUDP::ReceiveTCP() {
return 0; return 0;
} }
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
int bufferSize) {
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
// std::cout << "Send to " << remoteParticipant->ipAddress << ":" << // std::cout << "Send to " << remoteParticipant->ipAddress << ":" <<
// ntohs(remoteParticipant->port) // ntohs(remoteParticipant->port)
@ -173,18 +179,18 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
return true; return true;
} }
bool ParticipantUDP::SendTCP(int bufferSize) { // bool ParticipantUDP::SendTCP(int bufferSize) {
#if defined(__unix__) || defined(__APPLE__) // #if defined(__unix__) || defined(__APPLE__)
// send(sock, this->buffer, bufferSize, 0); // // send(sock, this->buffer, bufferSize, 0);
ssize_t sent_bytes = send(this->sock, this->buffer, bufferSize, 0); // ssize_t sent_bytes = send(this->sock, this->buffer, bufferSize, 0);
if (sent_bytes < 0) { // if (sent_bytes < 0) {
std::cerr << "Failed to send packet" << strerror(errno) << std::endl; // std::cerr << "Failed to send packet" << strerror(errno) << std::endl;
close(sock); // close(sock);
return -1; // return -1;
} // }
#endif // #endif
return false; // return false;
} // }
bool ParticipantUDP::Publish(IMessage* msg) { bool ParticipantUDP::Publish(IMessage* msg) {
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
@ -208,5 +214,6 @@ bool ParticipantUDP::Publish(IMessage* msg) {
return true; return true;
} }
} // namespace Posix
} // namespace RoboidControl } // namespace RoboidControl
#endif

32
Posix/PosixParticipant.h
View File

@ -1,20 +1,36 @@
#pragma once #pragma once
#if defined(__unix__) || defined(__APPLE__)
#include "Participants/ParticipantUDP.h" #include "Participants/ParticipantUDP.h"
namespace RoboidControl { namespace RoboidControl {
namespace Posix {
class ParticipantUDP : public RoboidControl::ParticipantUDP { class ParticipantUDP : public ParticipantUDPGeneric {
public: public:
void Setup(int localPort, const char* remoteIpAddress, int remotePort); /// @brief Create a participant without connecting to a site
/// @param port The port on which the participant communicates
/// These participant typically broadcast Participant messages to let site
/// servers on the local network know their presence. Alternatively they can
/// broadcast information which can be used directly by other participants.
ParticipantUDP(int port = 7681);
/// @brief Create a participant which will try to connect to a site.
/// @param ipAddress The IP address of the site
/// @param port The port used by the site
/// @param localPort The port used by the local participant
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
void SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) override;
void ReceiveUDP() override;
//void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void SetupTCP(const char* remoteIpAddress, int remotePort); void SetupTCP(const char* remoteIpAddress, int remotePort);
void Receive(); // void Receive();
int ReceiveTCP(); int ReceiveTCP();
bool Send(Participant* remoteParticipant, int bufferSize); bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool SendTCP(int bufferSize); bool Publish(IMessage* msg) override;
bool Publish(IMessage* msg);
protected: protected:
#if defined(__unix__) || defined(__APPLE__) #if defined(__unix__) || defined(__APPLE__)
@ -24,5 +40,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
#endif #endif
}; };
} // namespace Posix
} // namespace RoboidControl } // namespace RoboidControl
#endif

47
Posix/PoxisMQTT.cpp Normal file
View File

@ -0,0 +1,47 @@
#include "PoxisMQTT.h"
#if defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <fcntl.h> // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
namespace RoboidControl {
RoboidControl::MQTTParticipant::MQTTParticipant(const char* remoteIpAddress,
int port)
: MQTTParticipantBase(remoteIpAddress, port) {}
void MQTTParticipant::SetupTCP() {
// Create a TCP socket
this->sock = socket(AF_INET, SOCK_STREAM, 0);
if (this->sock < 0) {
std::cerr << "Error creating socket" << std::endl;
return;
}
this->remote_addr.sin_family = AF_INET;
this->remote_addr.sin_port = htons(this->remoteSite->port);
inet_pton(AF_INET, this->remoteSite->ipAddress, &remote_addr.sin_addr);
int connect_result =
connect(this->sock, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
if (connect_result < 0) { //} && errno != EINPROGRESS) {
std::cerr << "Error connecting to server" << std::endl;
close(this->sock);
}
std::cout << "TCP connection to " << this->remoteSite->ipAddress << ":" << this->remoteSite->port
<< "\n";
// Set the socket to non-blocking mode
int flags = fcntl(this->sock, F_GETFL, 0);
fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
}
void MQTTParticipant::SendTCP(int bufferSize) {
send(this->sock, this->buffer, bufferSize, 0);
}
} // namespace RoboidControl
#endif

23
Posix/PoxisMQTT.h Normal file
View File

@ -0,0 +1,23 @@
#pragma once
#if defined(__unix__) || defined(__APPLE__)
#include "Participants/ParticipantMQTT.h"
namespace RoboidControl {
class MQTTParticipant : public MQTTParticipantBase {
public:
MQTTParticipant(const char* ipAddress, int port = 1883);
protected:
void SetupTCP() override;
void SendTCP(int bufferSize) override;
#if defined(__unix__) || defined(__APPLE__)
sockaddr_in remote_addr;
#endif
};
}
#endif

58
README.md
View File

@ -14,4 +14,60 @@ Supporting:
# Basic components # Basic components
- RoboidControl::Thing - RoboidControl::Thing
- RoboidControl::Participant - RoboidControl::Participant
# Installation
## Core code
The repository uses cmake for building. You can place it in a subfolder of your project and include it in you `CMakeLists.txt`.
For example if the library is placed in the subfolder `roboidcontrol`:
```
# Add the path to Roboid Control
add_subdirectory(roboidcontrol)
# Your source files/executable
add_executable(my_executable main.cpp)
# Link against RoboidControl
target_link_libraries(my_executable RoboidControl)
```
## Arduino (PlatformIO)
Arduino is only supported in combination with PlatformIO. The Arduino IDE is not (yet?) supported.
The best way to include support for Roboid Control in PlatformIO is
to clone the Roboid Control for C++ repository into a subfolder of the /lib folder.
Alternatively you can download the zip file and unpack it as a subfolder of the /lib folder.
## ESP-IDF
The best way to include support for Roboid Control in PlatformIO is
to clone the Roboid Control for C++ repository into a subfolder of the /components folder.
Alternatively you can download the zip file and unpack it as a subfolder of the /components folder.
Make sure you have included RoboidControl as a component in your top-level CMakeLists.txt, for example:
```
list(APPEND EXTRA_COMPONENT_DIRS
components/RoboidControl
)
```
# Get Started
## Core C++ Examples
This repository contains examples in the `examples` folder. You can build these using cmake.
For example, to build the BB2A example:
```
cmake -B build -D BUILD_EXAMPLE_BB2A=ON
cmake --build build
```
The resulting executable is then `build/examples/Debug/BB2A.exe`
## Arduino (PlatformIO) Examples
Specific examples for the Arduino platform are found in the `Arduino\examples` folder.
To use them you should create a new project in PlatformIO and then copy the example code to your project.

3
Testing/Temporary/LastTest.log Normal file
View File

@ -0,0 +1,3 @@
Start testing: Jun 17 17:17 W. Europe Summer Time
----------------------------------------------------------
End testing: Jun 17 17:17 W. Europe Summer Time

10
Thing.cpp
View File

@ -42,7 +42,7 @@ Thing::Thing(Participant* owner) {
this->owner = owner; this->owner = owner;
this->owner->Add(this); 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) { void Thing::CreateRoot(Participant* owner) {
@ -253,12 +253,6 @@ unsigned long Thing::GetTimeMs() {
#endif #endif
} }
// void Thing::Update(bool recursive) {
// Update(GetTimeMs(), recursive);
// }
void Thing::PrepareForUpdate() {}
void Thing::Update(bool recursive) { void Thing::Update(bool recursive) {
this->positionUpdated = false; this->positionUpdated = false;
this->orientationUpdated = false; this->orientationUpdated = false;
@ -268,7 +262,7 @@ void Thing::Update(bool recursive) {
this->nameChanged = false; this->nameChanged = false;
if (recursive) { 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++) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx]; Thing* child = this->children[childIx];
if (child == nullptr) if (child == nullptr)

9
Thing.h
View File

@ -10,7 +10,7 @@
namespace RoboidControl { namespace RoboidControl {
class Participant; class Participant;
class ParticipantUDP; class ParticipantUDPGeneric;
#define THING_STORE_SIZE 256 #define THING_STORE_SIZE 256
// IMPORTANT: values higher than 256 will need to change the Thing::id type // IMPORTANT: values higher than 256 will need to change the Thing::id type
@ -88,6 +88,11 @@ class Thing {
/// This can be either a Thing::Type of a byte value for custom types /// This can be either a Thing::Type of a byte value for custom types
unsigned char type = Type::Undetermined; 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 /// @brief The participant owning this thing
Participant* owner = nullptr; Participant* owner = nullptr;
@ -223,7 +228,7 @@ class Thing {
#pragma region Update #pragma region Update
public: public:
virtual void PrepareForUpdate(); //virtual void PrepareForUpdate();
/// @brief Updates the state of the thing /// @brief Updates the state of the thing
/// @param recurse When true, this will Update the descendants recursively /// @param recurse When true, this will Update the descendants recursively

29
Things/DistanceSensor.cpp Normal file
View File

@ -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

41
Things/DistanceSensor.h Normal file
View File

@ -0,0 +1,41 @@
#pragma once
#if !NO_STD
#include <limits>
#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<double>::infinity();
float externalDistance = std::numeric_limits<double>::infinity();
#endif
};
} // namespace RoboidControl

6
Things/TouchSensor.cpp
View File

@ -11,9 +11,9 @@ bool TouchSensor::IsTouching() {
return this->internalTouch || this->externalTouch; return this->internalTouch || this->externalTouch;
} }
void TouchSensor::PrepareForUpdate() { // void TouchSensor::PrepareForUpdate() {
//this->internalTouch = this->externalTouch; // //this->internalTouch = this->externalTouch;
} // }
void TouchSensor::Update(bool recursive) { void TouchSensor::Update(bool recursive) {
Thing::Update(recursive); Thing::Update(recursive);

4
Things/TouchSensor.h
View File

@ -6,7 +6,7 @@ namespace RoboidControl {
/// @brief A sensor which can detect touches /// @brief A sensor which can detect touches
class TouchSensor : public Thing { 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: public:
/// @brief Create a new child touch sensor /// @brief Create a new child touch sensor
/// @param parent The parent thing /// @param parent The parent thing
@ -18,7 +18,7 @@ class TouchSensor : public Thing {
/// otherwise /// otherwise
bool IsTouching(); bool IsTouching();
virtual void PrepareForUpdate() override; //virtual void PrepareForUpdate() override;
virtual void Update(bool recursive) override; virtual void Update(bool recursive) override;
/// @brief Function used to generate binary data for this touch sensor /// @brief Function used to generate binary data for this touch sensor

7
Windows/WindowsParticipant.cpp
View File

@ -1,4 +1,5 @@
#include "WindowsParticipant.h" #include "WindowsParticipant.h"
#if defined(_WIN32) || defined(_WIN64)
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h> #include <winsock2.h>
@ -7,7 +8,6 @@
#endif #endif
namespace RoboidControl { namespace RoboidControl {
namespace Windows {
ParticipantUDP::ParticipantUDP() {} ParticipantUDP::ParticipantUDP() {}
@ -195,7 +195,7 @@ int ParticipantUDP::ReceiveTCP() {
return 0; return 0;
} }
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) { bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
#if defined(_WIN32) || defined(_WIN64) #if defined(_WIN32) || defined(_WIN64)
char ip_str[INET_ADDRSTRLEN]; char ip_str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN); inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@ -247,5 +247,6 @@ bool ParticipantUDP::Publish(IMessage* msg) {
return true; return true;
} }
} // namespace Windows
} // namespace RoboidControl } // namespace RoboidControl
#endif

8
Windows/WindowsParticipant.h
View File

@ -1,16 +1,16 @@
#pragma once #pragma once
#if defined(_WIN32) || defined(_WIN64)
#include "Participants/ParticipantUDP.h" #include "Participants/ParticipantUDP.h"
namespace RoboidControl { namespace RoboidControl {
namespace Windows {
class ParticipantUDP : public RoboidControl::ParticipantUDP { class ParticipantUDP : public ParticipantUDPGeneric {
public: public:
ParticipantUDP(); ParticipantUDP();
void Setup(int localPort, const char* remoteIpAddress, int remotePort); void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void SetupTCP(const char* remoteIpAddres, int remotePort); void SetupTCP(const char* remoteIpAddres, int remotePort);
bool Send(Participant* remoteParticipant, int bufferSize); bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool SendTCP(int bufferSize); bool SendTCP(int bufferSize);
bool Publish(IMessage* msg); bool Publish(IMessage* msg);
@ -23,5 +23,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
#endif #endif
}; };
} // namespace Windows
} // namespace RoboidControl } // namespace RoboidControl
#endif

4
examples/BB2B.cpp → examples/BB2A/main.cpp
View File

@ -40,9 +40,9 @@ int main() {
// and sleep for 100ms // and sleep for 100ms
#if defined(ARDUINO) #if defined(ARDUINO)
delay(100); delay(10);
#else #else
sleep_for(milliseconds(100)); sleep_for(milliseconds(10));
#endif #endif
} }

27
examples/CMakeLists.txt
View File

@ -1,25 +1,8 @@
# examples/CMakeLists.txt # examples/CMakeLists.txt
# Specify the minimum CMake version # Check if the options are enabled and add the corresponding examples
cmake_minimum_required(VERSION 3.10) if(BUILD_EXAMPLE_BB2A)
add_executable(BB2A BB2A/main.cpp) # Adjust the path as necessary
# Specify the path to the main project directory target_link_libraries(BB2A RoboidControl)
set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..") endif()
# 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
)

2
examples/README.md
View File

@ -1 +1 @@
Important: this folder has to be names 'examples' exactly to maintain compatibility with Arduino Important: this folder has to be named 'examples' exactly to maintain compatibility with Arduino

30
test/CMakeLists.txt Normal file
View File

@ -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)

2
test/participant_test.cc
View File

@ -77,7 +77,7 @@ TEST(Participant, ThingMsg) {
} }
TEST(Participant, MQTT) { TEST(Participant, MQTT) {
ParticipantMQTT* participant = new ParticipantMQTT("192.168.77.11"); MQTTParticipantBase* participant = new MQTTParticipantBase("192.168.77.11");
unsigned long milliseconds = Thing::GetTimeMs(); unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds; unsigned long startTime = milliseconds;