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

8
Arduino/ArduinoParticipant.h
View File

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

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

4
Arduino/Things/DRV8833.h
View File

@ -19,6 +19,8 @@ class DRV8833 : public Thing {
int BIn1;
int BIn2;
int standby = 255;
constexpr Configuration(int a1, int a2, int b1, int b2, int standby = 255) : AIn1(a1), AIn2(a2), BIn1(b1), BIn2(b2), standby(standby) {}
};
/// @brief Setup a DRV8833 motor controller
@ -43,7 +45,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
virtual void Update(bool recurse = false) override;
protected:
DRV8833 drv8833;
DRV8833* drv8833 = nullptr;
};
#pragma endregion Differential drive

30
Arduino/Things/UltrasonicSensor.cpp
View File

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

20
Arduino/Things/UltrasonicSensor.h
View File

@ -1,5 +1,6 @@
#pragma once
#include "Things/DistanceSensor.h"
#include "Things/TouchSensor.h"
namespace RoboidControl {
@ -39,9 +40,26 @@ class UltrasonicSensor : Thing {
unsigned char pinEcho = 0;
public:
class DistanceSensor;
class TouchSensor;
};
#pragma region Distance sensor
class UltrasonicSensor::DistanceSensor : public RoboidControl::DistanceSensor {
public:
DistanceSensor(UltrasonicSensor::Configuration config,
Thing* parent = Thing::LocalRoot());
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recursive = false) override;
protected:
UltrasonicSensor ultrasonic;
};
#pragma endregion Distance sensor
#pragma region Touch sensor
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
@ -58,7 +76,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
UltrasonicSensor ultrasonic;
};
#pragma region Touch sensor
#pragma endregion Touch sensor
} // namespace Arduino
} // namespace RoboidControl

41
CMakeLists.txt
View File

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

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
# control system is used.
PROJECT_NUMBER =
PROJECT_NUMBER = 0.4
# Using the PROJECT_BRIEF tag one can provide an optional one line description
# for a project that appears at the top of each page and should give viewer a

123
EspIdf/EspIdfParticipant.cpp
View File

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

36
EspIdf/EspIdfParticipant.h
View File

@ -1,26 +1,47 @@
#pragma once
#if defined(IDF_VER)
#include "Participants/ParticipantUDP.h"
#if defined(IDF_VER)
#include "lwip/sockets.h"
#endif
namespace RoboidControl {
namespace EspIdf {
class ParticipantUDP : public RoboidControl::ParticipantUDP {
class ParticipantUDP : public ParticipantUDPGeneric {
public:
/// @brief Create a participant without connecting to a site
/// @param port The port on which the participant communicates
/// These participant typically broadcast Participant messages to let site
/// servers on the local network know their presence. Alternatively they can
/// broadcast information which can be used directly by other participants.
ParticipantUDP(int port = 7681);
/// @brief Create a participant which will try to connect to a site.
/// @param ipAddress The IP address of the site
/// @param port The port used by the site
/// @param localPort The port used by the local participant
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive();
bool Send(Participant* remoteParticipant, int bufferSize);
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
// bool SendTest();
/// @brief Sens a message to the remote site (if set)
/// @param msg The message to send
/// @return True if a message could be sent.
bool Send(IMessage* msg) override;
void SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) override;
void ReceiveUDP() override;
protected:
#if defined(IDF_VER)
char broadcastIpAddress[INET_ADDRSTRLEN];
int sockfd;
int sock;
struct sockaddr_in dest_addr;
// struct sockaddr_in src_addr;
#endif
@ -28,5 +49,6 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
void GetBroadcastAddress();
};
} // namespace EspIdf
} // namespace RoboidControl
#endif

4
Messages/PoseMsg.cpp
View File

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

204
Participant.cpp
View File

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

29
Participant.h
View File

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

6
Participants/IsolatedParticipant.cpp
View File

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

2
Participants/IsolatedParticipant.h
View File

@ -1,3 +1,4 @@
/*
#include "Participant.h"
namespace RoboidControl {
@ -11,3 +12,4 @@ class IsolatedParticipant {
};
}
*/

74
Participants/ParticipantMQTT.cpp
View File

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

18
Participants/ParticipantMQTT.h
View File

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

364
Participants/ParticipantUDP.cpp
View File

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

131
Participants/ParticipantUDP.h
View File

@ -5,11 +5,11 @@
#include "Messages/InvestigateMsg.h"
#include "Messages/ModelUrlMsg.h"
#include "Messages/NameMsg.h"
#include "Messages/NetworkIdMsg.h"
#include "Messages/ParticipantMsg.h"
#include "Messages/PoseMsg.h"
#include "Messages/NetworkIdMsg.h"
#include "Messages/ThingMsg.h"
#include "Messages/TextMsg.h"
#include "Messages/ThingMsg.h"
#include "Participant.h"
#if !defined(NO_STD)
@ -31,6 +31,74 @@ namespace RoboidControl {
constexpr int MAX_SENDER_COUNT = 256;
class RemoteParticipantUDP : public Participant {
public:
/// @brief Create a new participant with the given communcation info
/// @param ipAddress The IP address of the participant
/// @param port The UDP port of the participant
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
RemoteParticipantUDP(const char* ipAddress, int port);
/// @brief The Ip Address of a participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant.
/// @remarks This does not belong here, it should move to ParticipantUDP or
/// something like that in the future
unsigned int port = 0;
bool Send(IMessage* msg) override;
};
/// @brief class which manages all known participants
class ParticipantRegistry {
public:
/// @brief Retrieve a participant by its address
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The participant or a nullptr when it could not be found
RemoteParticipantUDP* Get(const char* ipAddress, unsigned int port);
/// @brief Retrieve a participant by its network ID
/// @param networkID The network ID of the participant
/// @return The participant or a nullptr when it could not be found
RemoteParticipantUDP* Get(unsigned char networkID);
/// @brief Add a participant with the given details
/// @param ipAddress The IP address of the participant
/// @param port The port number of the participant
/// @return The added participant
RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(RemoteParticipantUDP* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(RemoteParticipantUDP* participant);
private:
#if defined(NO_STD)
public:
RemoteParticipantUDP** GetAll() const;
int count = 0;
private:
RemoteParticipantUDP** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<RemoteParticipantUDP*>& GetAll() const;
private:
/// @brief The list of known participants
std::list<RemoteParticipantUDP*> participants;
#endif
};
/// @brief A participant using UDP communication
/// A local participant is the local device which can communicate with
/// other participants It manages all local things and communcation with other
@ -43,8 +111,7 @@ constexpr int MAX_SENDER_COUNT = 256;
/// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing()
class ParticipantUDP : public Participant {
class ParticipantUDPGeneric : public RemoteParticipantUDP {
#pragma region Init
public:
@ -53,31 +120,32 @@ class ParticipantUDP : public Participant {
/// These participant typically broadcast Participant messages to let site
/// servers on the local network know their presence. Alternatively they can
/// broadcast information which can be used directly by other participants.
ParticipantUDP(int port = 7681);
ParticipantUDPGeneric(int port = 7681);
/// @brief Create a participant which will try to connect to a site.
/// @param ipAddress The IP address of the site
/// @param port The port used by the site
/// @param localPort The port used by the local participant
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
ParticipantUDPGeneric(const char* ipAddress,
int port = 7681,
int localPort = 7681);
#pragma endregion Init
#pragma region Properties
public:
public:
/// @brief True if the participant is running isolated.
/// Isolated participants do not communicate with other participants
bool isIsolated = false;
/// @brief The remote site when this participant is connected to a site
Participant* remoteSite = nullptr;
RemoteParticipantUDP* remoteSite = nullptr;
/// The interval in milliseconds for publishing (broadcasting) data on the
/// local network
long publishInterval = 3000; // 3 seconds
protected:
#if !defined(ARDUINO)
#if defined(__unix__) || defined(__APPLE__)
int sock;
@ -96,13 +164,13 @@ public:
#pragma region Update
public:
virtual void Update() override;
virtual void Update(bool recurse = true) override;
protected:
unsigned long nextPublishMe = 0;
/// @brief Prepare the local things for the next update
virtual void PrepMyThings();
// virtual void PrepMyThings();
virtual void UpdateMyThings();
virtual void UpdateOtherThings();
@ -114,35 +182,46 @@ public:
void PublishThingInfo(Thing* thing);
virtual bool Send(IMessage* msg) override;
bool Publish(IMessage* msg);
virtual bool Publish(IMessage* msg) = 0;
#pragma endregion Send
#pragma region Receive
protected:
protected:
void ReceiveData(unsigned char bufferSize,
char* senderIpAddress,
unsigned int senderPort);
void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant);
void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0;
void ReceiveUDP();
virtual void ReceiveUDP() = 0;
virtual void Process(Participant* sender, ParticipantMsg* msg);
virtual void Process(Participant* sender, NetworkIdMsg* msg);
virtual void Process(Participant* sender, InvestigateMsg* msg);
virtual void Process(Participant* sender, ThingMsg* msg);
virtual void Process(Participant* sender, NameMsg* msg);
virtual void Process(Participant* sender, ModelUrlMsg* msg);
virtual void Process(Participant* sender, PoseMsg* msg);
virtual void Process(Participant* sender, BinaryMsg* msg);
virtual void Process(Participant* sender, TextMsg* msg);
virtual void Process(Participant* sender, DestroyMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg);
virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender,
ThingMsg* msg,
bool isRemote);
virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg);
#pragma endregion Receive
public:
static ParticipantRegistry registry;
};
} // namespace RoboidControl
#include "EspIdf/EspIdfParticipant.h"
#include "Posix/PosixParticipant.h"

18
Participants/SiteServer.cpp
View File

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

6
Participants/SiteServer.h
View File

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

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

32
Posix/PosixParticipant.h
View File

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

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

56
README.md
View File

@ -15,3 +15,59 @@ Supporting:
- RoboidControl::Thing
- 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->Add(this);
// std::cout << this->owner->name << ": New root thing " << std::endl;
std::cout << this->owner->name << ": New root thing " << std::endl;
}
void Thing::CreateRoot(Participant* owner) {
@ -253,12 +253,6 @@ unsigned long Thing::GetTimeMs() {
#endif
}
// void Thing::Update(bool recursive) {
// Update(GetTimeMs(), recursive);
// }
void Thing::PrepareForUpdate() {}
void Thing::Update(bool recursive) {
this->positionUpdated = false;
this->orientationUpdated = false;
@ -268,7 +262,7 @@ void Thing::Update(bool recursive) {
this->nameChanged = false;
if (recursive) {
// std::cout << "# children: " << (int)this->childCount << std::endl;
std::cout << "# children: " << (int)this->childCount << std::endl;
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx];
if (child == nullptr)

9
Thing.h
View File

@ -10,7 +10,7 @@
namespace RoboidControl {
class Participant;
class ParticipantUDP;
class ParticipantUDPGeneric;
#define THING_STORE_SIZE 256
// IMPORTANT: values higher than 256 will need to change the Thing::id type
@ -88,6 +88,11 @@ class Thing {
/// This can be either a Thing::Type of a byte value for custom types
unsigned char type = Type::Undetermined;
/// @brief Is this a remote thing?
/// A remote thing is owned by other participant
/// and is not simulated by the local participant
bool isRemote = false;
/// @brief The participant owning this thing
Participant* owner = nullptr;
@ -223,7 +228,7 @@ class Thing {
#pragma region Update
public:
virtual void PrepareForUpdate();
//virtual void PrepareForUpdate();
/// @brief Updates the state of the thing
/// @param recurse When true, this will Update the descendants recursively

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;
}
void TouchSensor::PrepareForUpdate() {
//this->internalTouch = this->externalTouch;
}
// void TouchSensor::PrepareForUpdate() {
// //this->internalTouch = this->externalTouch;
// }
void TouchSensor::Update(bool recursive) {
Thing::Update(recursive);

4
Things/TouchSensor.h
View File

@ -6,7 +6,7 @@ namespace RoboidControl {
/// @brief A sensor which can detect touches
class TouchSensor : public Thing {
// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor
// When finishing this release (0.3), I notice that this is equivalent to a digital sensor
public:
/// @brief Create a new child touch sensor
/// @param parent The parent thing
@ -18,7 +18,7 @@ class TouchSensor : public Thing {
/// otherwise
bool IsTouching();
virtual void PrepareForUpdate() override;
//virtual void PrepareForUpdate() override;
virtual void Update(bool recursive) override;
/// @brief Function used to generate binary data for this touch sensor

7
Windows/WindowsParticipant.cpp
View File

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

8
Windows/WindowsParticipant.h
View File

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

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

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

27
examples/CMakeLists.txt
View File

@ -1,25 +1,8 @@
# examples/CMakeLists.txt
# Specify the minimum CMake version
cmake_minimum_required(VERSION 3.10)
# Check if the options are enabled and add the corresponding examples
if(BUILD_EXAMPLE_BB2A)
add_executable(BB2A BB2A/main.cpp) # Adjust the path as necessary
# Specify the path to the main project directory
set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..")
# Set the project name
project(Examples)
include_directories(..)
# Add the executable for the main project
#add_executable(MainExecutable ${SOURCES})
# Find the main project library (assuming it's defined in the root CMakeLists.txt)
#find_package(RoboidControl REQUIRED) # Replace MyLibrary with your actual library name
# Add example executables
add_executable(BB2B BB2B.cpp)
target_link_libraries(
BB2B
RoboidControl
LinearAlgebra
)
target_link_libraries(BB2A RoboidControl)
endif()

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) {
ParticipantMQTT* participant = new ParticipantMQTT("192.168.77.11");
MQTTParticipantBase* participant = new MQTTParticipantBase("192.168.77.11");
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;