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

Compare commits

base: RoboidControl:ff8570e2aa618937ede3d44147054a9fe53fa572
Branches Tags
RoboidControl:main
RoboidControl:MQTT
RoboidControl:0.4_dev
RoboidControl:Braccio
RoboidControl:ESP-IDF
RoboidControl:ControlCore
RoboidControl:Experimental
RoboidControl:subtree
RoboidControl:edition/arduino
RoboidControl:lighthousedeck
RoboidControl:hierarchy-support
RoboidControl:Quadcopter2
RoboidControl:DiscreteAngle
RoboidControl:master
RoboidControl:v0.3.0
RoboidControl:v0.2.1
RoboidControl:v0.1
RoboidControl:v0.2
..
compare: RoboidControl:26a80cd94a614bf926392109993b02d63e9b131e
Branches Tags
RoboidControl:MQTT
RoboidControl:0.4_dev
RoboidControl:main
RoboidControl:Braccio
RoboidControl:ESP-IDF
RoboidControl:ControlCore
RoboidControl:Experimental
RoboidControl:subtree
RoboidControl:edition/arduino
RoboidControl:lighthousedeck
RoboidControl:hierarchy-support
RoboidControl:Quadcopter2
RoboidControl:DiscreteAngle
RoboidControl:master
RoboidControl:v0.3.0
RoboidControl:v0.2.1
RoboidControl:v0.1
RoboidControl:v0.2

No commits in common. "ff8570e2aa618937ede3d44147054a9fe53fa572" and "26a80cd94a614bf926392109993b02d63e9b131e" have entirely different histories.
ff8570e2aa ... 26a80cd94a

43 changed files with 522 additions and 1158 deletions
Show Stats Expand all files Collapse all files

6
Arduino/ArduinoParticipant.cpp
View File

@ -1,5 +1,4 @@
#include "ArduinoParticipant.h"
#if defined(ARDUINO)
#if !defined(NO_STD)
#include <iostream>
@ -22,6 +21,7 @@
#endif
namespace RoboidControl {
namespace Arduino {
#if defined(ARDUINO) && defined(HAS_WIFI)
WiFiUDP* udp;
@ -88,7 +88,7 @@ void ParticipantUDP::Receive() {
#endif
}
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
bool ParticipantUDP::Send(Participant* 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 ParticipantUDPGeneric {
class ParticipantUDP : public RoboidControl::ParticipantUDP {
public:
void Setup();
void Receive();
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool Send(Participant* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg);
protected:
@ -18,5 +18,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
void GetBroadcastAddress();
};
} // namespace Arduino
} // namespace RoboidControl
#endif

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

@ -1,95 +0,0 @@
#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
View File

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

4
Arduino/Things/DRV8833.h
View File

@ -19,8 +19,6 @@ 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
@ -45,7 +43,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
virtual void Update(bool recurse = false) override;
protected:
DRV8833* drv8833 = nullptr;
DRV8833 drv8833;
};
#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,28 +57,6 @@ 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)
@ -87,11 +65,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 endregion Touch sensor
#pragma region Touch sensor
} // namespace Arduino
} // namespace RoboidControl

20
Arduino/Things/UltrasonicSensor.h
View File

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

43
CMakeLists.txt
View File

@ -22,23 +22,42 @@ 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}
enable_testing()
file(GLOB_RECURSE test_srcs test/*_test.cc)
add_executable(
RoboidControlTest
${test_srcs}
)
target_link_libraries(
RoboidControlTest
gtest_main
RoboidControl
LinearAlgebra
)
project(RoboidControl)
add_subdirectory(LinearAlgebra)
# Examples
option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF)
add_subdirectory(examples)
enable_testing()
add_subdirectory(test)
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 = 0.4
PROJECT_NUMBER =
# 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

121
EspIdf/EspIdfParticipant.cpp
View File

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

36
EspIdf/EspIdfParticipant.h
View File

@ -1,47 +1,26 @@
#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 ParticipantUDPGeneric {
class ParticipantUDP : public RoboidControl::ParticipantUDP {
public:
/// @brief Create a participant without connecting to a site
/// @param port The port on which the participant communicates
/// These participant typically broadcast Participant messages to let site
/// servers on the local network know their presence. Alternatively they can
/// broadcast information which can be used directly by other participants.
ParticipantUDP(int port = 7681);
/// @brief Create a participant which will try to connect to a site.
/// @param ipAddress The IP address of the site
/// @param port The port used by the site
/// @param localPort The port used by the local participant
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Receive();
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool Send(Participant* 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 sock;
int sockfd;
struct sockaddr_in dest_addr;
// struct sockaddr_in src_addr;
#endif
@ -49,6 +28,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
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,6 +10,8 @@ namespace RoboidControl {
#pragma region Participant
ParticipantRegistry Participant::registry;
Participant* Participant::LocalParticipant = new Participant();
void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
@ -22,7 +24,6 @@ Participant::Participant() {
//this->Add(this->root);
}
/*
Participant::Participant(const char* ipAddress, int port) {
Thing::CreateRoot(this);
//this->Add(this->root);
@ -42,26 +43,19 @@ Participant::Participant(const char* ipAddress, int port) {
this->ipAddress = addressString;
this->port = port;
}
*/
Participant::~Participant() {
// registry.Remove(this);
// delete[] this->ipAddress;
delete[] this->ipAddress;
}
void Participant::Update(bool recurse) {
void Participant::Update() {
for (Thing* thing : this->things) {
if (thing != nullptr)
thing->Update();
thing->Update(true);
}
}
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)
@ -73,31 +67,31 @@ bool Participant::Send(IMessage* msg) {
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->SendTo(this, bufferSize);
return thisWindows->Send(this, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->SendTo(this, bufferSize);
return thisPosix->Send(this, bufferSize);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->SendTo(this, bufferSize);
return thisArduino->Send(this, bufferSize);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->SendTo(this, bufferSize);
return thisEspIdf->Send(remoteParticipant, bufferSize);
#else
return false;
#endif
}
*/
Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
Thing* Participant::Get(unsigned char thingId) {
for (Thing* thing : this->things) {
if (thing->owner->networkId == networkId && thing->id == thingId)
if (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;
}
@ -119,24 +113,22 @@ void Participant::Add(Thing* thing, bool checkId) {
thing->id = highestIx + 1;
this->things.push_back(thing);
#endif
std::cout << "Add thing with generated ID "
//<< this->ipAddress << ":" << this->port
<< "[" << (int)thing->id << "]\n";
// std::cout << "Add thing with generated ID " << this->ipAddress << ":"
// << this->port << "[" << (int)thing->id << "]\n";
} else {
Thing* foundThing = Get(thing->owner->networkId, thing->id);
Thing* foundThing = Get(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";
}
}
}
@ -164,88 +156,86 @@ 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(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(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::Add(const char* ipAddress,
// // unsigned int port) {
// // Participant* participant = new Participant(ipAddress, port);
// // Add(participant);
// // return participant;
// // }
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;
}
// void ParticipantRegistry::Add(Participant* participant) {
// Participant* foundParticipant =
// Get(participant->networkId);
// //Get(participant->ipAddress, participant->port);
Participant* ParticipantRegistry::Add(const char* ipAddress,
unsigned int port) {
Participant* participant = new Participant(ipAddress, port);
Add(participant);
return participant;
}
// 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::Add(Participant* participant) {
Participant* foundParticipant =
Get(participant->ipAddress, participant->port);
// void ParticipantRegistry::Remove(Participant* participant) {
// // participants.remove(participant);
// }
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 defined(NO_STD)
// Participant** ParticipantRegistry::GetAll() const {
// return ParticipantRegistry::participants;
// }
// #else
// const std::list<Participant*>& ParticipantRegistry::GetAll() const {
// return ParticipantRegistry::participants;
// }
// #endif
void ParticipantRegistry::Remove(Participant* participant) {
// participants.remove(participant);
}
// #pragma endregion ParticipantRegistry
#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
} // namespace RoboidControl

29
Participant.h
View File

@ -7,7 +7,6 @@ namespace RoboidControl {
constexpr int MAX_THING_COUNT = 256;
/*
/// @brief class which manages all known participants
class ParticipantRegistry {
public:
@ -15,7 +14,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
@ -25,8 +24,7 @@ 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);
@ -54,7 +52,6 @@ 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
@ -91,6 +88,15 @@ 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;
@ -106,10 +112,9 @@ 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 networkId, unsigned char thingId);
Thing* Get(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
@ -124,7 +129,7 @@ class Participant {
public:
/// @brief Update all things for this participant
virtual void Update(bool recurse = true);
virtual void Update();
#pragma endregion Update
@ -137,12 +142,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

8
Participants/IsolatedParticipant.cpp
View File

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

4
Participants/IsolatedParticipant.h
View File

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

74
Participants/ParticipantMQTT.cpp
View File

@ -8,15 +8,19 @@
#include <unistd.h>
#endif
#include "Participants/ParticipantUDP.h"
namespace RoboidControl {
MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port)
: RemoteParticipantUDP("127.0.0.1", port) {
#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) {
this->name = "ParticipantUDP";
this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
// Participant::registry.Add(this);
this->remoteSite = new Participant(ipAddress, port);
Participant::registry.Add(this);
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
this->root->owner = this;
@ -26,27 +30,22 @@ MQTTParticipantBase::MQTTParticipantBase(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 MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
void ParticipantMQTT::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
@ -67,25 +66,25 @@ void MQTTParticipantBase::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
SendTCP(index);
thisWindows.SendTCP(index);
std::cout << "Send connect, client ID = " << client_id << std::endl;
std::cout << "Send connect, client ID = " << client_id << std::endl;
}
void MQTTParticipantBase::sendSubscribe(const char* topic) {
void ParticipantMQTT::sendSubscribe(const char* topic) {
// Packet Identifier (2 bytes)
static unsigned short packetId =
1; // Increment this for each new subscription
@ -113,28 +112,27 @@ void MQTTParticipantBase::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
SendTCP(7 + topicLength);
thisWindows.SendTCP(7 + topicLength);
std::cout << "Send subscribe to topic: " << topic << std::endl;
std::cout << "Send subscribe to topic: " << topic << std::endl;
}
void MQTTParticipantBase::Update(bool recurse) {
void ParticipantMQTT::Update() {
int packetSize = ReceiveTCP();
if (packetSize > 0) {
ReceiveData(packetSize);
}
}
void MQTTParticipantBase::SendTCP(int bufferSize) {}
int MQTTParticipantBase::ReceiveTCP() {
return false;
int ParticipantMQTT::ReceiveTCP() {
return thisWindows.ReceiveTCP();
}
void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) {
void ParticipantMQTT::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,7 +11,6 @@
#include "Messages/TextMsg.h"
#include "Messages/ThingMsg.h"
#include "Participant.h"
#include "Participants/ParticipantUDP.h"
#if !defined(NO_STD)
#include <functional>
@ -19,14 +18,15 @@
// #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,16 +43,14 @@ namespace RoboidControl {
/// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing()
class MQTTParticipantBase : public RemoteParticipantUDP {
class ParticipantMQTT : public Participant {
#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
MQTTParticipantBase(const char* ipAddress, int port = 1883);
protected:
virtual void SetupTCP();
ParticipantMQTT(const char* ipAddress, int port = 1883);
#pragma endregion Init
@ -60,7 +58,7 @@ protected:
public:
/// @brief The remote site when this participant is connected to a site
RemoteParticipantUDP* remoteSite = nullptr;
Participant* remoteSite = nullptr;
protected:
#if defined(__unix__) || defined(__APPLE__)
@ -77,7 +75,7 @@ protected:
#pragma region Update
public:
virtual void Update(bool recurse = true) override;
virtual void Update() override;
#pragma endregion Update
@ -98,15 +96,13 @@ protected:
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,167 +8,73 @@
#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
ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
: RemoteParticipantUDP("127.0.0.1", port) {
ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
this->name = "ParticipantUDP";
this->remoteSite = nullptr;
if (this->port == 0)
this->isIsolated = true;
registry.Add(this);
Participant::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);
}
ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
int port,
int localPort)
: RemoteParticipantUDP("127.0.0.1", localPort) {
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
: Participant("127.0.0.1", localPort) {
this->name = "ParticipantUDP";
if (this->port == 0)
this->isIsolated = true;
else
this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
registry.Add(this);
this->remoteSite = new Participant(ipAddress, port);
Participant::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 ParticipantUDPGeneric::begin() {
void ParticipantUDP::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
@ -177,55 +83,106 @@ void ParticipantUDPGeneric::begin() {
// 1. receive external messages
// 2. update the state
// 3. send out the updated messages
void ParticipantUDPGeneric::Update(bool recurse) {
void ParticipantUDP::Update() {
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 ParticipantUDPGeneric::UpdateMyThings() {
void ParticipantUDP::PrepMyThings() {
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
thing->PrepareForUpdate();
}
}
void ParticipantUDP::UpdateMyThings() {
// std::cout << this->things.size() << std::endl;
for (Thing* thing : this->things) {
if (thing == nullptr) // || thing->GetParent() != nullptr)
continue;
// Why don't we do recursive?
// std::cout << thing->name << "\n";
// if (thing->hierarchyChanged) {
// if (!(this->isIsolated || this->networkId == 0)) {
// ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
// this->remoteSite->Send(thingMsg);
// delete thingMsg;
// if (thing->nameChanged) {
// NameMsg* nameMsg = new NameMsg(this->networkId, thing);
// this->remoteSite->Send(nameMsg);
// delete nameMsg;
// }
// }
// }
// std::cout << "B\n";
// Why don't we do recursive?
// Because when a thing creates a thing in the update,
// that new thing is not sent out (because of hierarchyChanged)
// before it is updated itself: it is immediatedly updated!
thing->Update(false);
// std::cout << "C\n";
// if (!(this->isIsolated || this->networkId == 0)) {
// if (thing->terminate) {
// DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
// this->remoteSite->Send(destroyMsg);
// delete destroyMsg;
// } else {
// // Send to remote site
// if (thing->nameChanged) {
// NameMsg* nameMsg = new NameMsg(this->networkId, thing);
// this->remoteSite->Send(nameMsg);
// delete nameMsg;
// }
// PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
// this->remoteSite->Send(poseMsg);
// delete poseMsg;
// BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
// this->remoteSite->Send(binaryMsg);
// delete binaryMsg;
// }
// }
// std::cout << "D\n";
if (thing->terminate)
this->Remove(thing);
// std::cout << "E\n";
}
}
void ParticipantUDPGeneric::UpdateOtherThings() {
void ParticipantUDP::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 : registry.GetAll()) {
for (Participant* participant : Participant::registry.GetAll()) {
#endif
if (participant == nullptr || participant == this)
continue;
@ -253,8 +210,8 @@ void ParticipantUDPGeneric::UpdateOtherThings() {
#pragma region Send
void ParticipantUDPGeneric::SendThingInfo(Participant* remoteParticipant,
Thing* thing) {
void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
Thing* thing) {
// std::cout << "Send thing info [" << (int)thing->id << "] \n";
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
remoteParticipant->Send(thingMsg);
@ -273,14 +230,14 @@ void ParticipantUDPGeneric::SendThingInfo(Participant* remoteParticipant,
delete binaryMsg;
}
bool ParticipantUDPGeneric::Send(IMessage* msg) {
bool ParticipantUDP::Send(IMessage* msg) {
if (this->remoteSite != nullptr)
return this->remoteSite->Send(msg);
return true;
return false;
}
void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) {
void ParticipantUDP::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...
@ -301,31 +258,71 @@ void ParticipantUDPGeneric::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 ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
char* senderIpAddress,
unsigned int senderPort) {
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) {
// std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
// << std::endl;
RemoteParticipantUDP* sender =
this->registry.Get(senderIpAddress, senderPort);
Participant* 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 ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
RemoteParticipantUDP* sender) {
void ParticipantUDP::ReceiveData(unsigned char bufferSize,
Participant* sender) {
unsigned char msgId = this->buffer[0];
// std::cout << "receive msg " << (int)msgId << "\n";
// std::cout << " buffer size = " <<(int) bufferSize << "\n";
@ -398,16 +395,14 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
ParticipantMsg* msg) {
void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
<< "\n";
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
NetworkIdMsg* msg) {
void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
<< " -> " << (int)msg->networkId << "\n";
@ -422,68 +417,28 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
}
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
InvestigateMsg* msg) {
void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n";
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
ThingMsg* msg) {
void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
<< (int)msg->parentId << "\n";
#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);
}
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) {
void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "] ";
#endif
Thing* thing = sender->Get(msg->networkId, msg->thingId);
Thing* thing = sender->Get(msg->thingId);
if (thing != nullptr) {
int nameLength = msg->nameLength;
int stringLen = nameLength + 1;
@ -509,25 +464,23 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
ModelUrlMsg* msg) {
void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n";
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
PoseMsg* msg) {
void ParticipantUDP::Process(Participant* 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 = registry.Get(msg->networkId);
Participant* owner = Participant::registry.Get(msg->networkId);
if (owner == nullptr)
return;
Thing* thing = owner->Get(msg->networkId, msg->thingId);
Thing* thing = owner->Get(msg->thingId);
if (thing == nullptr)
return;
@ -541,16 +494,15 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
thing->SetAngularVelocity(msg->angularVelocity);
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
BinaryMsg* msg) {
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
<< "/" << (int)msg->thingId << "]\n";
#endif
Participant* owner = registry.Get(msg->networkId);
Participant* owner = Participant::registry.Get(msg->networkId);
if (owner != nullptr) {
Thing* thing = owner->Get(msg->networkId, msg->thingId);
Thing* thing = owner->Get(msg->thingId);
if (thing != nullptr)
thing->ProcessBinary(msg->data);
#if !defined(NO_STD)
@ -564,22 +516,20 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
}
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
TextMsg* msg) {
void ParticipantUDP::Process(Participant* sender, TextMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
<< (int)msg->text << "\n";
#endif
}
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
DestroyMsg* msg) {
void ParticipantUDP::Process(Participant* 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->networkId, msg->thingId);
Thing* thing = sender->Get(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/TextMsg.h"
#include "Messages/NetworkIdMsg.h"
#include "Messages/ThingMsg.h"
#include "Messages/TextMsg.h"
#include "Participant.h"
#if !defined(NO_STD)
@ -31,74 +31,6 @@ 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
@ -111,7 +43,8 @@ class ParticipantRegistry {
/// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing()
class ParticipantUDPGeneric : public RemoteParticipantUDP {
class ParticipantUDP : public Participant {
#pragma region Init
public:
@ -120,32 +53,31 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
/// 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.
ParticipantUDPGeneric(int port = 7681);
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
ParticipantUDPGeneric(const char* ipAddress,
int port = 7681,
int localPort = 7681);
ParticipantUDP(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
RemoteParticipantUDP* remoteSite = nullptr;
Participant* 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;
@ -164,13 +96,13 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
#pragma region Update
public:
virtual void Update(bool recurse = true) override;
virtual void Update() 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();
@ -182,46 +114,35 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
void PublishThingInfo(Thing* thing);
virtual bool Send(IMessage* msg) override;
virtual bool Publish(IMessage* msg) = 0;
bool Publish(IMessage* msg);
#pragma endregion Send
#pragma region Receive
protected:
protected:
void ReceiveData(unsigned char bufferSize,
char* senderIpAddress,
unsigned int senderPort);
void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant);
void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0;
void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
virtual void ReceiveUDP() = 0;
void ReceiveUDP();
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);
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);
#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 : registry.GetAll()) {
for (Participant* participant : Participant::registry.GetAll()) {
#endif
if (participant == nullptr || participant == this)
continue;
@ -56,7 +56,7 @@ void SiteServer::UpdateMyThings() {
#pragma region Receive
void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
if (msg->networkId != sender->networkId) {
// std::cout << this->name << " received New Client -> " <<
// sender->ipAddress
@ -67,19 +67,19 @@ void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
}
}
void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->networkId, msg->thingId);
if (thing == nullptr) {
void SiteServer::Process(Participant* sender, ThingMsg* msg) {
Thing* thing = sender->Get(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->networkId, msg->parentId));
thing->SetParent(Get(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(RemoteParticipantUDP* sender, ParticipantMsg* msg) override;
virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override;
virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override;
virtual void Process(Participant* sender, ParticipantMsg* msg) override;
virtual void Process(Participant* sender, NetworkIdMsg* msg) override;
virtual void Process(Participant* sender, ThingMsg* msg) override;
#pragma endregion Receive

2
Posix/PosixMQTT.cpp
View File

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

45
Posix/PosixParticipant.cpp
View File

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

32
Posix/PosixParticipant.h
View File

@ -1,36 +1,20 @@
#pragma once
#if defined(__unix__) || defined(__APPLE__)
#include "Participants/ParticipantUDP.h"
namespace RoboidControl {
namespace Posix {
class ParticipantUDP : public ParticipantUDPGeneric {
class ParticipantUDP : public RoboidControl::ParticipantUDP {
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 SetupUDP(int localPort,
const char* remoteIpAddress,
int remotePort) override;
void ReceiveUDP() override;
//void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void SetupTCP(const char* remoteIpAddress, int remotePort);
// void Receive();
void Receive();
int ReceiveTCP();
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool Publish(IMessage* msg) override;
bool Send(Participant* remoteParticipant, int bufferSize);
bool SendTCP(int bufferSize);
bool Publish(IMessage* msg);
protected:
#if defined(__unix__) || defined(__APPLE__)
@ -40,5 +24,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
#endif
};
} // namespace Posix
} // namespace RoboidControl
#endif

47
Posix/PoxisMQTT.cpp
View File

@ -1,47 +0,0 @@
#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
View File

@ -1,23 +0,0 @@
#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,60 +14,4 @@ Supporting:
# Basic components
- 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.
- RoboidControl::Participant

3
Testing/Temporary/LastTest.log
View File

@ -1,3 +0,0 @@
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,6 +253,12 @@ 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;
@ -262,7 +268,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 ParticipantUDPGeneric;
class ParticipantUDP;
#define THING_STORE_SIZE 256
// IMPORTANT: values higher than 256 will need to change the Thing::id type
@ -88,11 +88,6 @@ 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;
@ -228,7 +223,7 @@ class Thing {
#pragma region Update
public:
//virtual void PrepareForUpdate();
virtual void PrepareForUpdate();
/// @brief Updates the state of the thing
/// @param recurse When true, this will Update the descendants recursively

29
Things/DistanceSensor.cpp
View File

@ -1,29 +0,0 @@
#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
View File

@ -1,41 +0,0 @@
#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 {
// When finishing this release (0.3), I notice that this is equivalent to a digital sensor
// Why 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,5 +1,4 @@
#include "WindowsParticipant.h"
#if defined(_WIN32) || defined(_WIN64)
#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
@ -8,6 +7,7 @@
#endif
namespace RoboidControl {
namespace Windows {
ParticipantUDP::ParticipantUDP() {}
@ -195,7 +195,7 @@ int ParticipantUDP::ReceiveTCP() {
return 0;
}
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
#if defined(_WIN32) || defined(_WIN64)
char ip_str[INET_ADDRSTRLEN];
inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@ -247,6 +247,5 @@ 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 ParticipantUDPGeneric {
class ParticipantUDP : public RoboidControl::ParticipantUDP {
public:
ParticipantUDP();
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
void SetupTCP(const char* remoteIpAddres, int remotePort);
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
bool Send(Participant* remoteParticipant, int bufferSize);
bool SendTCP(int bufferSize);
bool Publish(IMessage* msg);
@ -23,5 +23,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
#endif
};
} // namespace Windows
} // namespace RoboidControl
#endif

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

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

27
examples/CMakeLists.txt
View File

@ -1,8 +1,25 @@
# examples/CMakeLists.txt
# 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 minimum CMake version
cmake_minimum_required(VERSION 3.10)
target_link_libraries(BB2A RoboidControl)
endif()
# 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
)

2
examples/README.md
View File

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

30
test/CMakeLists.txt
View File

@ -1,30 +0,0 @@
# 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) {
MQTTParticipantBase* participant = new MQTTParticipantBase("192.168.77.11");
ParticipantMQTT* participant = new ParticipantMQTT("192.168.77.11");
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;