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

Compare commits

base: RoboidControl:19a5c5d4d580759e0045ddc6ab724da773cfcecb
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:df19ecf95381982bdd7426b66bab2ab2f7eef132
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. "19a5c5d4d580759e0045ddc6ab724da773cfcecb" and "df19ecf95381982bdd7426b66bab2ab2f7eef132" have entirely different histories.
19a5c5d4d5 ... df19ecf953

89 changed files with 1286 additions and 2823 deletions
Show Stats Expand all files Collapse all files

77
Arduino/ArduinoParticipant.cpp
View File

@ -1,9 +1,4 @@
#include "ArduinoParticipant.h"
#if defined(ARDUINO)
#if !defined(NO_STD)
#include <iostream>
#endif
#if defined(ARDUINO)
#if defined(ARDUINO_ARCH_ESP8266)
@ -22,20 +17,19 @@
#endif
namespace RoboidControl {
namespace Arduino {
void ParticipantUDP::Setup(int localPort,
const char* remoteIpAddress,
int remotePort) {
#if defined(ARDUINO) && defined(HAS_WIFI)
WiFiUDP* udp;
#endif
void ParticipantUDP::Setup() {
#if defined(ARDUINO) && defined(HAS_WIFI)
this->remoteIpAddress = remoteIpAddress;
this->remotePort = remotePort;
GetBroadcastAddress();
#if defined(UNO_R4)
if (WiFi.status() == WL_NO_MODULE) {
#if !defined(NO_STD)
std::cout << "No network available!\n";
#endif
return;
}
#else
@ -44,16 +38,9 @@ void ParticipantUDP::Setup() {
return;
}
#endif
udp.begin(localPort);
udp = new WiFiUDP();
udp->begin(this->port);
#if !defined(NO_STD)
std::cout << "Wifi sync started local " << this->port;
if (this->remoteSite != nullptr)
std::cout << ", remote " << this->remoteSite->ipAddress << ":"
<< this->remoteSite->port << "\n";
#endif
std::cout << "Wifi sync started to port " << this->remotePort << "\n";
#endif
}
@ -65,51 +52,55 @@ void ParticipantUDP::GetBroadcastAddress() {
this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
broadcastIpString.toCharArray(this->broadcastIpAddress,
broadcastIpString.length() + 1);
#if !defined(NO_STD)
std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
#endif
#endif
}
void ParticipantUDP::Receive() {
#if defined(ARDUINO) && defined(HAS_WIFI)
int packetSize = udp->parsePacket();
int packetSize = udp.parsePacket();
while (packetSize > 0) {
udp->read(buffer, packetSize);
udp.read(buffer, packetSize);
String senderAddress = udp->remoteIP().toString();
String senderAddress = udp.remoteIP().toString();
char sender_ipAddress[16];
senderAddress.toCharArray(sender_ipAddress, 16);
unsigned int sender_port = udp->remotePort();
unsigned int sender_port = udp.remotePort();
// Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
// sender_port); if (remoteParticipant == nullptr) {
// remoteParticipant = this->AddParticipant(sender_ipAddress,
// sender_port);
// // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
// // << "\n";
// // std::cout << "New remote participant " <<
// remoteParticipant->ipAddress
// // << ":" << remoteParticipant->port << " "
// // << (int)remoteParticipant->networkId << "\n";
// }
// ReceiveData(packetSize, remoteParticipant);
ReceiveData(packetSize, sender_ipAddress, sender_port);
packetSize = udp->parsePacket();
packetSize = udp.parsePacket();
}
#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";
int n = 0;
int r = 0;
do {
if (n > 0) {
#if !defined(NO_STD)
std::cout << "Retry sending\n";
#endif
delay(10);
}
n++;
udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
udp->write((unsigned char*)buffer, bufferSize);
r = udp->endPacket();
// On an Uno R4 WiFi, endPacket blocks for 10 seconds the first time
// It is not cleary yet why
} while (r == 0 && n < 10);
udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
udp.write((unsigned char*)buffer, bufferSize);
} while (udp.endPacket() == 0 && n < 10);
#endif
return true;
@ -121,9 +112,9 @@ bool ParticipantUDP::Publish(IMessage* msg) {
if (bufferSize <= 0)
return true;
udp->beginPacket(this->broadcastIpAddress, this->port);
udp->write((unsigned char*)buffer, bufferSize);
udp->endPacket();
udp.beginPacket(this->broadcastIpAddress, this->remotePort);
udp.write((unsigned char*)buffer, bufferSize);
udp.endPacket();
// std::cout << "Publish to " << this->broadcastIpAddress << ":"
// << this->remotePort << "\n";
@ -131,5 +122,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
return true;
};
} // namespace Arduino
} // namespace RoboidControl
#endif

22
Arduino/ArduinoParticipant.h
View File

@ -1,22 +1,32 @@
#pragma once
#if defined(ARDUINO)
#include "Participants/ParticipantUDP.h"
namespace RoboidControl {
#if defined(HAS_WIFI)
#include <WiFiUdp.h>
#endif
class ParticipantUDP : public ParticipantUDPGeneric {
namespace RoboidControl {
namespace Arduino {
class ParticipantUDP : public RoboidControl::ParticipantUDP {
public:
void Setup();
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);
protected:
#if defined(HAS_WIFI)
const char* remoteIpAddress = nullptr;
unsigned short remotePort = 0;
char* broadcastIpAddress = nullptr;
WiFiUDP udp;
#endif
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

103
Arduino/Things/DRV8833.cpp
View File

@ -5,83 +5,49 @@
namespace RoboidControl {
namespace Arduino {
#pragma region DRV8833
DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) {
this->type = Type::Undetermined;
this->name = "DRV8833";
this->pinStandby = config.standby;
if (pinStandby != 255)
pinMode(pinStandby, OUTPUT);
this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
this->motorA->SetName("Motor A");
this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
this->motorB->SetName("Motor B");
}
#pragma endregion DRV8833
#pragma region Differential drive
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;
}
void DRV8833::DifferentialDrive::Update(bool recurse) {
RoboidControl::DifferentialDrive::Update(recurse);
this->drv8833->Update(false);
}
#pragma endregion Differential drive
#pragma region Motor
#if (ESP32)
uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
#endif
DRV8833Motor::DRV8833Motor(DRV8833* driver,
unsigned char pinIn1,
unsigned char pinIn2,
bool reverse)
: Motor(driver) {
DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
: Thing(participant) {
this->pinIn1 = pinIn1;
this->pinIn2 = pinIn2;
#if (ESP32)
in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
in1Ch = nextAvailablePwmChannel++;
ledcSetup(in1Ch, 500, 8);
ledcAttachPin(pinIn1, in1Ch);
in2Ch = DRV8833Motor::nextAvailablePwmChannel++;
in2Ch = nextAvailablePwmChannel++;
ledcSetup(in2Ch, 500, 8);
ledcAttachPin(pinIn2, in2Ch);
#else
pinMode(pinIn1, OUTPUT); // configure the in1 pin to output mode
pinMode(pinIn2, OUTPUT); // configure the in1 pin to output mode
#endif
// this->reverse = reverse;
this->reverse = reverse;
}
// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
// this->maxRpm = rpm;
// }
void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
this->maxRpm = rpm;
}
void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
Motor::SetTargetVelocity(motorSpeed);
void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
Thing::SetAngularVelocity(velocity);
// ignoring rotation axis for now.
// Spherical angularVelocity = this->GetAngularVelocity();
float angularSpeed = velocity.distance; // in degrees/sec
uint8_t motorSignal =
(uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
float rpm = angularSpeed / 360 * 60;
float motorSpeed = rpm / this->maxRpm;
// std::cout << "moto speed " << this->name << " = " << motorSpeed
uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
// if (direction == RotationDirection::CounterClockwise)
// speed = -speed;
// Determine the rotation direction
if (velocity.direction.horizontal.InDegrees() < 0)
motorSpeed = -motorSpeed;
if (this->reverse)
motorSpeed = -motorSpeed;
// std::cout << "ang speed " << this->name << " = " << angularSpeed << " rpm " << rpm
// << ", motor signal = " << (int)motorSignal << "\n";
#if (ESP32)
@ -131,7 +97,24 @@ void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
#endif
}
#pragma endregion Motor
DRV8833::DRV8833(Participant* participant,
unsigned char pinAIn1,
unsigned char pinAIn2,
unsigned char pinBIn1,
unsigned char pinBIn2,
unsigned char pinStandby,
bool reverseA,
bool reverseB)
: Thing(participant) {
this->pinStandby = pinStandby;
if (pinStandby != 255)
pinMode(pinStandby, OUTPUT);
this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA);
this->motorA->name = "Motor A";
this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB);
this->motorB->name = "Motor B";
}
} // namespace Arduino
} // namespace RoboidControl

103
Arduino/Things/DRV8833.h
View File

@ -1,87 +1,60 @@
#pragma once
#include <Arduino.h>
#include "Participants/IsolatedParticipant.h"
#include "Thing.h"
#include "Things/DifferentialDrive.h"
#include "Things/Motor.h"
namespace RoboidControl {
namespace Arduino {
class DRV8833Motor;
/// @brief Support for a DRV8833 motor controller
class DRV8833Motor : public Thing {
public:
/// @brief Motor turning direction
enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
/// @brief Setup the DC motor
/// @param pinIn1 the pin number for the in1 signal
/// @param pinIn2 the pin number for the in2 signal
/// @param direction the forward turning direction of the motor
DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false);
void SetMaxRPM(unsigned int rpm);
virtual void SetAngularVelocity(Spherical velocity) override;
bool reverse = false;
protected:
unsigned char pinIn1 = 255;
unsigned char pinIn2 = 255;
unsigned int maxRpm = 200;
};
class DRV8833 : public Thing {
public:
struct Configuration {
int AIn1;
int AIn2;
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
DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());
/// @param pinAIn1 The pin number connected to the AIn1 port
/// @param pinAIn2 The pin number connected to the AIn2 port
/// @param pinBIn1 The pin number connected to the BIn1 port
/// @param pinBIn2 The pin number connceted to the BIn2 port
/// @param pinStandby The pin number connected to the standby port, 255
/// indicated that the port is not connected
/// @param reverseA The forward turning direction of motor A
/// @param reverseB The forward turning direction of motor B
DRV8833(Participant* participant,
unsigned char pinAIn1,
unsigned char pinAIn2,
unsigned char pinBIn1,
unsigned char pinBIn2,
unsigned char pinStandby = 255,
bool reverseA = false,
bool reverseB = false);
DRV8833Motor* motorA = nullptr;
DRV8833Motor* motorB = nullptr;
protected:
unsigned char pinStandby = 255;
public:
class DifferentialDrive;
};
#pragma region Differential drive
class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
public:
DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot());
virtual void Update(bool recurse = false) override;
protected:
DRV8833* drv8833 = nullptr;
};
#pragma endregion Differential drive
#pragma region Motor
/// @brief Support for a DRV8833 motor controller
class DRV8833Motor : public Motor {
public:
/// @brief Setup the DC motor
/// @param pinIn1 the pin number for the in1 signal
/// @param pinIn2 the pin number for the in2 signal
/// @param direction the forward turning direction of the motor
DRV8833Motor(DRV8833* driver,
unsigned char pinIn1,
unsigned char pinIn2,
bool reverse = false);
// void SetMaxRPM(unsigned int rpm);
// virtual void SetAngularVelocity(Spherical velocity) override;
virtual void SetTargetVelocity(float targetSpeed) override;
// bool reverse = false;
protected:
unsigned char pinIn1 = 255;
unsigned char pinIn2 = 255;
// unsigned int maxRpm = 200;
#if (ESP32)
uint8_t in1Ch;
uint8_t in2Ch;
static uint8_t nextAvailablePwmChannel;
#endif
};
#pragma endregion Motor
} // namespace Arduino
} // namespace RoboidControl

125
Arduino/Things/DigitalInput.cpp
View File

@ -5,126 +5,19 @@
namespace RoboidControl {
namespace Arduino {
#pragma region Digital input
DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(parent) {
this->type = Type::Switch;
this->name = "Digital Input";
DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
: TouchSensor(participant) {
this->pin = pin;
pinMode(this->pin, INPUT);
std::cout << "digital input start\n";
pinMode(pin, INPUT);
}
void DigitalInput::Update(bool recursive) {
this->isHigh = digitalRead(this->pin);
this->isLow = !this->isHigh;
Thing::Update(recursive);
void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) {
this->touchedSomething = digitalRead(pin) == LOW;
// std::cout << "DigitalINput pin " << (int)this->pin << ": " <<
// this->touchedSomething << "\n";
Thing::Update(currentTimeMs, recursive);
}
#pragma endregion Digital input
#pragma region Touch sensor
DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
: RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
void DigitalInput::TouchSensor::Update(bool recursive) {
this->internalTouch = digitalInput.isLow;
}
#pragma endregion Touch sensor
#pragma region Relative encoder
int DigitalInput::RelativeEncoder::interruptCount = 0;
volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
Thing* parent)
: RoboidControl::RelativeEncoder(parent),
digitalInput(config.pin, parent),
pulsesPerRevolution(config.pulsesPerRevolution) {}
void DigitalInput::RelativeEncoder::Start() {
// We support up to 2 pulse counters
if (interruptCount == 0) {
std::cout << "pin interrupt 1 activated" << std::endl;
attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0,
RISING);
} else if (interruptCount == 1) {
std::cout << "pin interrupt 2 activated" << std::endl;
attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1,
RISING);
} else {
// maximum interrupt count reached
std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached"
<< std::endl;
return;
}
interruptIx = interruptCount;
interruptCount++;
std::cout << "pin ints. " << interruptCount << std::endl;
}
int DigitalInput::RelativeEncoder::GetPulseCount() {
if (interruptIx == 0) {
int pulseCount = pulseCount0;
pulseCount0 = 0;
return pulseCount;
} else if (interruptIx == 1) {
int pulseCount = pulseCount1;
pulseCount1 = 0;
return pulseCount;
}
return 0;
}
void DigitalInput::RelativeEncoder::Update(bool recursive) {
unsigned long currentTimeMs = GetTimeMs();
if (this->lastUpdateTime == 0) {
this->Start();
this->lastUpdateTime = currentTimeMs;
return;
}
float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f;
if (timeStep <= 0)
return;
int pulseCount = GetPulseCount();
netPulseCount += pulseCount;
this->pulseFrequency = pulseCount / timeStep;
this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
// std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
// << " timestep " << timeStep << std::endl;
this->lastUpdateTime = currentTimeMs;
}
#if defined(ESP8266) || defined(ESP32)
void ICACHE_RAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt0() {
pulseCount0++;
}
#else
void DigitalInput::RelativeEncoder::PulseInterrupt0() {
pulseCount0++;
}
#endif
#if defined(ESP8266) || defined(ESP32)
void IRAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt1() {
pulseCount1++;
}
#else
void DigitalInput::RelativeEncoder::PulseInterrupt1() {
pulseCount1++;
}
#endif
#pragma endregion Relative encoder
} // namespace Arduino
} // namespace RoboidControl

74
Arduino/Things/DigitalInput.h
View File

@ -1,92 +1,26 @@
#pragma once
#include "Things/RelativeEncoder.h"
#include "Things/TouchSensor.h"
namespace RoboidControl {
namespace Arduino {
/// @brief A digital input represents the stat of a digital GPIO pin
class DigitalInput : public Thing {
class DigitalInput : public TouchSensor {
public:
/// @brief Create a new digital input
/// @param participant The participant to use
/// @param pin The digital pin
//DigitalInput(Participant* participant, unsigned char pin);
// DigitalInput(Thing* parent, unsigned char pin);
DigitalInput(unsigned char pin, Thing* parent);
bool isHigh = false;
bool isLow = false;
DigitalInput(Participant* participant, unsigned char pin);
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recursive = false) override;
virtual void Update(unsigned long currentTimeMs,
bool recursive = false) override;
protected:
/// @brief The pin used for digital input
unsigned char pin = 0;
public:
class TouchSensor;
class RelativeEncoder;
};
#pragma region Touch sensor
class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
public:
TouchSensor(unsigned char pin, Thing* parent);
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recurse = false) override;
protected:
DigitalInput digitalInput;
};
#pragma endregion Touch sensor
#pragma region Incremental encoder
class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
public:
struct Configuration {
unsigned char pin;
unsigned char pulsesPerRevolution;
};
RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot());
unsigned char pulsesPerRevolution;
/// @brief The current pulse frequency in Hz
float pulseFrequency = 0;
/// @copydoc RoboidControl::Thing::Update()
virtual void Update(bool recurse = false) override;
protected:
DigitalInput digitalInput;
int interruptIx = 0;
static int interruptCount;
volatile static int pulseCount0;
static void PulseInterrupt0();
volatile static int pulseCount1;
static void PulseInterrupt1();
int GetPulseCount();
long netPulseCount = 0;
unsigned long lastUpdateTime = 0;
private:
void Start();
};
#pragma endregion Incremental encoder
} // namespace Arduino
} // namespace RoboidControl

77
Arduino/Things/UltrasonicSensor.cpp
View File

@ -1,17 +1,16 @@
#include "UltrasonicSensor.h"
#include <Arduino.h>
#include <iostream>
namespace RoboidControl {
namespace Arduino {
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
: Thing(parent) {
this->type = Type::DistanceSensor;
this->name = "Ultrasonic sensor";
this->pinTrigger = config.trigger;
this->pinEcho = config.echo;
UltrasonicSensor::UltrasonicSensor(Participant* participant,
unsigned char pinTrigger,
unsigned char pinEcho)
: TouchSensor(participant) {
this->pinTrigger = pinTrigger;
this->pinEcho = pinEcho;
pinMode(pinTrigger, OUTPUT); // configure the trigger pin to output mode
pinMode(pinEcho, INPUT); // configure the echo pin to input mode
@ -20,14 +19,14 @@ UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
float UltrasonicSensor::GetDistance() {
// Start the ultrasonic 'ping'
digitalWrite(pinTrigger, LOW);
delayMicroseconds(2);
delayMicroseconds(5);
digitalWrite(pinTrigger, HIGH);
delayMicroseconds(10);
digitalWrite(pinTrigger, LOW);
// Measure the duration of the pulse on the echo pin
unsigned long duration_us =
pulseIn(pinEcho, HIGH, 10000); // the result is in microseconds
float duration_us =
pulseIn(pinEcho, HIGH, 100000); // the result is in microseconds
// Calculate the distance:
// * Duration should be divided by 2, because the ping goes to the object
@ -38,60 +37,32 @@ float UltrasonicSensor::GetDistance() {
// * Now we calculate the distance based on the speed of sound (340 m/s):
// distance = duration_sec * 340;
// * The result calculation is therefore:
this->distance = (float)duration_us / 2 / 1000000 * 340;
// Serial.println(this->distance);
// std::cout << "US distance " << this->distance << std::endl;
this->distance = duration_us / 2 / 1000000 * 340;
// Filter faulty measurements. The sensor can often give values > 30 m which
// are not correct
// if (distance > 30)
// distance = 0;
return this->distance;
this->touchedSomething |= (this->distance <= this->touchDistance);
// std::cout << "Ultrasonic " << this->distance << " " <<
// this->touchedSomething << "\n";
return distance;
}
void UltrasonicSensor::Update(bool recursive) {
void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
this->touchedSomething = false;
GetDistance();
Thing::Update(recursive);
Thing::Update(currentTimeMs, 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)
: RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}
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);
}
#pragma endregion Touch sensor
// void UltrasonicSensor::ProcessBinary(char* bytes) {
// this->touchedSomething = (bytes[0] == 1);
// if (this->touchedSomething)
// std::cout << "Touching something!\n";
// }
} // namespace Arduino
} // namespace RoboidControl

59
Arduino/Things/UltrasonicSensor.h
View File

@ -1,25 +1,25 @@
#pragma once
#include "Things/DistanceSensor.h"
#include "Things/TouchSensor.h"
namespace RoboidControl {
namespace Arduino {
/// @brief An HC-SR04 ultrasonic distance sensor
class UltrasonicSensor : Thing {
class UltrasonicSensor : public TouchSensor {
public:
struct Configuration {
int trigger;
int echo;
};
UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
/// @brief Setup an ultrasonic sensor
/// @param participant The participant to use
/// @param pinTrigger The pin number of the trigger signal
/// @param pinEcho The pin number of the echo signal
UltrasonicSensor(Participant* participant,
unsigned char pinTrigger,
unsigned char pinEcho);
// parameters
/// @brief The distance at which the object is considered to be touched
// float touchDistance = 0.2f;
float touchDistance = 0.2f;
// state
@ -31,52 +31,15 @@ class UltrasonicSensor : Thing {
float GetDistance();
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recursive = false) override;
virtual void Update(unsigned long currentTimeMs,
bool recursive = false) override;
protected:
/// @brief The pin number of the trigger signal
unsigned char pinTrigger = 0;
/// @brief The pin number of the echo signal
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 {
public:
TouchSensor(UltrasonicSensor::Configuration config,
Thing* parent = Thing::LocalRoot());
float touchDistance = 0.2f;
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
virtual void Update(bool recursive = false) override;
protected:
UltrasonicSensor ultrasonic;
};
#pragma endregion Touch sensor
} // namespace Arduino
} // namespace RoboidControl

41
CMakeLists.txt
View File

@ -19,27 +19,46 @@ if(ESP_PLATFORM)
REQUIRES esp_netif esp_wifi
)
else()
project(RoboidControl)
add_subdirectory(LinearAlgebra)
add_subdirectory(Examples)
set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
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 = 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

118
EspIdf/EspIdfParticipant.cpp
View File

@ -4,12 +4,13 @@
#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();
@ -21,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() {
@ -97,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
@ -118,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";
@ -141,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
@ -217,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";
@ -225,4 +162,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
return true;
};
} // namespace EspIdf
} // namespace RoboidControl

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

18
examples/BB2A/main.cpp → Examples/BB2B.cpp
View File

@ -17,32 +17,32 @@ using namespace RoboidControl;
int main() {
// The robot's propulsion is a differential drive
DifferentialDrive bb2b = DifferentialDrive();
DifferentialDrive* bb2b = new DifferentialDrive();
// Is has a touch sensor at the front left of the roboid
TouchSensor touchLeft = TouchSensor(&bb2b);
TouchSensor* touchLeft = new TouchSensor(bb2b);
// and other one on the right
TouchSensor touchRight = TouchSensor(&bb2b);
TouchSensor* touchRight = new TouchSensor(bb2b);
// Do forever:
while (true) {
// 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 leftWheelSpeed = (touchRight.IsTouching()) ? -600.0f : 600.0f;
float leftWheelSpeed = (touchRight->touchedSomething) ? -600.0f : 600.0f;
// The right wheel does the same, but instead is controlled by
// touches on the left side
float rightWheelSpeed = (touchLeft.IsTouching()) ? -600.0f : 600.0f;
float rightWheelSpeed = (touchLeft->touchedSomething) ? -600.0f : 600.0f;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
bb2b->SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
// Update the roboid state
bb2b.Update(true);
bb2b->Update(true);
// and sleep for 100ms
#if defined(ARDUINO)
delay(10);
delay(100);
#else
sleep_for(milliseconds(10));
sleep_for(milliseconds(100));
#endif
}

25
Examples/CMakeLists.txt Normal file
View File

@ -0,0 +1,25 @@
# examples/CMakeLists.txt
# Specify the minimum CMake version
cmake_minimum_required(VERSION 3.10)
# 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
)

6
LinearAlgebra/Direction.cpp
View File

@ -9,7 +9,6 @@
#include <math.h>
namespace LinearAlgebra {
template <typename T>
DirectionOf<T>::DirectionOf() {
this->horizontal = AngleOf<T>();
@ -99,6 +98,5 @@ void DirectionOf<T>::Normalize() {
}
}
template class LinearAlgebra::DirectionOf<float>;
template class LinearAlgebra::DirectionOf<signed short>;
}
template class DirectionOf<float>;
template class DirectionOf<signed short>;

2
LinearAlgebra/Direction.h
View File

@ -99,4 +99,6 @@ using Direction = DirectionSingle;
} // namespace LinearAlgebra
using namespace LinearAlgebra;
#endif

26
LinearAlgebra/Matrix.cpp
View File

@ -1,7 +1,5 @@
#include "Matrix.h"
#if !defined(NO_STD)
#include <iostream>
#endif
namespace LinearAlgebra {
@ -63,9 +61,7 @@ Matrix2::Matrix2(const Matrix2& m)
this->data = nullptr;
else {
this->data = new float[this->nValues];
for (int ix = 0; ix < this->nValues; ++ix)
this->data[ix] = m.data[ix];
std::copy(m.data, m.data + nValues, this->data);
}
}
@ -80,8 +76,7 @@ Matrix2& Matrix2::operator=(const Matrix2& m) {
this->data = nullptr;
else {
this->data = new float[this->nValues];
for (int ix = 0; ix < this->nValues; ++ix)
this->data[ix] = m.data[ix];
std::copy(m.data, m.data + this->nValues, this->data);
}
}
return *this;
@ -94,8 +89,7 @@ Matrix2::~Matrix2() {
Matrix2 Matrix2::Clone() const {
Matrix2 r = Matrix2(this->nRows, this->nCols);
for (int ix = 0; ix < this->nValues; ++ix)
r.data[ix] = this->data[ix];
std::copy(this->data, this->data + this->nValues, r.data);
return r;
}
@ -139,7 +133,7 @@ Matrix2 Matrix2::Identity(int size) {
}
Matrix2 Matrix2::Diagonal(float f, int size) {
Matrix2 r = Matrix2::Zero(size, size);
Matrix2 r = Matrix2(size, size);
float* data = r.data;
int valueIx = 0;
for (int ix = 0; ix < size; ix++) {
@ -164,8 +158,8 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
Matrix2 Matrix2::Transpose() const {
Matrix2 r = Matrix2(this->nCols, this->nRows);
for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
for (int colIx = 0; colIx < this->nCols; colIx++)
for (uint rowIx = 0; rowIx < this->nRows; rowIx++) {
for (uint colIx = 0; colIx < this->nCols; colIx++)
r.data[colIx * this->nCols + rowIx] =
this->data[rowIx * this->nCols + colIx];
}
@ -206,16 +200,16 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
int BColOffset = i * BCols; // BColOffset is constant for each row of B
for (int j = 0; j < BCols; ++j) {
float sum = 0;
std::cout << " 0";
// std::cout << " 0";
int BIndex = j;
for (int k = 0; k < ACols; ++k) {
std::cout << " + " << this->data[ARowOffset + k] << " * "
<< B.data[BIndex];
// std::cout << " + " << this->data[ARowOffset + k] << " * "
// << B.data[BIndex];
sum += this->data[ARowOffset + k] * B.data[BIndex];
BIndex += BCols;
}
r.data[BColOffset + j] = sum;
std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
// std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
}
}
return r;

4
LinearAlgebra/Polar.cpp
View File

@ -175,5 +175,5 @@ PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
return r;
}
template class LinearAlgebra::PolarOf<float>;
template class LinearAlgebra::PolarOf<signed short>;
template class PolarOf<float>;
template class PolarOf<signed short>;

4
LinearAlgebra/Spherical.cpp
View File

@ -5,8 +5,6 @@
#include <math.h>
namespace LinearAlgebra {
template <typename T>
SphericalOf<T>::SphericalOf() {
this->distance = 0.0f;
@ -303,5 +301,3 @@ SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
template class SphericalOf<float>;
template class SphericalOf<signed short>;
} // namespace LinearAlgebra

1
LinearAlgebra/Spherical.h
View File

@ -186,6 +186,7 @@ using Spherical = SphericalSingle;
#endif
} // namespace LinearAlgebra
using namespace LinearAlgebra;
#include "Polar.h"
#include "Vector3.h"

4
LinearAlgebra/SwingTwist.cpp
View File

@ -4,8 +4,6 @@
#include "SwingTwist.h"
namespace LinearAlgebra {
template <typename T>
SwingTwistOf<T>::SwingTwistOf() {
this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
@ -168,5 +166,3 @@ void SwingTwistOf<T>::Normalize() {
template class SwingTwistOf<float>;
template class SwingTwistOf<signed short>;
}

2
LinearAlgebra/test/Direction_test.cc
View File

@ -6,8 +6,6 @@
#include "Direction.h"
using namespace LinearAlgebra;
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Direction16, Compare) {

1
LinearAlgebra/test/Polar_test.cc
View File

@ -2,7 +2,6 @@
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include <chrono>
#include "Polar.h"
#include "Spherical.h"

1
LinearAlgebra/test/Spherical16_test.cc
View File

@ -2,7 +2,6 @@
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include <chrono>
#include "Spherical.h"
#include "Vector3.h"

1
LinearAlgebra/test/SphericalSingle_test.cc
View File

@ -2,7 +2,6 @@
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include <chrono>
#include "Spherical.h"

5
Messages/BinaryMsg.cpp
View File

@ -6,8 +6,7 @@ BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
this->networkId = networkId;
this->thingId = thing->id;
this->thing = thing;
unsigned char ix = 0; //BinaryMsg::length;
this->data = new char[255];
unsigned char ix = BinaryMsg::length;
this->dataLength = this->thing->GenerateBinary(this->data, &ix);
}
@ -41,8 +40,6 @@ unsigned char BinaryMsg::Serialize(char* buffer) {
buffer[ix++] = this->networkId;
buffer[ix++] = this->thingId;
buffer[ix++] = this->dataLength;
for (int dataIx = 0; dataIx < this->dataLength; dataIx++)
buffer[ix++] = this->data[dataIx];
return this->length + this->dataLength;
}

10
Messages/BinaryMsg.h
View File

@ -1,17 +1,15 @@
#pragma once
#include "IMessage.h"
#include "Thing.h"
#include "Messages.h"
namespace RoboidControl {
/// @brief A message containing binary data for custom communication
/// @brief Message to send thing-specific data
class BinaryMsg : public IMessage {
public:
/// @brief The message ID
static const unsigned char id = 0xB1;
/// @brief The length of the message in bytes, excluding the binary data
/// For the total size of the message this.bytes.Length should be added to this value.
/// @brief The length of the message without the binary data itslef
static const unsigned length = 4;
/// @brief The network ID of the thing
@ -25,7 +23,7 @@ class BinaryMsg : public IMessage {
/// @brief The binary data which is communicated
char* data = nullptr;
/// @brief Create a BinaryMsg
/// @brief Create a new message for sending
/// @param networkId The network ID of the thing
/// @param thing The thing for which binary data is sent
BinaryMsg(unsigned char networkId, Thing* thing);

17
Messages/DestroyMsg.cpp
View File

@ -2,22 +2,19 @@
namespace RoboidControl {
DestroyMsg::DestroyMsg(unsigned char networkId, Thing* thing) {
DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
this->networkId = networkId;
this->thingId = thing->id;
}
DestroyMsg::DestroyMsg(char* buffer) {
this->networkId = buffer[1];
this->thingId = buffer[2];
}
DestroyMsg::DestroyMsg(char* buffer) {}
DestroyMsg::~DestroyMsg() {}
unsigned char DestroyMsg::Serialize(char* buffer) {
#if defined(DEBUG)
std::cout << "Send DestroyMsg [" << (int)this->networkId << "/"
<< (int)this->thingId << "] " << std::endl;
unsigned char DestroyMsg::Serialize(char *buffer) {
#if defined(DEBUG)
std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId
<< "] " << std::endl;
#endif
unsigned char ix = 0;
buffer[ix++] = this->id;
@ -26,4 +23,4 @@ unsigned char DestroyMsg::Serialize(char* buffer) {
return ix;
}
} // namespace RoboidControl
} // namespace RoboidControl

9
Messages/DestroyMsg.h
View File

@ -1,16 +1,13 @@
#pragma once
#include "IMessage.h"
#include "Thing.h"
#include "Messages.h"
namespace RoboidControl {
/// @brief A Message notifiying that a Thing no longer exists
/// @brief Message notifiying that a Thing no longer exists
class DestroyMsg : public IMessage {
public:
/// @brief The message ID
static const unsigned char id = 0x20;
/// @brief The length of the message in bytes
/// @brief The length of the message
static const unsigned length = 3;
/// @brief The network ID of the thing
unsigned char networkId;

16
Messages/IMessage.h
View File

@ -1,16 +0,0 @@
#pragma once
namespace RoboidControl {
/// @brief Root structure for all communcation messages
class IMessage {
public:
IMessage();
/// @brief Serialize the message into a byte array for sending
/// @param buffer The buffer to serilize into
/// @return The length of the message in the buffer
virtual unsigned char Serialize(char* buffer);
};
} // namespace RoboidControl

4
Messages/InvestigateMsg.cpp
View File

@ -7,9 +7,9 @@ InvestigateMsg::InvestigateMsg(char* buffer) {
this->networkId = buffer[ix++];
this->thingId = buffer[ix++];
}
InvestigateMsg::InvestigateMsg(unsigned char networkId, Thing* thing) {
InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
this->networkId = networkId;
this->thingId = thing->id;
this->thingId = thingId;
}
InvestigateMsg::~InvestigateMsg() {}

11
Messages/InvestigateMsg.h
View File

@ -1,7 +1,4 @@
#pragma once
#include "IMessage.h"
#include "Thing.h"
#include "Messages.h"
namespace RoboidControl {
@ -17,10 +14,10 @@ class InvestigateMsg : public IMessage {
/// @brief the ID of the thing
unsigned char thingId;
/// @brief Create an investigate message
/// @brief Create a new message for sending
/// @param networkId The network ID for the thing
/// @param thing The thing for which the details are requested
InvestigateMsg(unsigned char networkId, Thing* thing);
/// @param thingId The ID of the thing
InvestigateMsg(unsigned char networkId, unsigned char thingId);
/// @copydoc RoboidControl::IMessage::IMessage(char*)
InvestigateMsg(char* buffer);
/// @brief Destructor for the message

130
Messages/LowLevelMessages.cpp
View File

@ -5,83 +5,6 @@
namespace RoboidControl {
void LowLevelMessages::SendSpherical(char* buffer,
unsigned char* ix,
Spherical s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
unsigned char* startIndex) {
float distance = ReceiveFloat16(buffer, startIndex);
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle horizontal = Angle::Radians(horizontal8.InRadians());
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle vertical = Angle::Radians(vertical8.InRadians());
Spherical s = Spherical(distance, horizontal, vertical);
return s;
}
void LowLevelMessages::SendSwingTwist(char* buffer,
unsigned char* ix,
SwingTwist s) {
SendAngle8(buffer, ix, s.swing.horizontal.InDegrees());
SendAngle8(buffer, ix, s.swing.vertical.InDegrees());
SendAngle8(buffer, ix, s.twist.InDegrees());
}
SwingTwist LowLevelMessages::ReceiveSwingTwist(const char* buffer,
unsigned char* startIndex) {
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle horizontal = Angle::Radians(horizontal8.InRadians());
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle vertical = Angle::Radians(vertical8.InRadians());
Angle8 twist8 = ReceiveAngle8(buffer, startIndex);
Angle twist = Angle::Radians(twist8.InRadians());
SwingTwist s = SwingTwist(horizontal, vertical, twist);
return s;
}
void LowLevelMessages::SendQuat32(char* buffer,
unsigned char* ix,
SwingTwist rotation) {
Quaternion q = rotation.ToQuaternion();
unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128);
unsigned char qz = (char)(q.z * 127 + 128);
unsigned char qw = (char)(q.w * 255);
if (q.w < 0) {
qx = -qx;
qy = -qy;
qz = -qz;
qw = -qw;
}
// std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
// << "\n";
buffer[(*ix)++] = qx;
buffer[(*ix)++] = qy;
buffer[(*ix)++] = qz;
buffer[(*ix)++] = qw;
}
SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
unsigned char* ix) {
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qw = buffer[(*ix)++] / 255.0F;
Quaternion q = Quaternion(qx, qy, qz, qw);
SwingTwist s = SwingTwist::FromQuaternion(q);
return s;
}
void LowLevelMessages::SendAngle8(char* buffer,
unsigned char* ix,
const float angle) {
@ -119,5 +42,58 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
return (float)f.toFloat();
}
void LowLevelMessages::SendSpherical(char* buffer,
unsigned char* ix,
Spherical s) {
SendFloat16(buffer, ix, s.distance);
SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
}
Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
unsigned char* startIndex) {
float distance = ReceiveFloat16(buffer, startIndex);
Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
Angle horizontal = Angle::Radians(horizontal8.InRadians());
Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
Angle vertical = Angle::Radians(vertical8.InRadians());
Spherical s = Spherical(distance, horizontal, vertical);
return s;
}
void LowLevelMessages::SendQuat32(char* buffer,
unsigned char* ix,
SwingTwist rotation) {
Quaternion q = rotation.ToQuaternion();
unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128);
unsigned char qz = (char)(q.z * 127 + 128);
unsigned char qw = (char)(q.w * 255);
if (q.w < 0) {
qx = -qx;
qy = -qy;
qz = -qz;
qw = -qw;
}
// std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
// << "\n";
buffer[(*ix)++] = qx;
buffer[(*ix)++] = qy;
buffer[(*ix)++] = qz;
buffer[(*ix)++] = qw;
}
SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
unsigned char* ix) {
float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
float qw = buffer[(*ix)++] / 255.0F;
Quaternion q = Quaternion(qx, qy, qz, qw);
SwingTwist s = SwingTwist::FromQuaternion(q);
return s;
}
} // namespace RoboidControl

20
Messages/LowLevelMessages.h
View File

@ -1,5 +1,3 @@
#pragma once
#include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"
@ -7,22 +5,18 @@ namespace RoboidControl {
class LowLevelMessages {
public:
static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
static Spherical ReceiveSpherical(const char* buffer,
unsigned char* startIndex);
static void SendSwingTwist(char* buffer, unsigned char* ix, SwingTwist s);
static SwingTwist ReceiveSwingTwist(const char* buffer,
unsigned char* startIndex);
static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
static void SendAngle8(char* buffer, unsigned char* ix, const float angle);
static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex);
static void SendFloat16(char* buffer, unsigned char* ix, float value);
static float ReceiveFloat16(const char* buffer, unsigned char* startIndex);
static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
static Spherical ReceiveSpherical(const char* buffer,
unsigned char* startIndex);
static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
};
} // namespace RoboidControl

5
Messages/IMessage.cpp → Messages/Messages.cpp
View File

@ -1,4 +1,7 @@
#include "IMessage.h"
#include "Messages.h"
#include "LowLevelMessages.h"
#include "string.h"
namespace RoboidControl {

22
Messages/Messages.h Normal file
View File

@ -0,0 +1,22 @@
#pragma once
#include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"
#include "Thing.h"
namespace RoboidControl {
class ParticipantUDP;
class IMessage {
public:
IMessage();
virtual unsigned char Serialize(char* buffer);
static unsigned char* ReceiveMsg(unsigned char packetSize);
// bool Publish(ParticipantUDP *participant);
// bool SendTo(ParticipantUDP *participant);
};
} // namespace RoboidControl

7
Messages/ModelUrlMsg.h
View File

@ -1,7 +1,4 @@
#pragma once
#include "IMessage.h"
#include "Thing.h"
#include "Messages.h"
namespace RoboidControl {
@ -29,6 +26,8 @@ class ModelUrlMsg : public IMessage {
ModelUrlMsg(unsigned char networkId, Thing* thing);
/// @copydoc RoboidControl::IMessage::IMessage(char*)
ModelUrlMsg(const char* buffer);
// ModelUrlMsg(unsigned char networkId, unsigned char thingId,
// unsigned char urlLegth, const char *url, float scale = 1);
/// @brief Destructor for the message
virtual ~ModelUrlMsg();

7
Messages/NameMsg.cpp
View File

@ -7,16 +7,15 @@ namespace RoboidControl {
NameMsg::NameMsg(unsigned char networkId, Thing* thing) {
this->networkId = networkId;
this->thingId = thing->id;
const char* thingName = thing->GetName();
if (thingName == nullptr)
if (thing->name == nullptr)
this->nameLength = 0;
else
this->nameLength = (unsigned char)strlen(thingName);
this->nameLength = (unsigned char)strlen(thing->name);
// the name string in the buffer is not \0 terminated!
char* name = new char[this->nameLength + 1];
for (int i = 0; i < this->nameLength; i++)
name[i] = thingName[i];
name[i] = thing->name[i];
name[this->nameLength] = '\0';
this->name = name;
}

8
Messages/NameMsg.h
View File

@ -1,7 +1,4 @@
#pragma once
#include "IMessage.h"
#include "Thing.h"
#include "Messages.h"
namespace RoboidControl {
@ -25,6 +22,9 @@ class NameMsg : public IMessage {
/// @param networkId The network ID of the thing
/// @param thing The ID of the thing
NameMsg(unsigned char networkId, Thing* thing);
// NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
// unsigned char nameLength);
/// @copydoc RoboidControl::IMessage::IMessage(char*)
NameMsg(const char* buffer);
/// @brief Destructor for the message

27
Messages/NetworkIdMsg.cpp
View File

@ -1,27 +0,0 @@
#include "NetworkIdMsg.h"
#include <iostream>
namespace RoboidControl {
NetworkIdMsg::NetworkIdMsg(const char* buffer) {
this->networkId = buffer[1];
}
NetworkIdMsg::NetworkIdMsg(unsigned char networkId) {
this->networkId = networkId;
}
NetworkIdMsg::~NetworkIdMsg() {}
unsigned char NetworkIdMsg::Serialize(char* buffer) {
#if defined(DEBUG)
std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl;
#endif
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
return NetworkIdMsg::length;
}
} // namespace RoboidControl

6
Messages/ParticipantMsg.cpp
View File

@ -1,9 +1,5 @@
#include "ParticipantMsg.h"
#if !defined(NO_STD)
#include <iostream>
#endif
namespace RoboidControl {
ParticipantMsg::ParticipantMsg(char networkId) {
@ -17,7 +13,7 @@ ParticipantMsg::ParticipantMsg(const char* buffer) {
ParticipantMsg::~ParticipantMsg() {}
unsigned char ParticipantMsg::Serialize(char* buffer) {
#if defined(DEBUG) && !defined(NO_STD)
#if defined(DEBUG)
std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] "
<< std::endl;
#endif

2
Messages/ParticipantMsg.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "IMessage.h"
#include "Messages.h"
namespace RoboidControl {

10
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) {
this->position = thing->GetPosition();
this->poseType |= Pose_Position;
}
if (thing->orientationUpdated || (force || thing->IsRoot())) {
if (thing->orientationUpdated || force) {
this->orientation = thing->GetOrientation();
this->poseType |= Pose_Orientation;
}
@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) {
this->thingId = buffer[ix++];
this->poseType = buffer[ix++];
this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
// linearVelocity
// angularVelocity
}
@ -45,7 +45,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
if (this->poseType == 0)
return 0;
#if defined(DEBUG) && DEBUG > 1
#if defined(DEBUG)
std::cout << "Send PoseMsg [" << (int)this->networkId << "/"
<< (int)this->thingId << "] " << (int)this->poseType << std::endl;
#endif
@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
if ((this->poseType & Pose_Position) != 0)
LowLevelMessages::SendSpherical(buffer, &ix, this->position);
if ((this->poseType & Pose_Orientation) != 0)
LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
if ((this->poseType & Pose_LinearVelocity) != 0)
LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
if ((this->poseType & Pose_AngularVelocity) != 0)

9
Messages/PoseMsg.h
View File

@ -1,6 +1,4 @@
#pragma once
#include "IMessage.h"
#include "Thing.h"
#include "Messages.h"
namespace RoboidControl {
@ -11,7 +9,7 @@ class PoseMsg : public IMessage {
public:
/// @brief The message ID
static const unsigned char id = 0x10;
/// @brief The length of the message in bytes
/// @brief The length of the message
unsigned char length = 4 + 4 + 4;
/// @brief The network ID of the thing
@ -42,8 +40,7 @@ class PoseMsg : public IMessage {
/// @brief Create a new message for sending
/// @param networkId he network ID of the thing
/// @param thing The thing for which the pose should be sent
/// @param force If true, position and orientation are always included, even when they are not updated
/// @param thing The thing for which the pose shouldbe sent
PoseMsg(unsigned char networkId, Thing* thing, bool force = false);
/// @copydoc RoboidControl::IMessage::IMessage(char*)

25
Messages/SiteMsg.cpp Normal file
View File

@ -0,0 +1,25 @@
#include "SiteMsg.h"
namespace RoboidControl {
SiteMsg::SiteMsg(const char* buffer) {
this->networkId = buffer[1];
}
SiteMsg::SiteMsg(unsigned char networkId) {
this->networkId = networkId;
}
SiteMsg::~SiteMsg() {}
unsigned char SiteMsg::Serialize(char* buffer) {
#if defined(DEBUG)
std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl;
#endif
unsigned char ix = 0;
buffer[ix++] = this->id;
buffer[ix++] = this->networkId;
return SiteMsg::length;
}
} // namespace RoboidControl

12
Messages/NetworkIdMsg.h → Messages/SiteMsg.h
View File

@ -1,11 +1,9 @@
#pragma once
#include "IMessage.h"
#include "Messages.h"
namespace RoboidControl {
/// @brief A message communicating the network ID for that participant
class NetworkIdMsg : public IMessage {
class SiteMsg : public IMessage {
public:
/// @brief The message ID
static const unsigned char id = 0xA1;
@ -16,11 +14,11 @@ public:
/// @brief Create a new message for sending
/// @param networkId The network ID for the participant
NetworkIdMsg(unsigned char networkId);
SiteMsg(unsigned char networkId);
/// @copydoc RoboidControl::IMessage::IMessage(char*)
NetworkIdMsg(const char *buffer);
SiteMsg(const char *buffer);
/// @brief Destructor for the message
virtual ~NetworkIdMsg();
virtual ~SiteMsg();
/// @copydoc RoboidControl::IMessage::Serialize
virtual unsigned char Serialize(char *buffer) override;

6
Messages/TextMsg.cpp
View File

@ -1,9 +1,5 @@
#include "TextMsg.h"
#if !defined(NO_STD)
#include <iostream>
#endif
namespace RoboidControl {
TextMsg::TextMsg(const char* text, unsigned char textLength) {
@ -28,7 +24,7 @@ unsigned char TextMsg::Serialize(char* buffer) {
if (this->textLength == 0 || this->text == nullptr)
return 0;
#if defined(DEBUG) && !defined(NO_STD)
#if defined(DEBUG)
std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl;
#endif
unsigned char ix = 0;

6
Messages/TextMsg.h
View File

@ -1,4 +1,4 @@
#include "IMessage.h"
#include "Messages.h"
namespace RoboidControl {
@ -9,6 +9,10 @@ class TextMsg : public IMessage {
static const unsigned char id = 0xB0;
/// @brief The length of the message without the text itself
static const unsigned char length = 2;
/// @brief The network ID of the thing
unsigned char networkId;
/// @brief the ID of the thing
unsigned char thingId;
/// @brief The text without the null terminator
const char* text;
/// @brief The length of the text

22
Messages/ThingMsg.cpp
View File

@ -14,21 +14,27 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
this->networkId = networkId;
this->thingId = thing->id;
this->thingType = thing->type;
if (thing->IsRoot())
this->parentId = 0;
else {
Thing* parent = thing->GetParent();
Thing* parent = thing->GetParent();
if (parent != nullptr)
this->parentId = parent->id;
}
else
this->parentId = 0;
}
// ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
// unsigned char thingType, unsigned char parentId) {
// this->networkId = networkId;
// this->thingId = thingId;
// this->thingType = thingType;
// this->parentId = parentId;
// }
ThingMsg::~ThingMsg() {}
unsigned char ThingMsg::Serialize(char* buffer) {
#if defined(DEBUG)
std::cout << "Send ThingMsg [" << (int)this->networkId << "/"
<< (int)this->thingId << "] " << (int)this->thingType << " "
<< (int)this->parentId << std::endl;
std::cout << "Send ThingMsg [" << (int)this->networkId << "/" << (int)this->thingId
<< "] " << (int)this->thingType << " " << (int)this->parentId << std::endl;
#endif
unsigned char ix = 0;
buffer[ix++] = this->id;

11
Messages/ThingMsg.h
View File

@ -1,9 +1,8 @@
#include "IMessage.h"
#include "Thing.h"
#include "Messages.h"
namespace RoboidControl {
/// @brief Message providing generic details about a Thing
/// @brief Message providing generic information about a Thing
class ThingMsg : public IMessage {
public:
/// @brief The message ID
@ -14,15 +13,17 @@ class ThingMsg : public IMessage {
unsigned char networkId;
/// @brief The ID of the thing
unsigned char thingId;
/// @brief The type of thing
/// @brief The Thing.Type of the thing
unsigned char thingType;
/// @brief The ID of the parent thing in the hierarchy. This is zero for root things
/// @brief The parent of the thing in the hierarachy. This is null for root Things
unsigned char parentId;
/// @brief Create a message for sending
/// @param networkId The network ID of the thing</param>
/// @param thing The thing
ThingMsg(unsigned char networkId, Thing* thing);
// ThingMsg(unsigned char networkId, unsigned char thingId,
// unsigned char thingType, unsigned char parentId);
/// @copydoc RoboidControl::IMessage::IMessage(char*)
ThingMsg(const char* buffer);

195
Participant.cpp
View File

@ -1,32 +1,12 @@
#include "Participant.h"
#include <string.h>
#include "Arduino/ArduinoParticipant.h"
#include "EspIdf/EspIdfParticipant.h"
#include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.h"
namespace RoboidControl {
#pragma region Participant
Participant::Participant() {}
Participant* Participant::LocalParticipant = new Participant();
void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
LocalParticipant = &newParticipant;
std::cout << "Replaced local participant" << std::endl;
}
Participant::Participant() {
Thing::CreateRoot(this);
//this->Add(this->root);
}
/*
Participant::Participant(const char* ipAddress, int port) {
Thing::CreateRoot(this);
//this->Add(this->root);
// make a copy of the ip address string
int addressLength = (int)strlen(ipAddress);
int stringLength = addressLength + 1;
@ -42,62 +22,25 @@ 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(unsigned long currentTimeMs) {
for (Thing* thing : this->things) {
if (thing != nullptr)
thing->Update();
thing->Update(currentTimeMs, 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)
return true;
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
// << " to " << this->ipAddress << std::endl;
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->SendTo(this, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->SendTo(this, bufferSize);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->SendTo(this, bufferSize);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->SendTo(this, 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;
}
@ -108,35 +51,24 @@ void Participant::Add(Thing* thing, bool checkId) {
thing->id = this->thingCount + 1;
this->things[this->thingCount++] = thing;
#else
// find highest id
int highestIx = 0;
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
if (thing->id > highestIx)
highestIx = thing->id;
}
thing->id = highestIx + 1;
thing->id = (unsigned char)this->things.size() + 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";
}
}
}
@ -157,95 +89,22 @@ void Participant::Remove(Thing* thing) {
this->thingCount = lastThingIx;
#else
this->things.remove_if([thing](Thing* obj) { return obj == thing; });
// std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id
// << "] list size = " << this->things.size() << "\n";
std::cout << "Removing " << thing->networkId << "/" << thing->id
<< " list size = " << this->things.size() << "\n";
#endif
}
#pragma endregion
// void Participant::UpdateAll(unsigned long currentTimeMs) {
// // Not very efficient, but it works for now.
// #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;
// for (Thing* thing : this->things) {
// if (thing != nullptr && thing->GetParent() == nullptr) { // update all
// root things
// // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
// // << "\n";
// thing->Update(currentTimeMs);
// }
// }
// 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::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);
// 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);
// }
// #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

132
Participant.h
View File

@ -1,61 +1,10 @@
#pragma once
#include "Messages/IMessage.h"
#include "Thing.h"
namespace RoboidControl {
constexpr int MAX_THING_COUNT = 256;
/*
/// @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
//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
Participant* 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
//Participant* Add(const char* ipAddress, unsigned int port);
/// @brief Add a participant
/// @param participant The participant to add
void Add(Participant* participant);
/// @brief Remove a participant
/// @param participant The participant to remove
void Remove(Participant* participant);
private:
#if defined(NO_STD)
public:
Participant** GetAll() const;
int count = 0;
private:
Participant** participants;
#else
public:
/// @brief Get all participants
/// @return All participants
const std::list<Participant*>& GetAll() const;
private:
/// @brief The list of known participants
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
/// things. This class is used to register the things the participant is
@ -63,86 +12,53 @@ class ParticipantRegistry {
/// participant. It is used as a basis for the local participant, but also as a
/// reference to remote participants.
class Participant {
#pragma region Init
public:
/// @brief Create a generic participant
/// @brief The Ip Address of a participant. When the participant is local,
/// this contains 0.0.0.0
const char* ipAddress = "0.0.0.0";
/// @brief The port number for UDP communication with the participant. This is
/// 0 for isolated participants.
int port = 0;
/// @brief The network Id to identify the participant.
/// @note This field is likely to disappear in future versions
unsigned char networkId = 0;
/// @brief Default constructor
Participant();
/// @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
/// @param port The port of the participant
Participant(const char* ipAddress, int port);
/// @brief Destructor for the participant
~Participant();
/// @brief The local participant for this application
static Participant* LocalParticipant;
/// @brief Replace the local participant
/// @param newParticipant The new local Participant
static void ReplaceLocalParticipant(Participant& newParticipant);
virtual void Update(unsigned long currentTimeMs = 0);
#pragma endregion Init
#pragma region Properties
public:
/// @brief The name of the participant
const char* name = "Participant";
/// @brief The network Id to identify the participant
unsigned char networkId = 0;
/// @brief The root thing for this participant
Thing* root = nullptr;
public:
protected:
#if defined(NO_STD)
unsigned char thingCount = 0;
Thing* things[MAX_THING_COUNT];
#else
/// @brief The things managed by this participant
/// @brief The list of things managed by this participant
std::list<Thing*> things;
#endif
public:
/// @brief Find a thing managed by this participant
/// @param networkId The network ID of the thing
/// @param networkId The network ID for 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);
/// @return The thing if found or nullptr when no thing has been found
/// @note The use of the network ID is likely to disappear in future versions.
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
/// @param checkId Checks the thing ID of the thing. If it is 0, a new thing
/// Id will be assigned.
void Add(Thing* thing, bool checkId = true);
/// @brief Remove a thing for this participant
/// @param thing The thing to remove
void Remove(Thing* thing);
#pragma endregion Properties
#pragma region Update
public:
/// @brief Update all things for this participant
virtual void Update(bool recurse = true);
#pragma endregion Update
#pragma region Send
public:
char buffer[1024];
virtual bool Send(IMessage* msg);
#pragma endregion Send
// #pragma region Participant Registry
// public:
// static ParticipantRegistry registry;
// #pragma endregion Participant Registry
};
} // namespace RoboidControl

6
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
*/

2
Participants/IsolatedParticipant.h
View File

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

551
Participants/ParticipantUDP.cpp
View File

@ -1,184 +1,86 @@
#include "ParticipantUDP.h"
#include "Participant.h"
#include "Thing.h"
#include "Arduino/ArduinoParticipant.h"
#include "EspIdf/EspIdfParticipant.h"
#include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.h"
#include "Things/DifferentialDrive.h"
#include "Things/DistanceSensor.h"
#include "Things/TouchSensor.h"
#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
#include <ws2tcpip.h>
#include "Windows/WindowsParticipant.h"
#pragma comment(lib, "ws2_32.lib")
#elif defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <fcntl.h> // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#include <chrono>
#include "Posix/PosixParticipant.h"
#endif
#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;
ParticipantUDP::ParticipantUDP(int port) {
this->ipAddress = "0.0.0.0";
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) {
this->name = "ParticipantUDP";
this->remoteSite = nullptr;
if (this->port == 0)
this->isIsolated = true;
registry.Add(this);
this->root = Thing::LocalRoot(); //::LocalParticipant->root;
this->root->owner = this;
this->root->name = "UDP Root";
std::cout << "P2 " << (int)this->root << std::endl;
this->Add(this->root);
Participant::ReplaceLocalParticipant(*this);
}
ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
int port,
int localPort)
: RemoteParticipantUDP("127.0.0.1", localPort) {
this->name = "ParticipantUDP";
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
: Participant("127.0.0.1", localPort) {
if (this->port == 0)
this->isIsolated = true;
else
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 " << (int)this->root << std::endl;
this->Add(this->root);
Participant::ReplaceLocalParticipant(*this);
this->remoteSite = new Participant(ipAddress, port);
}
void ParticipantUDPGeneric::begin() {
if (this->isIsolated || this->remoteSite == nullptr)
static ParticipantUDP* isolatedParticipant = nullptr;
ParticipantUDP* ParticipantUDP::Isolated() {
if (isolatedParticipant == nullptr)
isolatedParticipant = new ParticipantUDP(0);
return isolatedParticipant;
}
void ParticipantUDP::begin() {
if (this->isIsolated)
return;
SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
}
#pragma endregion Init
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(localPort, remoteIpAddress, remotePort);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
#endif
this->connected = true;
}
#pragma region Update
// The update order
// 1. receive external messages
// 2. update the state
// 3. send out the updated messages
void ParticipantUDPGeneric::Update(bool recurse) {
unsigned long currentTimeMs = Thing::GetTimeMs();
void ParticipantUDP::Update(unsigned long currentTimeMs) {
if (currentTimeMs == 0)
currentTimeMs = Thing::GetTimeMs();
if (this->isIsolated == false) {
if (this->connected == false)
@ -186,101 +88,122 @@ void ParticipantUDPGeneric::Update(bool recurse) {
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
ParticipantMsg* msg = new ParticipantMsg(this->networkId);
if (this->remoteSite == nullptr)
this->Publish(msg);
else
this->Send(msg);
this->Send(this->remoteSite, msg);
delete msg;
this->nextPublishMe = currentTimeMs + this->publishInterval;
}
//this->ReceiveUDP();
this->ReceiveUDP();
}
UpdateMyThings();
UpdateOtherThings();
}
void ParticipantUDPGeneric::UpdateMyThings() {
for (Thing* thing : this->things) {
if (thing == nullptr) // || thing->GetParent() != nullptr)
if (thing == nullptr)
continue;
// 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);
if (thing->terminate)
this->Remove(thing);
}
}
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 : registry.GetAll()) {
#endif
if (participant == nullptr || participant == this)
continue;
// Call only the Participant version of the Update.
// This is to deal with the function where one of the (remote)
// participants is actually a local participant
participant->Participant::Update();
if (this->isIsolated)
continue;
for (Thing* thing : participant->things) {
PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
participant->Send(poseMsg);
std::cout << "Update thing " << (int)thing->id << std::endl;
if (this->isIsolated == false) {
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
this->Send(thing->owner, poseMsg);
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
this->Send(thing->owner, binaryMsg);
delete poseMsg;
BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
participant->Send(binaryMsg);
delete binaryMsg;
}
thing->Update(currentTimeMs, true);
}
}
// Update
#pragma endregion
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
}
Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) {
for (Participant* sender : this->senders) {
if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
return sender;
}
return nullptr;
}
Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
// std::cout << "New Participant " << ipAddress << ":" << port << "\n";
Participant* participant = new Participant(ipAddress, port);
#if defined(NO_STD)
participant->networkId = this->senderCount;
this->senders[this->senderCount++] = participant;
#else
participant->networkId = (unsigned char)this->senders.size();
this->senders.push_back(participant);
#endif
return participant;
}
#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);
this->Send(remoteParticipant, thingMsg);
delete thingMsg;
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
remoteParticipant->Send(nameMsg);
this->Send(remoteParticipant, nameMsg);
delete nameMsg;
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
remoteParticipant->Send(modelMsg);
this->Send(remoteParticipant, modelMsg);
delete modelMsg;
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
remoteParticipant->Send(poseMsg);
this->Send(remoteParticipant, poseMsg);
delete poseMsg;
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
remoteParticipant->Send(binaryMsg);
delete binaryMsg;
BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
this->Send(remoteParticipant, customMsg);
delete customMsg;
}
bool ParticipantUDPGeneric::Send(IMessage* msg) {
if (this->remoteSite != nullptr)
return this->remoteSite->Send(msg);
bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
return true;
return true;
#if defined(_WIN32) || defined(_WIN64)
Windows::ParticipantUDP* thisWindows =
static_cast<Windows::ParticipantUDP*>(this);
return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
return thisPosix->Send(remoteParticipant, bufferSize);
#elif defined(ARDUINO)
Arduino::ParticipantUDP* thisArduino =
static_cast<Arduino::ParticipantUDP*>(this);
return thisArduino->Send(remoteParticipant, bufferSize);
#elif defined(IDF_VER)
EspIdf::ParticipantUDP* thisEspIdf =
static_cast<EspIdf::ParticipantUDP*>(this);
return thisEspIdf->Send(remoteParticipant, bufferSize);
#else
return false;
#endif
}
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 +224,47 @@ void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) {
delete customMsg;
}
bool ParticipantUDP::Publish(IMessage* msg) {
#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) {
// std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
// << std::endl;
RemoteParticipantUDP* sender =
this->registry.Get(senderIpAddress, senderPort);
void ParticipantUDP::ReceiveData(unsigned char packetSize,
char* senderIpAddress,
unsigned int senderPort) {
Participant* sender = this->GetParticipant(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;
#endif
sender = this->AddParticipant(senderIpAddress, senderPort);
std::cout << "New remote participant " << sender->ipAddress << ":"
<< sender->port << std::endl;
}
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";
@ -336,8 +275,8 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
Process(sender, msg);
delete msg;
} break;
case NetworkIdMsg::id: {
NetworkIdMsg* msg = new NetworkIdMsg(this->buffer);
case SiteMsg::id: {
SiteMsg* msg = new SiteMsg(this->buffer);
bufferSize -= msg->length;
Process(sender, msg);
delete msg;
@ -377,113 +316,56 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
Process(sender, msg);
delete msg;
} break;
case TextMsg::id: {
TextMsg* msg = new TextMsg(this->buffer);
bufferSize -= msg->length + msg->textLength;
Process(sender, msg);
delete msg;
} break;
case DestroyMsg::id: {
DestroyMsg* msg = new DestroyMsg(this->buffer);
bufferSize -= msg->length;
Process(sender, msg);
delete msg;
} break;
};
// Check if the buffer has been read completely
#if !defined(NO_STD)
// Check if the buffer has been read completely
if (bufferSize > 0)
std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
#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, SiteMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
std::cout << this->name << ": process SiteMsg " << (int)this->networkId
<< " -> " << (int)msg->networkId << "\n";
#endif
if (this->networkId != msg->networkId) {
this->networkId = msg->networkId;
std::cout << this->things.size() << " things\n";
// std::cout << this->things.size() << " things\n";
for (Thing* thing : this->things)
this->SendThingInfo(sender, thing);
}
}
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;
@ -498,90 +380,41 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
nameLength); // Leave space for null terminator
#endif
thingName[nameLength] = '\0';
thing->SetName(thingName);
thing->name = thingName;
#if !defined(NO_STD)
std::cout << thing->GetName();
#endif
std::cout << thing->name;
}
#if !defined(NO_STD)
std::cout << std::endl;
#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) {
#if !defined(DEBUG) && !defined(NO_STD)
void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
#if defined(DEBUG)
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
<< "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
<< "/" << (int)msg->networkId << "]\n";
#endif
Participant* owner = registry.Get(msg->networkId);
if (owner == nullptr)
return;
Thing* thing = owner->Get(msg->networkId, msg->thingId);
if (thing == nullptr)
return;
if ((msg->poseType & PoseMsg::Pose_Position) != 0)
thing->SetPosition(msg->position);
if ((msg->poseType & PoseMsg::Pose_Orientation) != 0)
thing->SetOrientation(msg->orientation);
if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0)
thing->SetLinearVelocity(msg->linearVelocity);
if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0)
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";
<< "/" << (int)msg->thingId << "] ";
#endif
Participant* owner = registry.Get(msg->networkId);
if (owner != nullptr) {
Thing* thing = owner->Get(msg->networkId, msg->thingId);
if (thing != nullptr)
thing->ProcessBinary(msg->data);
#if !defined(NO_STD)
else {
#if defined(DEBUG)
std::cout << " unknown thing [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "]";
#endif
}
#endif
}
}
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 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->networkId, msg->thingId);
Thing* thing = sender->Get(msg->thingId);
if (thing != nullptr)
this->Remove(thing);
thing->ProcessBinary(msg->data);
else {
std::cout << " unknown thing [" << (int)msg->networkId << "/"
<< (int)msg->thingId << "]";
}
std::cout << std::endl;
}
// Receive

221
Participants/ParticipantUDP.h
View File

@ -1,14 +1,12 @@
#pragma once
#include "Messages/BinaryMsg.h"
#include "Messages/DestroyMsg.h"
#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/SiteMsg.h"
#include "Messages/ThingMsg.h"
#include "Participant.h"
@ -31,76 +29,7 @@ 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
/// @brief A local participant is the local device which can communicate with
/// other participants It manages all local things and communcation with other
/// participants. Each application has a local participant which is usually
/// explicit in the code. An participant can be isolated. In that case it is
@ -111,42 +40,52 @@ class ParticipantRegistry {
/// participant is created which can be obtained using
/// RoboidControl::IsolatedParticipant::Isolated().
/// @sa RoboidControl::Thing::Thing()
class ParticipantUDPGeneric : public RemoteParticipantUDP {
#pragma region Init
class ParticipantUDP : public Participant {
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.
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);
// Note to self: one cannot specify the port used by the local participant
// now!!
#pragma endregion Init
/// @brief Isolated participant is used when the application is run without
/// networking
/// @return A participant without networking support
static ParticipantUDP* Isolated();
#pragma region Properties
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;
/// The interval in milliseconds for publishing (broadcasting) data on the
/// local network
long publishInterval = 3000; // 3 seconds
protected:
#if !defined(ARDUINO)
/// @brief The name of the participant
const char* name = "ParticipantUDP";
// int localPort = 0;
/// @brief The remote site when this participant is connected to a site
Participant* remoteSite = nullptr;
#if defined(ARDUINO)
// const char* remoteIpAddress = nullptr;
// unsigned short remotePort = 0;
// char* broadcastIpAddress = nullptr;
// WiFiUDP udp;
#else
#if defined(__unix__) || defined(__APPLE__)
int sock;
#elif defined(_WIN32) || defined(_WIN64)
@ -154,73 +93,81 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
sockaddr_in server_addr;
sockaddr_in broadcast_addr;
#endif
#endif
public:
void begin();
bool connected = false;
#pragma endregion Properties
#pragma region Update
public:
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 UpdateMyThings();
virtual void UpdateOtherThings();
#pragma endregion Update
#pragma region Send
virtual void Update(unsigned long currentTimeMs = 0) override;
void SendThingInfo(Participant* remoteParticipant, Thing* thing);
void PublishThingInfo(Thing* thing);
virtual bool Send(IMessage* msg) override;
virtual bool Publish(IMessage* msg) = 0;
bool Send(Participant* remoteParticipant, IMessage* msg);
bool Publish(IMessage* msg);
#pragma endregion Send
#pragma region Receive
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;
#if defined(NO_STD)
unsigned char senderCount = 0;
Participant* senders[MAX_SENDER_COUNT];
#else
std::list<Participant*> senders;
#endif
virtual void ReceiveUDP() = 0;
protected:
unsigned long nextPublishMe = 0;
virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg);
virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg);
char buffer[1024];
virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg);
virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender,
ThingMsg* msg,
bool isRemote);
void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
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);
Participant* GetParticipant(const char* ipAddress, int port);
Participant* AddParticipant(const char* ipAddress, int port);
#pragma endregion Receive
void ReceiveUDP();
public:
static ParticipantRegistry registry;
virtual void Process(Participant* sender, ParticipantMsg* msg);
virtual void Process(Participant* sender, SiteMsg* 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);
#if !defined(NO_STD)
// public:
// using ThingConstructor = std::function<Thing*(Participant* participant,
// unsigned char networkId,
// unsigned char thingId)>;
// template <typename ThingClass>
// void Register(unsigned char thingType) {
// thingMsgProcessors[thingType] = [](Participant* participant,
// unsigned char networkId,
// unsigned char thingId) {
// return new ThingClass(participant, networkId, thingId);
// };
// };
// template <typename ThingClass>
// void Register2(unsigned char thingType, ThingConstructor f) {
// thingMsgProcessors[thingType] = [f](Participant* participant,
// unsigned char networkId,
// unsigned char thingId) {
// return f(participant, networkId, thingId);
// };
// };
// protected:
// std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
#endif
};
} // namespace RoboidControl
#include "EspIdf/EspIdfParticipant.h"

95
Participants/SiteServer.cpp
View File

@ -9,89 +9,58 @@
namespace RoboidControl {
#pragma region Init
SiteServer::SiteServer(int port) : ParticipantUDPGeneric(port) {
SiteServer::SiteServer(int port) {
this->name = "Site Server";
this->publishInterval = 0;
//SetupUDP(port, ipAddress, 0);
}
this->ipAddress = "0.0.0.0";
this->port = port;
#pragma endregion Init
#pragma region Update
void SiteServer::UpdateMyThings() {
for (Thing* thing : this->things) {
if (thing == nullptr)
continue;
thing->Update(true);
if (this->isIsolated == false) {
// Send to all other participants
#if defined(NO_STD)
Participant** participants = Participant::registry.GetAll();
for (int ix = 0; ix < Participant::registry.count; ix++) {
Participant* participant = participants[ix];
this->senders[this->senderCount++] = this;
#else
for (Participant* participant : registry.GetAll()) {
this->senders.push_back(this);
#endif
if (participant == nullptr || participant == this)
continue;
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
participant->Send(poseMsg);
delete poseMsg;
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
participant->Send(binaryMsg);
delete binaryMsg;
}
}
}
SetupUDP(port, ipAddress, 0);
#if !defined(NO_STD)
// Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
#endif
}
#pragma endregion Update
#pragma region Receive
void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
if (msg->networkId != sender->networkId) {
void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
if (msg->networkId == 0) {
// std::cout << this->name << " received New Client -> " <<
// sender->ipAddress
// << ":" << (int)sender->port << "\n";
NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
sender->Send(msg);
SiteMsg* msg = new SiteMsg(sender->networkId);
this->Send(sender, msg);
delete msg;
}
}
void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {}
void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) {
Thing* thing = sender->Get(msg->networkId, msg->thingId);
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);
// #if defined(NO_STD)
new Thing(sender, (Thing::Type)msg->thingType,
msg->thingId);
// #else
// auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
// //Thing* newThing;
// if (thingMsgProcessor != thingMsgProcessors.end()) // found item
// //newThing =
// thingMsgProcessor->second(sender, msg->networkId,
// msg->thingId);
// else
// //newThing =
// new Thing(sender, msg->networkId, msg->thingId,
// (Thing::Type)msg->thingType);
// #endif
}
thing->id = msg->thingId;
if (msg->parentId != 0) {
thing->SetParent(Get(msg->networkId, msg->parentId));
if (thing->IsRoot())
// if (thing->GetParent() != nullptr)
#if defined(NO_STD)
;
#else
std::cout << "Could not find parent [" << (int)msg->networkId << "/"
<< (int)msg->parentId << "]\n";
#endif
} else
thing->SetParent(Thing::LocalRoot());
}
#pragma endregion Receive
} // namespace RoboidControl

50
Participants/SiteServer.h
View File

@ -2,43 +2,45 @@
#include "ParticipantUDP.h"
// #if !defined(NO_STD)
// #include <functional>
// #include <memory>
// #include <unordered_map>
// #endif
#if !defined(NO_STD)
#include <functional>
#include <memory>
#include <unordered_map>
#endif
namespace RoboidControl {
/// @brief A participant is device which can communicate with other participants
class SiteServer : public ParticipantUDPGeneric {
#pragma region Init
class SiteServer : public ParticipantUDP {
public:
/// @brief Create a new site server
/// @param port The port of which to receive the messages
SiteServer(int port = 7681);
#pragma endregion Init
// virtual void Update(unsigned long currentTimeMs = 0) override;
#pragma region Update
virtual void UpdateMyThings() override;
#pragma endregion Update
#pragma region Receive
// #if !defined(NO_STD)
// template <typename ThingClass>
// void Register(unsigned char thingType) {
// thingMsgProcessors[thingType] = [](Participant* participant,
// unsigned char networkId,
// unsigned char thingId) {
// return new ThingClass(participant, networkId, thingId);
// };
// };
// #endif
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;
#pragma endregion Receive
virtual void Process(Participant* sender, ParticipantMsg* msg) override;
virtual void Process(Participant* sender, SiteMsg* msg) override;
virtual void Process(Participant* sender, ThingMsg* msg) override;
// #if !defined(NO_STD)
// using ThingConstructor = std::function<Thing*(Participant* participant,
// unsigned char networkId,
// unsigned char thingId)>;
// std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
// #endif
};
} // namespace RoboidControl

17
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,8 +9,9 @@
#endif
namespace RoboidControl {
namespace Posix {
void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
void Setup(int localPort, const char* remoteIpAddress, int remotePort) {
#if defined(__unix__) || defined(__APPLE__)
// Create a UDP socket
@ -63,7 +63,7 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
#endif
}
void ParticipantUDP::Receive() {
void Receive() {
#if defined(__unix__) || defined(__APPLE__)
sockaddr_in client_addr;
socklen_t len = sizeof(client_addr);
@ -74,9 +74,9 @@ void ParticipantUDP::Receive() {
unsigned int sender_port = ntohs(client_addr.sin_port);
ReceiveData(packetSize, sender_ipAddress, sender_port);
// RoboidControl::Participant* remoteParticipant = this->Get(sender_ipAddress, sender_port);
// RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
// if (remoteParticipant == nullptr) {
// remoteParticipant = this->Add(sender_ipAddress, sender_port);
// remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
// // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
// // << "\n";
// // std::cout << "New remote participant " << remoteParticipant->ipAddress
@ -90,7 +90,7 @@ void ParticipantUDP::Receive() {
#endif
}
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
bool Send(Participant* remoteParticipant, int bufferSize) {
#if defined(__unix__) || defined(__APPLE__)
// std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
// << "\n";
@ -113,7 +113,7 @@ bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferS
return true;
}
bool ParticipantUDP::Publish(IMessage* msg) {
bool Publish(IMessage* msg) {
#if defined(__unix__) || defined(__APPLE__)
int bufferSize = msg->Serialize(this->buffer);
if (bufferSize <= 0)
@ -132,6 +132,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
return true;
}
} // namespace Posix
} // namespace RoboidControl
#endif

15
Posix/PosixParticipant.h
View File

@ -1,24 +1,17 @@
#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:
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);
protected:
#if defined(__unix__) || defined(__APPLE__)
sockaddr_in remote_addr;
sockaddr_in server_addr;
sockaddr_in broadcast_addr;
#endif
};
} // namespace Posix
} // namespace RoboidControl
#endif

59
README.md
View File

@ -14,60 +14,5 @@ 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::LocalParticipant
- RoboidControl::SiteServer

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

242
Thing.cpp
View File

@ -5,7 +5,6 @@
#include "Participants/IsolatedParticipant.h"
#include <string.h>
// #include <iostream>
#if defined(ARDUINO)
#include "Arduino.h"
@ -18,78 +17,72 @@
namespace RoboidControl {
#pragma region Init
// LocalParticipant* Thing::CheckHiddenParticipant() {
// if (isolatedParticipant == nullptr)
// isolatedParticipant = new LocalParticipant(0);
// return isolatedParticipant;
// }
Thing* Thing::LocalRoot() {
Participant* p = Participant::LocalParticipant;
Thing* localRoot = p->root;
return localRoot;
}
Thing::Thing(int thingType)
: Thing(IsolatedParticipant::Isolated(), thingType) {}
// Only use this for root things
Thing::Thing(Participant* owner) {
this->type = Type::Root;
this->name = "Root";
this->position = Spherical::zero;
this->positionUpdated = true;
this->orientation = SwingTwist::identity;
this->orientationUpdated = true;
this->hierarchyChanged = true;
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
Thing::Thing(Participant* owner, Type thingType, unsigned char thingId)
: Thing(owner, (unsigned char)thingType, thingId) {}
Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
this->owner = owner;
this->owner->Add(this);
std::cout << this->owner->name << ": New root thing " << std::endl;
}
void Thing::CreateRoot(Participant* owner) {
owner->root = new Thing(owner);
}
Thing::Thing(Thing* parent) {
this->type = Type::Undetermined;
this->id = thingId;
this->type = thingType;
this->networkId = 0;
this->position = Spherical::zero;
this->positionUpdated = true;
this->orientation = SwingTwist::identity;
this->orientationUpdated = true;
this->hierarchyChanged = true;
this->linearVelocity = Spherical::zero;
this->angularVelocity = Spherical::zero;
this->owner = parent->owner;
// std::cout << "add thing [" << (int)this->id << "] to owner "
// << this->owner->ipAddress << ":" << this->owner->port << std::endl;
this->owner->Add(this, true);
this->SetParent(parent);
std::cout << this->owner->name << ": New thing for " << parent->name
<< std::endl;
}
Thing::~Thing() {
std::cout << "Destroy thing " << this->name << std::endl;
// Thing::Thing(Participant* owner,
// Type thingType,
// int thingId) {
// // no participant reference yet..
// this->owner = owner;
// this->networkId = networkId;
// this->id = thingId;
// this->type = (unsigned char)thingType;
// this->linearVelocity = Spherical::zero;
// this->angularVelocity = Spherical::zero;
// // std::cout << "Created thing " << (int)this->networkId << "/" <<
// // (int)this->id
// // << "\n";
// owner->Add(this, false);
// }
void Thing::Terminate() {
// Thing::Remove(this);
}
#pragma endregion Init
Thing* Thing::FindThing(const char* name) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx];
if (child == nullptr || child->name == nullptr)
continue;
void Thing::SetName(const char* name) {
this->name = name;
this->nameChanged = true;
if (strcmp(child->name, name) == 0)
return child;
Thing* foundChild = child->FindThing(name);
if (foundChild != nullptr)
return foundChild;
}
return nullptr;
}
const char* Thing::GetName() const {
return this->name;
}
void Thing::SetModel(const char* url) {
this->modelUrl = url;
}
#pragma region Hierarchy
void Thing::SetParent(Thing* parent) {
if (parent == nullptr) {
Thing* parentThing = this->parent;
@ -98,21 +91,18 @@ void Thing::SetParent(Thing* parent) {
this->parent = nullptr;
} else
parent->AddChild(this);
this->hierarchyChanged = true;
}
bool Thing::IsRoot() const {
return this == LocalRoot() || this->parent == nullptr;
void Thing::SetParent(Thing* root, const char* name) {
Thing* thing = root->FindThing(name);
if (thing != nullptr)
this->SetParent(thing);
}
Thing* Thing::GetParent() {
return this->parent;
}
Thing* Thing::GetChildByIndex(unsigned char ix) {
return this->children[ix];
}
void Thing::AddChild(Thing* child) {
unsigned char newChildCount = this->childCount + 1;
Thing** newChildren = new Thing*[newChildCount];
@ -152,7 +142,7 @@ Thing* Thing::RemoveChild(Thing* child) {
}
}
child->parent = Thing::LocalRoot();
child->parent = nullptr;
delete[] this->children;
this->children = newChildren;
@ -161,7 +151,7 @@ Thing* Thing::RemoveChild(Thing* child) {
return child;
}
Thing* Thing::GetChild(unsigned char id, bool recurse) {
Thing* Thing::GetChild(unsigned char id, bool recursive) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx];
if (child == nullptr)
@ -169,8 +159,8 @@ Thing* Thing::GetChild(unsigned char id, bool recurse) {
if (child->id == id)
return child;
if (recurse) {
Thing* foundChild = child->GetChild(id, recurse);
if (recursive) {
Thing* foundChild = child->GetChild(id, recursive);
if (foundChild != nullptr)
return foundChild;
}
@ -178,25 +168,57 @@ Thing* Thing::GetChild(unsigned char id, bool recurse) {
return nullptr;
}
Thing* Thing::FindChild(const char* name, bool recurse) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx];
if (child == nullptr || child->name == nullptr)
continue;
if (strcmp(child->name, name) == 0)
return child;
Thing* foundChild = child->FindChild(name);
if (foundChild != nullptr)
return foundChild;
}
return nullptr;
Thing* Thing::GetChildByIndex(unsigned char ix) {
return this->children[ix];
}
#pragma endregion Hierarchy
void Thing::SetModel(const char* url) {
this->modelUrl = url;
}
#pragma region Pose
unsigned long Thing::GetTimeMs() {
#if defined(ARDUINO)
return millis();
#else
auto now = std::chrono::steady_clock::now();
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
now.time_since_epoch());
return static_cast<unsigned long>(ms.count());
#endif
}
void Thing::Update(bool recursive) {
Update(GetTimeMs(), recursive);
}
void Thing::Update(unsigned long currentTimeMs, bool recursive) {
// if (this->positionUpdated || this->orientationUpdated)
// OnPoseChanged callback
this->positionUpdated = false;
this->orientationUpdated = false;
if (recursive) {
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
Thing* child = this->children[childIx];
if (child == nullptr)
continue;
child->Update(currentTimeMs, recursive);
}
}
}
void Thing::UpdateThings(unsigned long currentTimeMs) {
IsolatedParticipant::Isolated()->Update(currentTimeMs);
}
int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
(void)buffer;
(void)ix;
return 0;
}
void Thing::ProcessBinary(char* bytes) {
(void)bytes;
};
void Thing::SetPosition(Spherical position) {
this->position = position;
@ -216,10 +238,8 @@ SwingTwist Thing::GetOrientation() {
}
void Thing::SetLinearVelocity(Spherical linearVelocity) {
if (this->linearVelocity.distance != linearVelocity.distance) {
this->linearVelocity = linearVelocity;
this->linearVelocityUpdated = true;
}
this->linearVelocity = linearVelocity;
this->linearVelocityUpdated = true;
}
Spherical Thing::GetLinearVelocity() {
@ -227,60 +247,12 @@ Spherical Thing::GetLinearVelocity() {
}
void Thing::SetAngularVelocity(Spherical angularVelocity) {
if (this->angularVelocity.distance != angularVelocity.distance) {
this->angularVelocity = angularVelocity;
this->angularVelocityUpdated = true;
}
this->angularVelocity = angularVelocity;
this->angularVelocityUpdated = true;
}
Spherical Thing::GetAngularVelocity() {
return this->angularVelocity;
}
#pragma endregion Pose
#pragma region Update
unsigned long Thing::GetTimeMs() {
#if defined(ARDUINO)
unsigned long ms = millis();
return ms;
#else
auto now = std::chrono::steady_clock::now();
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
now.time_since_epoch());
return static_cast<unsigned long>(ms.count());
#endif
}
void Thing::Update(bool recursive) {
this->positionUpdated = false;
this->orientationUpdated = false;
this->linearVelocityUpdated = false;
this->angularVelocityUpdated = false;
this->hierarchyChanged = false;
this->nameChanged = false;
if (recursive) {
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)
continue;
child->Update(recursive);
}
}
}
#pragma endregion Update
int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
(void)buffer;
(void)ix;
return 0;
}
void Thing::ProcessBinary(char* bytes) {
(void)bytes;
};
} // namespace RoboidControl

244
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
@ -20,122 +20,71 @@ class ParticipantUDPGeneric;
class Thing {
public:
/// @brief Predefined thing types
struct Type {
static const unsigned char Undetermined = 0x00;
// Sensor
static const unsigned char Switch = 0x01;
static const unsigned char DistanceSensor = 0x02;
static const unsigned char DirectionalSensor = 0x03;
static const unsigned char TemperatureSensor = 0x04;
static const unsigned char TouchSensor = 0x05;
// Motor
static const unsigned char ControlledMotor = 0x06;
static const unsigned char UncontrolledMotor = 0x07;
static const unsigned char Servo = 0x08;
static const unsigned char RelativeEncoder = 0x19;
enum Type {
Undetermined,
// Sensor,
Switch,
DistanceSensor,
DirectionalSensor,
TemperatureSensor,
TouchSensor,
// Motor,
ControlledMotor,
UncontrolledMotor,
Servo,
// Other
static const unsigned char Root = 0x10;
static const unsigned char Roboid = 0x09;
static const unsigned char Humanoid = 0x0A;
static const unsigned char ExternalSensor = 0x08;
static const unsigned char Animator = 0x0C;
static const unsigned char DifferentialDrive = 0x0D;
Roboid,
Humanoid,
ExternalSensor,
};
#pragma region Init
/// @brief Create a new thing using an implicit local participant
/// @param thingType The type of thing
Thing(int thingType = Type::Undetermined);
/// @brief Create a new thing of the given type
/// @param thingType The predefined type of thing
Thing(Participant* participant,
Type thingType = Type::Undetermined,
unsigned char thingId = 0);
/// @brief Create a new thing of the give type
/// @param thingType The custom type of the thing
Thing(Participant* participant, int thingType, unsigned char thingId = 0);
/// @brief Create a new thing for the given participant
/// @param participant The participant for which this thing is created
/// @param networkId The network ID of the thing
/// @param thingId The ID of the thing
/// @param thingType The type of thing
// Thing(Participant* participant,
// unsigned char networkId,
// unsigned char thingId,
// Type thingType = Type::Undetermined);
private:
public:
/// @brief Create a new Thing
/// @param parent (optional) The parent thing
/// The owner will be the same as the owner of the parent thing, it will
/// be Participant::LocalParticipant if the parent is not specified. A thing
/// without a parent will be connected to the root thing.
Thing(Thing* parent = LocalRoot());
private:
/// @brief Constructor to create a root thing
/// @param owner The participant who will own this root thing
/// @remarks This function is private because CreateRoot() should be used
/// instead
Thing(Participant* owener);
public:
/// @brief Destructor for a Thing
~Thing();
/// @brief Create a root thing for a participant
/// @param owner The participant who will own this root thing
static void CreateRoot(Participant* owner);
/// @brief The root thing for the local participant
/// @return The root thing for the local participant
static Thing* LocalRoot();
#pragma endregion Init
public:
/// @brief Terminated things are no longer updated
bool terminate = false;
#pragma region Properties
public:
/// @brief The participant managing this thing
Participant* owner;
/// @brief The network ID of this thing
/// @note This field will likely disappear in future versions
unsigned char networkId = 0;
/// @brief The ID of the thing
unsigned char id = 0;
/// @brief The type of Thing
/// This can be either a Thing::Type of a byte value for custom types
unsigned char type = Type::Undetermined;
unsigned char type = 0;
/// @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 Find a thing by name
/// @param name Rhe name of the thing
/// @return The found thing or nullptr when nothing is found
Thing* FindThing(const char* name);
/// @brief The participant owning this thing
Participant* owner = nullptr;
/// @brief The name of the thing
const char* name = nullptr;
void SetName(const char* name);
const char* GetName() const;
bool nameChanged = false;
/// @brief Sets the location from where the 3D model of this Thing can be
/// loaded from
/// @param url The url of the model
/// @remark Although the roboid implementation is not dependent on the model,
/// the only official supported model formats are .png (sprite), .gltf and .glb
void SetModel(const char* url);
/// @brief An URL pointing to the location where a model of the thing can be
/// found
const char* modelUrl = nullptr;
#pragma endregion Properties
#pragma region Hierarchy
/// @brief Sets the parent of this Thing
/// @param parent The Thing which should become the parent
void SetParent(Thing* parent);
/// @brief Gets the parent of this Thing
/// @brief Sets the parent Thing
/// @param parent The Thing which should become the parnet
/// @remark This is equivalent to calling parent->AddChild(this);
virtual void SetParent(Thing* parent);
void SetParent(Thing* root, const char* name);
/// @brief Gets the parent Thing
/// @return The parent Thing
Thing* GetParent();
/// @brief Check if this is a root thing
/// @return True is this thing is a root
bool IsRoot() const;
/// @brief The number of children
unsigned char childCount = 0;
/// @brief Get a child by index
/// @param ix The child index
/// @return The found thing of nullptr when nothing is found
Thing* GetChildByIndex(unsigned char ix);
/// @brief Add a child Thing to this Thing
/// @param child The Thing which should become a child
/// @remark When the Thing is already a child, it will not be added again
@ -145,46 +94,49 @@ class Thing {
/// @return The removed child or nullptr if the child could not be found
Thing* RemoveChild(Thing* child);
/// @brief The number of children
unsigned char childCount = 0;
/// @brief Get a child by thing Id
/// @param id The thing ID to find
/// @param recurse Look recursively through all descendants
/// @param recursive Look recursively through all descendants
/// @return The found thing of nullptr when nothing is found
Thing* GetChild(unsigned char id, bool recurse = false);
Thing* GetChild(unsigned char id, bool recursive = false);
/// @brief Get a child by index
/// @param ix The child index
/// @return The found thing of nullptr when nothing is found
Thing* GetChildByIndex(unsigned char ix);
/// @brief Find a thing by name
/// @param name The name of the thing
/// @param recurse Look recursively through all descendants
/// @return The found thing or nullptr when nothing is found
Thing* FindChild(const char* name, bool recurse = true);
/// @brief Indicator that the hierarchy of the thing has changed
bool hierarchyChanged = true;
private:
protected:
Thing* parent = nullptr;
Thing** children = nullptr;
#pragma endregion Hierarchy
#pragma region Pose
public:
/// @brief The name of the thing
const char* name = nullptr;
/// @brief An URL pointing to the location where a model of the thing can be
/// found
const char* modelUrl = nullptr;
/// @brief The scale of the model (deprecated I think)
float modelScale = 1;
/// @brief Set the position of the thing
/// @param position The new position in local space, in meters
void SetPosition(Spherical position);
/// @brief Get the position of the thing
/// @return The position in local space, in meters
Spherical GetPosition();
/// @brief Boolean indicating that the thing has an updated position
bool positionUpdated = false;
/// @brief Set the orientation of the thing
/// @param orientation The new orientation in local space
void SetOrientation(SwingTwist orientation);
/// @brief Get the orientation of the thing
/// @return The orienation in local space
SwingTwist GetOrientation();
/// @brief Boolean indicating the thing has an updated orientation
/// @brief The scale of the thing (deprecated I think)
// float scale = 1; // assuming uniform scale
/// @brief boolean indicating if the position was updated
bool positionUpdated = false;
/// @brief boolean indicating if the orientation was updated
bool orientationUpdated = false;
/// @brief Set the linear velocity of the thing
@ -194,59 +146,57 @@ class Thing {
/// @brief Get the linear velocity of the thing
/// @return The linear velocity in local space, in meters per second
virtual Spherical GetLinearVelocity();
/// @brief Boolean indicating the thing has an updated linear velocity
bool linearVelocityUpdated = false;
/// @brief Set the angular velocity of the thing
/// @param angularVelocity the new angular velocity in local space
virtual void SetAngularVelocity(Spherical angularVelocity);
/// @brief Get the angular velocity of the thing
/// @return The angular velocity in local space
virtual Spherical GetAngularVelocity();
/// @brief Boolean indicating the thing has an updated angular velocity
bool linearVelocityUpdated = false;
bool angularVelocityUpdated = false;
private:
/// @brief The position of the thing in local space, in meters
/// @brief The position in local space
/// @remark When this Thing has a parent, the position is relative to the
/// parent's position and orientation
Spherical position;
/// @brief The orientation of the thing in local space
/// @brief The orientation in local space
/// @remark When this Thing has a parent, the orientation is relative to the
/// parent's orientation
SwingTwist orientation;
/// @brief The linear velocity of the thing in local space, in meters per
/// second
/// @brief The linear velocity in local space
Spherical linearVelocity;
/// @brief The angular velocity of the thing in local space, in degrees per
/// second
/// @brief The angular velocity in local spze
Spherical angularVelocity;
#pragma endregion Pose
#pragma region Update
public:
//virtual void PrepareForUpdate();
/// @brief Terminated things are no longer updated
void Terminate();
/// @brief Updates the state of the thing
/// @param recurse When true, this will Update the descendants recursively
virtual void Update(bool recurse = false);
/// @brief Sets the location from where the 3D model of this Thing can be
/// loaded from
/// @param url The url of the model
/// @remark Although the roboid implementation is not dependent on the model,
/// the only official supported model format is .obj
void SetModel(const char* url);
/// @brief Get the current time in milliseconds
/// @return The current time in milliseconds
static unsigned long GetTimeMs();
#pragma endregion Update
void Update(bool recursive = false);
/// @brief Updates the state of the thing
/// @param currentTimeMs The current clock time in milliseconds
virtual void Update(unsigned long currentTimeMs, bool recursive = false);
static void UpdateThings(unsigned long currentTimeMs);
public:
/// @brief Function used to generate binary data for this thing
/// @param buffer The byte array for thw binary data
/// @param ix The starting position for writing the binary data
/// @return The size of the binary data
/// @returns The size of the binary data
virtual int GenerateBinary(char* buffer, unsigned char* ix);
/// @brief Function used to process binary data received for this thing
// /// @brief FUnction used to process binary data received for this thing
/// @param bytes The binary data
virtual void ProcessBinary(char* bytes);
};

68
Things/ControlledMotor.cpp
View File

@ -1,68 +0,0 @@
#include "ControlledMotor.h"
#include "LinearAlgebra/FloatSingle.h"
namespace RoboidControl {
ControlledMotor::ControlledMotor(Motor* motor,
RelativeEncoder* encoder,
Thing* parent)
: Motor(parent), motor(motor), encoder(encoder) {
this->type = Type::ControlledMotor;
//encoder->SetParent(null);
// Thing parent = motor.GetParent();
// this->SetParent(parent);
this->integral = 0;
}
void ControlledMotor::SetTargetVelocity(float velocity) {
this->targetVelocity = velocity;
this->rotationDirection =
(targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
}
void ControlledMotor::Update(bool recurse) {
unsigned long currentTimeMs = GetTimeMs();
float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
this->lastUpdateTime = currentTimeMs;
if (timeStep <= 0)
return;
// encoder->Update(false);
this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
float error = this->targetVelocity - this->actualVelocity;
float p_term = error * pidP;
this->integral += error * timeStep;
float i_term = pidI * this->integral;
float d_term = pidD * (error - this->lastError) / timeStep;
this->lastError = error;
float output = p_term + i_term + d_term;
// std::cout << "target " << this->targetVelocity << " actual "
// << this->actualVelocity << " output = " << output << std::endl;
// float acceleration =
// error * timeStep * pidP; // Just P is used at this moment
// std::cout << "motor acc. " << acceleration << std::endl;
// float newTargetVelocity = motor->targetVelocity + acceleration;
output = LinearAlgebra::Float::Clamp(output, -1, 1);
motor->SetTargetVelocity(output); // or something like that
//motor->Update(false);
}
// float ControlledMotor::GetActualVelocity() {
// return (int)rotationDirection * encoder->rotationSpeed;
// }
// bool ControlledMotor::Drive(float distance) {
// if (!driving) {
// targetDistance = distance;
// startDistance = encoder->GetDistance();
// driving = true;
// }
// float totalDistance = encoder->GetDistance() - startDistance;
// bool completed = totalDistance > targetDistance;
// return completed;
// }
} // namespace RoboidControl

40
Things/ControlledMotor.h
View File

@ -1,40 +0,0 @@
#pragma once
#include "Motor.h"
#include "RelativeEncoder.h"
namespace RoboidControl {
/// @brief A motor with speed control
/// It uses a feedback loop from an encoder to regulate the speed
/// The speed is measured in revolutions per second.
class ControlledMotor : public Motor {
public:
ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());
float pidP = 0.5;
float pidD = 0;
float pidI = 0.2;
/// @brief The actual velocity in revolutions per second
float actualVelocity;
enum Direction { Forward = 1, Reverse = -1 };
Direction rotationDirection;
virtual void Update(bool recurse = false) override;
/// @brief Set the target verlocity for the motor controller
/// @param speed the target velocity in revolutions per second
virtual void SetTargetVelocity(float velocity) override;
Motor* motor;
RelativeEncoder* encoder;
protected:
float integral = 0;
float lastError = 0;
float lastUpdateTime;
};
} // namespace RoboidControl

108
Things/DifferentialDrive.cpp
View File

@ -1,29 +1,10 @@
#include "DifferentialDrive.h"
#include "Messages/LowLevelMessages.h"
namespace RoboidControl {
DifferentialDrive::DifferentialDrive() : Thing() {}
DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent) {
this->type = Type::DifferentialDrive;
this->name = "Differential drive";
this->leftWheel = new Motor(this);
this->leftWheel->name = "Left motor";
this->rightWheel = new Motor(this);
this->rightWheel->name = "Right motor";
}
DifferentialDrive::DifferentialDrive(Motor* leftMotor,
Motor* rightMotor,
Thing* parent)
: Thing(parent) {
this->type = Type::DifferentialDrive;
this->name = "Differential drive";
this->leftWheel = leftMotor;
this->rightWheel = rightMotor;
}
RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
: Thing(participant) {}
void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
float wheelSeparation) {
@ -40,66 +21,51 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}
// Motor& DifferentialDrive::GetMotorLeft() {
// return *this->leftWheel;
// }
// Motor& DifferentialDrive::GetMotorRight() {
// return *this->rightWheel;
// }
void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
float distance = this->wheelSeparation / 2;
this->leftWheel = &leftMotor;
this->leftWheel->SetPosition(Spherical(distance, Direction::left));
this->leftWheel = leftWheel;
;
if (leftWheel != nullptr)
this->leftWheel->SetPosition(Spherical(distance, Direction::left));
this->rightWheel = &rightMotor;
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
this->rightWheel = rightWheel;
if (rightWheel != nullptr)
this->rightWheel->SetPosition(Spherical(distance, Direction::right));
}
void DifferentialDrive::SetWheelVelocity(float velocityLeft,
float velocityRight) {
// if (this->leftWheel != nullptr)
// this->leftWheel->SetAngularVelocity(Spherical(velocityLeft,
// Direction::left));
// if (this->rightWheel != nullptr)
// this->rightWheel->SetAngularVelocity(
// Spherical(velocityRight, Direction::right));
void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
if (this->leftWheel != nullptr)
this->leftWheel->SetTargetVelocity(velocityLeft);
this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
if (this->rightWheel != nullptr)
this->rightWheel->SetTargetVelocity(velocityRight);
this->rightWheel->SetAngularVelocity(
Spherical(speedRight, Direction::right));
}
void DifferentialDrive::Update(bool recursive) {
if (this->linearVelocityUpdated) {
// this assumes forward velocity only....
float linearVelocity = this->GetLinearVelocity().distance;
void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
if (this->linearVelocityUpdated == false)
return;
// this assumes forward velocity only....
float linearVelocity = this->GetLinearVelocity().distance;
Spherical angularVelocity = this->GetAngularVelocity();
float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec
// Determine the rotation direction
if (angularVelocity.direction.horizontal.InDegrees() < 0)
angularSpeed = -angularSpeed;
Spherical angularVelocity = this->GetAngularVelocity();
float angularSpeed = angularVelocity.distance * Deg2Rad; // in degrees/sec
// Determine the rotation direction
if (angularVelocity.direction.horizontal.InDegrees() < 0)
angularSpeed = -angularSpeed;
// wheel separation can be replaced by this->leftwheel->position->distance
float speedLeft =
(linearVelocity + angularSpeed * this->wheelSeparation / 2) /
this->wheelRadius * Rad2Deg;
float speedRight =
(linearVelocity - angularSpeed * this->wheelSeparation / 2) /
this->wheelRadius * Rad2Deg;
// wheel separation can be replaced by this->leftwheel->position->distance
float speedLeft =
(linearVelocity + angularSpeed * this->wheelSeparation / 2) /
this->wheelRadius * Rad2Deg;
float speedRight =
(linearVelocity - angularSpeed * this->wheelSeparation / 2) /
this->wheelRadius * Rad2Deg;
this->SetWheelVelocity(speedLeft, speedRight);
}
Thing::Update(recursive);
}
int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
data[(*ix)++] = this->leftWheel->id;
data[(*ix)++] = this->rightWheel->id;
LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
return 4;
this->SetWheelVelocity(speedLeft, speedRight);
Thing::Update(currentMs, recursive);
// std::cout << "lin. speed " << linearVelocity << " ang. speed " <<
// angularVelocity.distance << " left wheel "
// << speedLeft << " right wheel " << speedRight << "\n";
}
} // namespace RoboidControl

37
Things/DifferentialDrive.h
View File

@ -1,7 +1,6 @@
#pragma once
#include "Thing.h"
#include "Motor.h"
namespace RoboidControl {
@ -10,13 +9,11 @@ namespace RoboidControl {
/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
class DifferentialDrive : public Thing {
public:
/// @brief Create a new child differential drive
/// @param parent The parent thing
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
DifferentialDrive(Thing* parent = Thing::LocalRoot());
DifferentialDrive(Motor* leftMotor, Motor* rightMotor, Thing* parent = Thing::LocalRoot());
/// @brief Create a differential drive without networking support
DifferentialDrive();
/// @brief Create a differential drive with networking support
/// @param participant The local participant
DifferentialDrive(Participant* participant);
/// @brief Configures the dimensions of the drive
/// @param wheelDiameter The diameter of the wheels in meters
@ -26,41 +23,35 @@ class DifferentialDrive : public Thing {
/// linear and angular velocity.
/// @sa SetLinearVelocity SetAngularVelocity
void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
// Motor& GetMotorLeft();
// Motor& GetMotorRight();
/// @brief Congures the motors for the wheels
/// @param leftWheel The motor for the left wheel
/// @param rightWheel The motor for the right wheel
void SetMotors(Motor& leftMotor, Motor& rightMotor);
void SetMotors(Thing* leftWheel, Thing* rightWheel);
/// @brief Directly specify the speeds of the motors
/// @param speedLeft The speed of the left wheel in degrees per second.
/// Positive moves the robot in the forward direction.
/// @param speedRight The speed of the right wheel in degrees per second.
/// Positive moves the robot in the forward direction.
void SetWheelVelocity(float speedLeft, float speedRight);
/// @copydoc RoboidControl::Thing::Update(unsigned long)
virtual void Update(bool recursive = true) override;
int GenerateBinary(char* bytes, unsigned char* ix) override;
// virtual void ProcessBinary(char* bytes) override;
/// @brief The left wheel
Motor* leftWheel = nullptr;
/// @brief The right wheel
Motor* rightWheel = nullptr;
virtual void Update(unsigned long currentMs, bool recursive = true) override;
protected:
/// @brief The radius of a wheel in meters
float wheelRadius = 0.0f;
float wheelRadius = 1.0f;
/// @brief The distance between the wheels in meters
float wheelSeparation = 0.0f;
float wheelSeparation = 1.0f;
/// @brief Convert revolutions per second to meters per second
float rpsToMs = 1.0f;
/// @brief The left wheel
Thing* leftWheel = nullptr;
/// @brief The right wheel
Thing* rightWheel = nullptr;
};
} // namespace RoboidControl

13
Things/DigitalSensor.cpp
View File

@ -2,17 +2,8 @@
namespace RoboidControl {
DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent) {
this->type = Type::Switch;
}
//DigitalSensor::DigitalSensor() {}
int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
bytes[(*ix)++] = state ? 1 : 0;
return 1;
}
void DigitalSensor::ProcessBinary(char* bytes) {
this->state |= (bytes[0] == 1);
}
DigitalSensor::DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant) {}
} // namespace RoboidControl

28
Things/DigitalSensor.h
View File

@ -7,31 +7,15 @@ namespace RoboidControl {
/// @brief A digital (on/off, 1/0, true/false) sensor
class DigitalSensor : public Thing {
public:
/// @brief Create a digital sensor without communication abilities
//DigitalSensor();
/// @brief Create a digital sensor for a participant
/// @param owner The owning participant
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
DigitalSensor(Participant* owner = nullptr); //, unsigned char thingId = 0);
/// @brief Create a new child digital sensor
/// @param parent The parent thing
/// @param thingId The ID of the thing, leave out or set to zero to generate
/// an ID
// DigitalSensor(Thing* parent); //, unsigned char thingId = 0);
DigitalSensor(Thing* parent = Thing::LocalRoot());
/// @brief The sigital state
bool state = 0;
/// @brief Function used to generate binary data for this digital 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 digital
/// sensor
/// @param bytes The binary data to process
virtual void ProcessBinary(char* bytes) override;
/// @brief The default constructor
//DigitalSensor();
/// @brief Create a temperature sensor with the given ID
/// @param networkId The network ID of the sensor
/// @param thingId The ID of the thing
DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
};
} // namespace RoboidControl

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

34
Things/Motor.cpp
View File

@ -1,34 +0,0 @@
#include "Motor.h"
#include "Messages/BinaryMsg.h"
#include "Participant.h"
namespace RoboidControl {
Motor::Motor(Thing* parent) : Thing(parent) {
this->type = Type::UncontrolledMotor;
}
void Motor::SetTargetVelocity(float targetSpeed) {
if (targetSpeed != this->targetVelocity) {
this->targetVelocity = targetSpeed;
if (this->owner->networkId != 0) {
// in other word: if we are connected...
BinaryMsg* binaryMsg = new BinaryMsg(this->owner->networkId, this);
this->owner->Send(binaryMsg);
delete binaryMsg;
}
}
}
float Motor::GetTargetVelocity() {
return this->targetVelocity;
}
int Motor::GenerateBinary(char* data, unsigned char* ix) {
data[(*ix)++] = this->targetVelocity * 127.0f;
return 1;
}
} // namespace RoboidControl

25
Things/Motor.h
View File

@ -1,25 +0,0 @@
#pragma once
#include "Thing.h"
namespace RoboidControl {
class Motor : public Thing {
public:
Motor(Thing* parent = Thing::LocalRoot());
/// @brief Motor turning direction
enum class Direction { Clockwise = 1, CounterClockwise = -1 };
/// @brief The forward turning direction of the motor
Direction direction;
virtual void SetTargetVelocity(float velocity); // -1..0..1
virtual float GetTargetVelocity();
int GenerateBinary(char* bytes, unsigned char* ix) override;
protected:
float targetVelocity = 0;
};
} // namespace RoboidControl

22
Things/RelativeEncoder.cpp
View File

@ -1,22 +0,0 @@
#include "RelativeEncoder.h"
namespace RoboidControl {
RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(parent) {
this->type = Type::RelativeEncoder;
}
float RelativeEncoder::GetRotationSpeed() {
return rotationSpeed;
}
int RelativeEncoder::GenerateBinary(char* data, unsigned char* ix) {
data[(*ix)++] = (unsigned char)(this->rotationSpeed * 127);
return 1;
}
void RelativeEncoder::ProcessBinary(char* data) {
this->rotationSpeed = (float)data[0] / 127;
}
} // namespace RoboidControl

39
Things/RelativeEncoder.h
View File

@ -1,39 +0,0 @@
#pragma once
#include "Thing.h"
namespace RoboidControl {
/// @brief An Incremental Encoder measures the rotations of an axle using a
/// rotary sensor. Some encoders are able to detect direction, while others can
/// not.
class RelativeEncoder : public Thing {
public:
/// @brief Creates a sensor which measures distance from pulses
/// @param pulsesPerRevolution The number of pulse edges which make a
/// full rotation
/// @param distancePerRevolution The distance a wheel travels per full
/// rotation
//RelativeEncoder(Participant* owner);
// RelativeEncoder(Thing* parent);
RelativeEncoder(Thing* parent = Thing::LocalRoot());
/// @brief Get the rotation speed
/// @return The speed in revolutions per second
virtual float GetRotationSpeed();
float rotationSpeed = 0;
/// @brief Function used to generate binary data for this touch 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 touch sensor
/// @param bytes The binary data to process
virtual void ProcessBinary(char* bytes) override;
protected:
/// @brief rotation speed in revolutions per second
//float _rotationSpeed;
};
} // namespace RoboidControl

6
Things/TemperatureSensor.cpp
View File

@ -4,9 +4,9 @@
namespace RoboidControl {
TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent) {
this->type = Type::TemperatureSensor;
}
TemperatureSensor::TemperatureSensor(Participant* participant,
unsigned char thingId)
: Thing(participant, Type::TemperatureSensor, thingId) {}
void TemperatureSensor::SetTemperature(float temp) {
this->temperature = temp;

10
Things/TemperatureSensor.h
View File

@ -7,17 +7,15 @@ namespace RoboidControl {
/// @brief A temperature sensor
class TemperatureSensor : public Thing {
public:
/// @brief The measured temperature
float temperature = 0;
/// @brief The default constructor
//TemperatureSensor();
/// @brief Create a temperature sensor with the given ID
/// @param networkId The network ID of the sensor
/// @param thingId The ID of the thing
TemperatureSensor(Participant* participant); //, unsigned char thingId);
// TemperatureSensor(Thing* parent);
TemperatureSensor(Thing* parent = Thing::LocalRoot());
/// @brief The measured temperature
float temperature = 0;
TemperatureSensor(Participant* participant, unsigned char thingId);
/// @brief Manually override the measured temperature
/// @param temperature The new temperature

31
Things/TouchSensor.cpp
View File

@ -1,36 +1,23 @@
#include "TouchSensor.h"
namespace RoboidControl {
TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}
TouchSensor::TouchSensor(Thing* parent) : Thing(parent) {
this->type = Type::TouchSensor;
this->name = "Touch sensor";
}
TouchSensor::TouchSensor(Participant* participant)
: Thing(participant, Thing::Type::TouchSensor) {}
bool TouchSensor::IsTouching() {
return this->internalTouch || this->externalTouch;
}
// void TouchSensor::PrepareForUpdate() {
// //this->internalTouch = this->externalTouch;
// }
void TouchSensor::Update(bool recursive) {
Thing::Update(recursive);
TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
this->SetParent(parent);
}
int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
std::cout << "BinaryMsg Touch " << this->internalTouch << std::endl;
bytes[(*ix)++] = this->internalTouch ? 1 : 0;
return 1;
return 0;
}
void TouchSensor::ProcessBinary(char* bytes) {
this->externalTouch = (bytes[0] == 1);
if (this->externalTouch)
std::cout << "touching!\n";
else
std::cout << "not touching\n";
// if (bytes[0] == 1)
// std::cout << this->name << " is Touching something!\n";
this->touchedSomething |= (bytes[0] == 1);
}
} // namespace RoboidControl

30
Things/TouchSensor.h
View File

@ -6,31 +6,29 @@ 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
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
TouchSensor(Thing* parent = Thing::LocalRoot());
/// @brief Create a touch sensor with isolated participant
TouchSensor();
/// @brief Create a touch sensor
TouchSensor(Participant* participant);
/// @brief Create a temperature sensor with the given ID
/// @param networkId The network ID of the sensor
/// @param thingId The ID of the thing
TouchSensor(Thing* parent);
// TouchSensor(RemoteParticipant* participant, unsigned char networkId,
// unsigned char thingId);
/// @brief Value which is true when the sensor is touching something, false
/// otherwise
bool IsTouching();
bool touchedSomething = false;
//virtual void PrepareForUpdate() override;
virtual void Update(bool recursive) override;
/// @brief Function used to generate binary data for this touch sensor
/// @brief Function to create a binary message with the temperature
/// @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 touch sensor
/// @param bytes The binary data to process
/// @brief Function to extract the temperature received in the binary message
/// @param bytes The binary data
virtual void ProcessBinary(char* bytes) override;
protected:
bool externalTouch = false;
bool internalTouch = false;
};
} // namespace RoboidControl

11
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 {
void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
#if defined(_WIN32) || defined(_WIN64)
@ -85,9 +85,9 @@ void ParticipantUDP::Receive() {
unsigned int sender_port = ntohs(client_addr.sin_port);
ReceiveData(packetSize, sender_ipAddress, sender_port);
// RoboidControl::ParticipantUDP* remoteParticipant = this->Get(sender_ipAddress, sender_port);
// RoboidControl::ParticipantUDP* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
// if (remoteParticipant == nullptr) {
// remoteParticipant = this->Add(sender_ipAddress, sender_port);
// remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
// // std::cout << "New sender " << sender_ipAddress << ":"
// // << sender_port << "\n";
// // std::cout << "New remote participant " <<
@ -102,7 +102,7 @@ void ParticipantUDP::Receive() {
#endif // _WIN32 || _WIN64
}
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);
@ -142,6 +142,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
return true;
}
} // namespace Windows
} // namespace RoboidControl
#endif

8
Windows/WindowsParticipant.h
View File

@ -1,15 +1,15 @@
#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:
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);
protected:
@ -18,5 +18,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
#endif
};
} // namespace Windows
} // namespace RoboidControl
#endif

8
examples/CMakeLists.txt
View File

@ -1,8 +0,0 @@
# 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
target_link_libraries(BB2A RoboidControl)
endif()

1
examples/README.md
View File

@ -1 +0,0 @@
Important: this folder has to be named '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)

146
test/participant_test.cc
View File

@ -4,75 +4,111 @@
// not supported using Visual Studio 2022 compiler...
#include <gtest/gtest.h>
#include <chrono>
#include <thread>
// #include <ws2tcpip.h>
#include "Participant.h"
#include "Participants/SiteServer.h"
#include "Thing.h"
#include <chrono>
#include <thread>
using namespace RoboidControl;
TEST(Participant, Participant) {
ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7682);
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
participant->Update();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
}
SUCCEED();
// Function to get the current time in milliseconds as unsigned long
unsigned long get_time_ms() {
auto now = std::chrono::steady_clock::now();
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
now.time_since_epoch());
return static_cast<unsigned long>(ms.count());
}
TEST(Participant, SiteServer) {
SiteServer* siteServer = new SiteServer();
// class RoboidControlSuite : public ::testing::test {
// TEST_F(RoboidControlSuite, HiddenParticipant) {
// Thing thing = Thing();
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer->Update();
// unsigned long milliseconds = get_time_ms();
// unsigned long startTime = milliseconds;
// while (milliseconds < startTime + 1000) {
// Thing.Update(milliseconds);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
// milliseconds = get_time_ms();
// }
// ASSERT_EQ(1, 1);
// }
// }
class ParticipantSuite : public ::testing::Test {
protected:
// SetUp and TearDown can be used to set up and clean up before/after each
// test
void SetUp() override {
// Initialize test data here
}
SUCCEED();
}
TEST(Participant, SiteParticipant) {
SiteServer* siteServer = new SiteServer(7681);
ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682);
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer->Update();
participant->Update();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
void TearDown() override {
// Clean up test data here
}
SUCCEED();
}
};
TEST(Participant, ThingMsg) {
SiteServer* siteServer = new SiteServer(7681);
ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682);
Thing* thing = new Thing();
thing->SetName("First Thing");
thing->SetModel("https://passer.life/extras/ant.jpg");
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 7000) {
siteServer->Update();
participant->Update();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
milliseconds = Thing::GetTimeMs();
}
SUCCEED();
}
// TEST_F(ParticipantSuite, ParticipantUDP) {
// ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681);
// unsigned long milliseconds = get_time_ms();
// unsigned long startTime = milliseconds;
// while (milliseconds < startTime + 1000) {
// participant->Update(milliseconds);
// milliseconds = get_time_ms();
// }
// ASSERT_EQ(1, 1);
// }
// TEST_F(ParticipantSuite, SiteServer) {
// SiteServer site = SiteServer(7681);
// unsigned long milliseconds = get_time_ms();
// unsigned long startTime = milliseconds;
// while (milliseconds < startTime + 1000) {
// site.Update(milliseconds);
// milliseconds = get_time_ms();
// }
// ASSERT_EQ(1, 1);
// }
// TEST_F(ParticipantSuite, SiteParticipant) {
// SiteServer site = SiteServer(7681);
// ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681);
// unsigned long milliseconds = get_time_ms();
// unsigned long startTime = milliseconds;
// while (milliseconds < startTime + 1000) {
// site.Update(milliseconds);
// participant.Update(milliseconds);
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
// milliseconds = get_time_ms();
// }
// ASSERT_EQ(1, 1);
// }
// TEST_F(ParticipantSuite, Thing) {
// SiteServer site = SiteServer(7681);
// ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681);
// Thing thing = Thing(&participant);
// unsigned long milliseconds = get_time_ms();
// unsigned long startTime = milliseconds;
// while (milliseconds < startTime + 1000) {
// site.Update(milliseconds);
// participant.Update(milliseconds);
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
// milliseconds = get_time_ms();
// }
// ASSERT_EQ(1, 1);
// }
#endif

8
test/thing_test.cc
View File

@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 1000) {
thing->Update();
Thing::UpdateThings(milliseconds);
milliseconds = Thing::GetTimeMs();
}
@ -23,13 +23,13 @@ TEST(RoboidControlSuite, HiddenParticipant) {
}
TEST(RoboidControlSuite, IsolatedParticipant) {
ParticipantUDP* participant = new ParticipantUDP(0);
Thing* thing = new Thing();
ParticipantUDP* participant = ParticipantUDP::Isolated();
Thing* thing = new Thing(participant);
unsigned long milliseconds = Thing::GetTimeMs();
unsigned long startTime = milliseconds;
while (milliseconds < startTime + 1000) {
participant->Update();
participant->Update(milliseconds);
milliseconds = Thing::GetTimeMs();
}