Compare commits
3 Commits
Author | SHA1 | Date | |
---|---|---|---|
c95d1cfc06 | |||
627a79f56c | |||
90e89b2ecd |
@ -1,5 +1,4 @@
|
|||||||
#include "ArduinoParticipant.h"
|
#include "ArduinoParticipant.h"
|
||||||
#if defined(ARDUINO)
|
|
||||||
|
|
||||||
#if !defined(NO_STD)
|
#if !defined(NO_STD)
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
@ -22,6 +21,7 @@
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
namespace Arduino {
|
||||||
|
|
||||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||||
WiFiUDP* udp;
|
WiFiUDP* udp;
|
||||||
@ -88,7 +88,7 @@ void ParticipantUDP::Receive() {
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
|
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
|
||||||
#if defined(ARDUINO) && defined(HAS_WIFI)
|
#if defined(ARDUINO) && defined(HAS_WIFI)
|
||||||
// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
|
// std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
|
||||||
// << remoteParticipant->port << "\n";
|
// << remoteParticipant->port << "\n";
|
||||||
@ -131,5 +131,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
|
|||||||
return true;
|
return true;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
} // namespace Arduino
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
#endif
|
|
@ -1,15 +1,15 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
#if defined(ARDUINO)
|
|
||||||
|
|
||||||
#include "Participants/ParticipantUDP.h"
|
#include "Participants/ParticipantUDP.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
namespace Arduino {
|
||||||
|
|
||||||
class ParticipantUDP : public ParticipantUDPGeneric {
|
class ParticipantUDP : public RoboidControl::ParticipantUDP {
|
||||||
public:
|
public:
|
||||||
void Setup();
|
void Setup();
|
||||||
void Receive();
|
void Receive();
|
||||||
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
|
bool Send(Participant* remoteParticipant, int bufferSize);
|
||||||
bool Publish(IMessage* msg);
|
bool Publish(IMessage* msg);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
@ -18,5 +18,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
|
|||||||
void GetBroadcastAddress();
|
void GetBroadcastAddress();
|
||||||
};
|
};
|
||||||
|
|
||||||
|
} // namespace Arduino
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
#endif
|
|
@ -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);
|
|
||||||
}
|
|
@ -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
|
|
@ -7,9 +7,7 @@ namespace Arduino {
|
|||||||
|
|
||||||
#pragma region DRV8833
|
#pragma region DRV8833
|
||||||
|
|
||||||
DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) {
|
DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
|
||||||
this->type = Type::Undetermined;
|
|
||||||
this->name = "DRV8833";
|
|
||||||
this->pinStandby = config.standby;
|
this->pinStandby = config.standby;
|
||||||
if (pinStandby != 255)
|
if (pinStandby != 255)
|
||||||
pinMode(pinStandby, OUTPUT);
|
pinMode(pinStandby, OUTPUT);
|
||||||
@ -26,15 +24,14 @@ DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) {
|
|||||||
|
|
||||||
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
|
DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
|
||||||
Thing* parent)
|
Thing* parent)
|
||||||
: RoboidControl::DifferentialDrive(nullptr, nullptr, parent) {
|
: RoboidControl::DifferentialDrive(this->drv8833.motorA,
|
||||||
this->drv8833 = new DRV8833(config, this);
|
this->drv8833.motorB,
|
||||||
this->leftWheel = this->drv8833->motorA;
|
parent),
|
||||||
this->rightWheel = this->drv8833->motorB;
|
drv8833(config, this) {}
|
||||||
}
|
|
||||||
|
|
||||||
void DRV8833::DifferentialDrive::Update(bool recurse) {
|
void DRV8833::DifferentialDrive::Update(bool recurse) {
|
||||||
RoboidControl::DifferentialDrive::Update(recurse);
|
RoboidControl::DifferentialDrive::Update(recurse);
|
||||||
this->drv8833->Update(false);
|
this->drv8833.Update(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
#pragma endregion Differential drive
|
#pragma endregion Differential drive
|
||||||
@ -49,7 +46,8 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
|
|||||||
unsigned char pinIn1,
|
unsigned char pinIn1,
|
||||||
unsigned char pinIn2,
|
unsigned char pinIn2,
|
||||||
bool reverse)
|
bool reverse)
|
||||||
: Motor(driver) {
|
: Motor() {
|
||||||
|
this->SetParent(driver);
|
||||||
|
|
||||||
this->pinIn1 = pinIn1;
|
this->pinIn1 = pinIn1;
|
||||||
this->pinIn2 = pinIn2;
|
this->pinIn2 = pinIn2;
|
||||||
|
@ -19,8 +19,6 @@ class DRV8833 : public Thing {
|
|||||||
int BIn1;
|
int BIn1;
|
||||||
int BIn2;
|
int BIn2;
|
||||||
int standby = 255;
|
int standby = 255;
|
||||||
|
|
||||||
constexpr Configuration(int a1, int a2, int b1, int b2, int standby = 255) : AIn1(a1), AIn2(a2), BIn1(b1), BIn2(b2), standby(standby) {}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/// @brief Setup a DRV8833 motor controller
|
/// @brief Setup a DRV8833 motor controller
|
||||||
@ -45,7 +43,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
|
|||||||
virtual void Update(bool recurse = false) override;
|
virtual void Update(bool recurse = false) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
DRV8833* drv8833 = nullptr;
|
DRV8833 drv8833;
|
||||||
};
|
};
|
||||||
|
|
||||||
#pragma endregion Differential drive
|
#pragma endregion Differential drive
|
||||||
|
@ -7,9 +7,8 @@ namespace Arduino {
|
|||||||
|
|
||||||
#pragma region Digital input
|
#pragma region Digital input
|
||||||
|
|
||||||
DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(parent) {
|
DigitalInput::DigitalInput(unsigned char pin, Thing* parent)
|
||||||
this->type = Type::Switch;
|
: Thing(Type::Undetermined, parent) {
|
||||||
this->name = "Digital Input";
|
|
||||||
this->pin = pin;
|
this->pin = pin;
|
||||||
pinMode(this->pin, INPUT);
|
pinMode(this->pin, INPUT);
|
||||||
std::cout << "digital input start\n";
|
std::cout << "digital input start\n";
|
||||||
@ -29,7 +28,7 @@ DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
|
|||||||
: RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
|
: RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
|
||||||
|
|
||||||
void DigitalInput::TouchSensor::Update(bool recursive) {
|
void DigitalInput::TouchSensor::Update(bool recursive) {
|
||||||
this->internalTouch = digitalInput.isLow;
|
this->touchedSomething = digitalInput.isLow;
|
||||||
}
|
}
|
||||||
|
|
||||||
#pragma endregion Touch sensor
|
#pragma endregion Touch sensor
|
||||||
@ -98,8 +97,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) {
|
|||||||
this->pulseFrequency = pulseCount / timeStep;
|
this->pulseFrequency = pulseCount / timeStep;
|
||||||
this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
|
this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
|
||||||
|
|
||||||
// std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
|
std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
|
||||||
// << " timestep " << timeStep << std::endl;
|
<< " timestep " << timeStep << std::endl;
|
||||||
|
|
||||||
this->lastUpdateTime = currentTimeMs;
|
this->lastUpdateTime = currentTimeMs;
|
||||||
}
|
}
|
||||||
|
@ -7,8 +7,7 @@ namespace RoboidControl {
|
|||||||
namespace Arduino {
|
namespace Arduino {
|
||||||
|
|
||||||
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
|
UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
|
||||||
: Thing(parent) {
|
: Thing(Type::Undetermined, parent) {
|
||||||
this->type = Type::DistanceSensor;
|
|
||||||
this->name = "Ultrasonic sensor";
|
this->name = "Ultrasonic sensor";
|
||||||
this->pinTrigger = config.trigger;
|
this->pinTrigger = config.trigger;
|
||||||
this->pinEcho = config.echo;
|
this->pinEcho = config.echo;
|
||||||
@ -57,28 +56,6 @@ void UltrasonicSensor::Update(bool recursive) {
|
|||||||
Thing::Update(recursive);
|
Thing::Update(recursive);
|
||||||
}
|
}
|
||||||
|
|
||||||
#pragma region Distance sensor
|
|
||||||
|
|
||||||
UltrasonicSensor::DistanceSensor::DistanceSensor(
|
|
||||||
UltrasonicSensor::Configuration config,
|
|
||||||
Thing* parent)
|
|
||||||
: RoboidControl::DistanceSensor(parent), ultrasonic(config, this) {}
|
|
||||||
|
|
||||||
void UltrasonicSensor::DistanceSensor::Update(bool recursive) {
|
|
||||||
RoboidControl::DistanceSensor::Update(recursive);
|
|
||||||
this->ultrasonic.Update(false);
|
|
||||||
if (this->ultrasonic.distance > 0)
|
|
||||||
this->internalDistance = this->ultrasonic.distance;
|
|
||||||
else
|
|
||||||
#if ARDUNIO
|
|
||||||
this->internalDistance = INFINITY;
|
|
||||||
#else
|
|
||||||
this->internalDistance = std::numeric_limits<float>::infinity();
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
#pragma endregion Distance sensor
|
|
||||||
|
|
||||||
#pragma region Touch sensor
|
#pragma region Touch sensor
|
||||||
|
|
||||||
UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
|
UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
|
||||||
@ -87,11 +64,11 @@ UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
|
|||||||
void UltrasonicSensor::TouchSensor::Update(bool recursive) {
|
void UltrasonicSensor::TouchSensor::Update(bool recursive) {
|
||||||
RoboidControl::TouchSensor::Update(recursive);
|
RoboidControl::TouchSensor::Update(recursive);
|
||||||
this->ultrasonic.Update(false);
|
this->ultrasonic.Update(false);
|
||||||
this->internalTouch = (this->ultrasonic.distance > 0 &&
|
this->touchedSomething |= (this->ultrasonic.distance > 0 &&
|
||||||
this->ultrasonic.distance <= this->touchDistance);
|
this->ultrasonic.distance <= this->touchDistance);
|
||||||
}
|
}
|
||||||
|
|
||||||
#pragma endregion Touch sensor
|
#pragma region Touch sensor
|
||||||
|
|
||||||
} // namespace Arduino
|
} // namespace Arduino
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
@ -1,6 +1,5 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "Things/DistanceSensor.h"
|
|
||||||
#include "Things/TouchSensor.h"
|
#include "Things/TouchSensor.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
@ -40,26 +39,9 @@ class UltrasonicSensor : Thing {
|
|||||||
unsigned char pinEcho = 0;
|
unsigned char pinEcho = 0;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
class DistanceSensor;
|
|
||||||
class TouchSensor;
|
class TouchSensor;
|
||||||
};
|
};
|
||||||
|
|
||||||
#pragma region Distance sensor
|
|
||||||
|
|
||||||
class UltrasonicSensor::DistanceSensor : public RoboidControl::DistanceSensor {
|
|
||||||
public:
|
|
||||||
DistanceSensor(UltrasonicSensor::Configuration config,
|
|
||||||
Thing* parent = Thing::LocalRoot());
|
|
||||||
|
|
||||||
/// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
|
|
||||||
virtual void Update(bool recursive = false) override;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
UltrasonicSensor ultrasonic;
|
|
||||||
};
|
|
||||||
|
|
||||||
#pragma endregion Distance sensor
|
|
||||||
|
|
||||||
#pragma region Touch sensor
|
#pragma region Touch sensor
|
||||||
|
|
||||||
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
|
class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
|
||||||
@ -76,7 +58,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
|
|||||||
UltrasonicSensor ultrasonic;
|
UltrasonicSensor ultrasonic;
|
||||||
};
|
};
|
||||||
|
|
||||||
#pragma endregion Touch sensor
|
#pragma region Touch sensor
|
||||||
|
|
||||||
} // namespace Arduino
|
} // namespace Arduino
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
@ -22,23 +22,43 @@ else()
|
|||||||
set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard
|
set(CMAKE_CXX_STANDARD 17) # Enable c++11 standard
|
||||||
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
|
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
|
||||||
|
|
||||||
|
project(RoboidControl)
|
||||||
|
add_subdirectory(LinearAlgebra)
|
||||||
|
add_subdirectory(Examples)
|
||||||
|
|
||||||
|
add_compile_definitions(GTEST)
|
||||||
|
include(FetchContent)
|
||||||
|
FetchContent_Declare(
|
||||||
|
googletest
|
||||||
|
DOWNLOAD_EXTRACT_TIMESTAMP ON
|
||||||
|
URL https://github.com/google/googletest/archive/refs/heads/main.zip
|
||||||
|
)
|
||||||
|
|
||||||
|
# For Windows: Prevent overriding the parent project's compiler/linker settings
|
||||||
|
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
|
||||||
|
FetchContent_MakeAvailable(googletest)
|
||||||
|
|
||||||
include_directories(
|
include_directories(
|
||||||
.
|
.
|
||||||
LinearAlgebra
|
LinearAlgebra
|
||||||
)
|
)
|
||||||
add_library(RoboidControl STATIC ${srcs})
|
add_library(RoboidControl STATIC ${srcs})
|
||||||
target_include_directories(RoboidControl PUBLIC
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}
|
|
||||||
)
|
|
||||||
|
|
||||||
project(RoboidControl)
|
|
||||||
add_subdirectory(LinearAlgebra)
|
|
||||||
|
|
||||||
# Examples
|
|
||||||
option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF)
|
|
||||||
add_subdirectory(examples)
|
|
||||||
|
|
||||||
enable_testing()
|
enable_testing()
|
||||||
add_subdirectory(test)
|
|
||||||
|
|
||||||
|
file(GLOB_RECURSE test_srcs test/*_test.cc)
|
||||||
|
add_executable(
|
||||||
|
RoboidControlTest
|
||||||
|
${test_srcs}
|
||||||
|
)
|
||||||
|
target_link_libraries(
|
||||||
|
RoboidControlTest
|
||||||
|
gtest_main
|
||||||
|
RoboidControl
|
||||||
|
LinearAlgebra
|
||||||
|
)
|
||||||
|
|
||||||
|
include(GoogleTest)
|
||||||
|
gtest_discover_tests(RoboidControlTest)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
@ -48,7 +48,7 @@ PROJECT_NAME = "Roboid Control for C++"
|
|||||||
# could be handy for archiving the generated documentation or if some version
|
# could be handy for archiving the generated documentation or if some version
|
||||||
# control system is used.
|
# control system is used.
|
||||||
|
|
||||||
PROJECT_NUMBER = 0.4
|
PROJECT_NUMBER =
|
||||||
|
|
||||||
# Using the PROJECT_BRIEF tag one can provide an optional one line description
|
# Using the PROJECT_BRIEF tag one can provide an optional one line description
|
||||||
# for a project that appears at the top of each page and should give viewer a
|
# for a project that appears at the top of each page and should give viewer a
|
||||||
|
@ -2,13 +2,15 @@
|
|||||||
|
|
||||||
#if defined(IDF_VER)
|
#if defined(IDF_VER)
|
||||||
#include "esp_wifi.h"
|
#include "esp_wifi.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
#include <arpa/inet.h>
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
namespace EspIdf {
|
||||||
|
|
||||||
void ParticipantUDP::SetupUDP(int localPort,
|
void ParticipantUDP::Setup(int localPort,
|
||||||
const char* remoteIpAddress,
|
const char* remoteIpAddress,
|
||||||
int remotePort) {
|
int remotePort) {
|
||||||
|
#if defined(IDF_VER)
|
||||||
std::cout << "Set up UDP\n";
|
std::cout << "Set up UDP\n";
|
||||||
GetBroadcastAddress();
|
GetBroadcastAddress();
|
||||||
|
|
||||||
@ -20,57 +22,42 @@ void ParticipantUDP::SetupUDP(int localPort,
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Create a UDP socket
|
// Create a UDP socket
|
||||||
this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
||||||
if (this->sock < 0) {
|
if (this->sockfd < 0) {
|
||||||
std::cout << "Unable to create UDP socket: errno " << errno << "\n";
|
std::cout << "Unable to create UDP socket: errno " << errno << "\n";
|
||||||
vTaskDelete(NULL);
|
vTaskDelete(NULL);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
// Set up the server address structure
|
||||||
// Set up the receiving(local) address
|
|
||||||
struct sockaddr_in local_addr;
|
struct sockaddr_in local_addr;
|
||||||
memset(&local_addr, 0, sizeof(local_addr));
|
memset(&local_addr, 0, sizeof(local_addr));
|
||||||
local_addr.sin_family = AF_INET;
|
local_addr.sin_family = AF_INET;
|
||||||
local_addr.sin_port = htons(localPort);
|
local_addr.sin_port = htons(this->port);
|
||||||
local_addr.sin_addr.s_addr =
|
local_addr.sin_addr.s_addr =
|
||||||
htonl(INADDR_ANY); // Listen on all available network interfaces
|
htonl(INADDR_ANY); // Listen on all available network interfaces
|
||||||
|
|
||||||
// Bind the socket to the receiving address
|
// Bind the socket to the address and port
|
||||||
if (bind(this->sock, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) {
|
if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) <
|
||||||
|
0) {
|
||||||
std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
|
std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
|
||||||
close(sock);
|
close(sockfd);
|
||||||
vTaskDelete(NULL);
|
vTaskDelete(NULL);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
*/
|
|
||||||
|
|
||||||
// Initialize the destination(remote) address
|
// Initialize the dest_addr structure
|
||||||
memset(&this->dest_addr, 0,
|
memset(&this->dest_addr, 0,
|
||||||
sizeof(this->dest_addr)); // Clear the entire structure
|
sizeof(this->dest_addr)); // Clear the entire structure
|
||||||
this->dest_addr.sin_family = AF_INET;
|
this->dest_addr.sin_family = AF_INET;
|
||||||
this->dest_addr.sin_port = htons(this->remoteSite->port);
|
this->dest_addr.sin_port = htons(this->remoteSite->port);
|
||||||
this->dest_addr.sin_addr.s_addr = inet_addr(this->remoteSite->ipAddress);
|
inet_pton(AF_INET, this->remoteSite->ipAddress,
|
||||||
// inet_pton(AF_INET, this->remoteSite->ipAddress,
|
&this->dest_addr.sin_addr.s_addr);
|
||||||
// &this->dest_addr.sin_addr.s_addr);
|
|
||||||
|
|
||||||
this->connected = true;
|
std::cout << "Wifi sync started local " << this->port << ", remote "
|
||||||
|
|
||||||
std::cout << "Wifi sync started local " << localPort << ", remote "
|
|
||||||
<< this->remoteSite->ipAddress << ":" << this->remoteSite->port
|
<< this->remoteSite->ipAddress << ":" << this->remoteSite->port
|
||||||
<< "\n";
|
<< "\n";
|
||||||
|
#endif // IDF_VER
|
||||||
// std::cout << "socket: " << (int)this->sock << std::endl;
|
|
||||||
// ParticipantMsg* msg = new ParticipantMsg(this->networkId);
|
|
||||||
// int bufferSize = msg->Serialize(this->buffer);
|
|
||||||
// int err = sendto(this->sock, buffer, bufferSize, 0,
|
|
||||||
// (struct sockaddr*)&dest_addr, sizeof(dest_addr));
|
|
||||||
// if (errno != 0)
|
|
||||||
// std::cout << "AASend error " << err << " or " << errno << "\n";
|
|
||||||
// else
|
|
||||||
// std::cout << "AASend SUCCESS\n";
|
|
||||||
|
|
||||||
//SendTest();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDP::GetBroadcastAddress() {
|
void ParticipantUDP::GetBroadcastAddress() {
|
||||||
@ -96,11 +83,10 @@ void ParticipantUDP::GetBroadcastAddress() {
|
|||||||
#endif // IDF_VER
|
#endif // IDF_VER
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDP::ReceiveUDP() {
|
void ParticipantUDP::Receive() {
|
||||||
#if defined(IDF_VER)
|
#if defined(IDF_VER)
|
||||||
/*
|
|
||||||
struct pollfd fds[1];
|
struct pollfd fds[1];
|
||||||
fds[0].fd = sock;
|
fds[0].fd = sockfd;
|
||||||
fds[0].events = POLLIN; // We're looking for data available to read
|
fds[0].events = POLLIN; // We're looking for data available to read
|
||||||
|
|
||||||
// Use poll() with a timeout of 0 to return immediately
|
// Use poll() with a timeout of 0 to return immediately
|
||||||
@ -117,7 +103,7 @@ void ParticipantUDP::ReceiveUDP() {
|
|||||||
char sender_ipAddress[INET_ADDRSTRLEN];
|
char sender_ipAddress[INET_ADDRSTRLEN];
|
||||||
|
|
||||||
while (ret > 0 && fds[0].revents & POLLIN) {
|
while (ret > 0 && fds[0].revents & POLLIN) {
|
||||||
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer) - 1, 0,
|
int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0,
|
||||||
(struct sockaddr*)&source_addr, &addr_len);
|
(struct sockaddr*)&source_addr, &addr_len);
|
||||||
if (packetSize < 0) {
|
if (packetSize < 0) {
|
||||||
std::cout << "recvfrom() error\n";
|
std::cout << "recvfrom() error\n";
|
||||||
@ -140,66 +126,18 @@ void ParticipantUDP::ReceiveUDP() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
// std::cout << "no more messages\n";
|
// std::cout << "no more messages\n";
|
||||||
*/
|
|
||||||
#endif // IDF_VER
|
#endif // IDF_VER
|
||||||
}
|
}
|
||||||
|
|
||||||
ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}
|
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
|
||||||
|
|
||||||
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
|
|
||||||
: ParticipantUDPGeneric(ipAddress, port, localPort) {}
|
|
||||||
|
|
||||||
// bool ParticipantUDP::SendTest() {
|
|
||||||
// #if defined(IDF_VER)
|
|
||||||
// // std::cout << "socket: " << (int)this->sock << std::endl;
|
|
||||||
// // UBaseType_t stack_size = uxTaskGetStackHighWaterMark(NULL); // NULL to check the main task
|
|
||||||
// // size_t free_heap = xPortGetFreeHeapSize();
|
|
||||||
// // std::cout << "Stack High Water Mark: " << stack_size << " heap " << free_heap << std::endl;
|
|
||||||
|
|
||||||
// ParticipantMsg* msg = new ParticipantMsg(this->networkId);
|
|
||||||
// int bSize = msg->Serialize(this->buffer);
|
|
||||||
// // std::cout << "buffer size " << bSize << std::endl;
|
|
||||||
// int err = sendto(this->sock, buffer, bSize, 0, (struct sockaddr*)&dest_addr,
|
|
||||||
// sizeof(dest_addr));
|
|
||||||
// if (errno != 0)
|
|
||||||
// std::cout << "BBSend error " << err << " or " << errno << "\n";
|
|
||||||
// else
|
|
||||||
// std::cout << "BBSend SUCCESS\n";
|
|
||||||
|
|
||||||
// #endif
|
|
||||||
// return true;
|
|
||||||
// }
|
|
||||||
|
|
||||||
bool ParticipantUDP::Send(IMessage* msg) {
|
|
||||||
int bufferSize = msg->Serialize(this->buffer);
|
|
||||||
if (bufferSize <= 0)
|
|
||||||
return true;
|
|
||||||
|
|
||||||
std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
|
|
||||||
<< " to " << this->remoteSite->ipAddress << std::endl;
|
|
||||||
|
|
||||||
return this->SendTo(this->remoteSite, bufferSize);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
|
|
||||||
int bufferSize) {
|
|
||||||
#if defined(IDF_VER)
|
#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];
|
int err = sendto(this->sockfd, buffer, bufferSize, 0,
|
||||||
inet_ntop(AF_INET, &dest_addr.sin_addr, ip_str, sizeof(ip_str));
|
(struct sockaddr*)&dest_addr, sizeof(dest_addr));
|
||||||
std::cout << "Sending " << bufferSize << " bytes to " << ip_str << ":" << port << "\n";
|
if (errno != 0)
|
||||||
|
|
||||||
// Print the IP address and port
|
|
||||||
// printf("IP Address: %s\n", ip_str);
|
|
||||||
// printf("Port: %d\n", port);
|
|
||||||
|
|
||||||
this->dest_addr.sin_port = htons(remoteParticipant->port);
|
|
||||||
this->dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);
|
|
||||||
|
|
||||||
int err = sendto(this->sock, buffer, bufferSize, 0,
|
|
||||||
(struct sockaddr*)&this->dest_addr, sizeof(this->dest_addr));
|
|
||||||
if (errno < 0)
|
|
||||||
std::cout << "Send error " << err << " or " << errno << "\n";
|
std::cout << "Send error " << err << " or " << errno << "\n";
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
@ -216,7 +154,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
|
|||||||
dest_addr.sin_family = AF_INET;
|
dest_addr.sin_family = AF_INET;
|
||||||
dest_addr.sin_port = htons(this->port);
|
dest_addr.sin_port = htons(this->port);
|
||||||
inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr);
|
inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr);
|
||||||
int err = sendto(sock, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
|
int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
|
||||||
sizeof(dest_addr));
|
sizeof(dest_addr));
|
||||||
if (err != 0)
|
if (err != 0)
|
||||||
std::cout << "Publish error\n";
|
std::cout << "Publish error\n";
|
||||||
@ -224,6 +162,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
|
|||||||
return true;
|
return true;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
} // namespace EspIdf
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
|
|
||||||
#endif
|
|
@ -1,47 +1,26 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
#if defined(IDF_VER)
|
|
||||||
|
|
||||||
#include "Participants/ParticipantUDP.h"
|
#include "Participants/ParticipantUDP.h"
|
||||||
|
|
||||||
|
#if defined(IDF_VER)
|
||||||
#include "lwip/sockets.h"
|
#include "lwip/sockets.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
namespace EspIdf {
|
||||||
|
|
||||||
class ParticipantUDP : public ParticipantUDPGeneric {
|
class ParticipantUDP : public RoboidControl::ParticipantUDP {
|
||||||
public:
|
public:
|
||||||
/// @brief Create a participant without connecting to a site
|
|
||||||
/// @param port The port on which the participant communicates
|
|
||||||
/// These participant typically broadcast Participant messages to let site
|
|
||||||
/// servers on the local network know their presence. Alternatively they can
|
|
||||||
/// broadcast information which can be used directly by other participants.
|
|
||||||
ParticipantUDP(int port = 7681);
|
|
||||||
/// @brief Create a participant which will try to connect to a site.
|
|
||||||
/// @param ipAddress The IP address of the site
|
|
||||||
/// @param port The port used by the site
|
|
||||||
/// @param localPort The port used by the local participant
|
|
||||||
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
|
|
||||||
|
|
||||||
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
|
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
|
||||||
void Receive();
|
void Receive();
|
||||||
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
|
bool Send(Participant* remoteParticipant, int bufferSize);
|
||||||
bool Publish(IMessage* msg);
|
bool Publish(IMessage* msg);
|
||||||
|
|
||||||
// bool SendTest();
|
|
||||||
|
|
||||||
/// @brief Sens a message to the remote site (if set)
|
|
||||||
/// @param msg The message to send
|
|
||||||
/// @return True if a message could be sent.
|
|
||||||
bool Send(IMessage* msg) override;
|
|
||||||
void SetupUDP(int localPort,
|
|
||||||
const char* remoteIpAddress,
|
|
||||||
int remotePort) override;
|
|
||||||
void ReceiveUDP() override;
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
#if defined(IDF_VER)
|
#if defined(IDF_VER)
|
||||||
char broadcastIpAddress[INET_ADDRSTRLEN];
|
char broadcastIpAddress[INET_ADDRSTRLEN];
|
||||||
|
|
||||||
int sock;
|
int sockfd;
|
||||||
struct sockaddr_in dest_addr;
|
struct sockaddr_in dest_addr;
|
||||||
// struct sockaddr_in src_addr;
|
// struct sockaddr_in src_addr;
|
||||||
#endif
|
#endif
|
||||||
@ -49,6 +28,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
|
|||||||
void GetBroadcastAddress();
|
void GetBroadcastAddress();
|
||||||
};
|
};
|
||||||
|
|
||||||
|
} // namespace EspIdf
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
|
|
||||||
#endif
|
|
||||||
|
@ -5,83 +5,6 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
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,
|
void LowLevelMessages::SendAngle8(char* buffer,
|
||||||
unsigned char* ix,
|
unsigned char* ix,
|
||||||
const float angle) {
|
const float angle) {
|
||||||
@ -119,5 +42,58 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
|
|||||||
return (float)f.toFloat();
|
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
|
} // namespace RoboidControl
|
@ -11,10 +11,6 @@ class LowLevelMessages {
|
|||||||
static Spherical ReceiveSpherical(const char* buffer,
|
static Spherical ReceiveSpherical(const char* buffer,
|
||||||
unsigned char* startIndex);
|
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 void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
|
||||||
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
|
static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
|
||||||
|
|
||||||
|
@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
|
|||||||
this->thingId = thing->id;
|
this->thingId = thing->id;
|
||||||
|
|
||||||
this->poseType = 0;
|
this->poseType = 0;
|
||||||
if (thing->positionUpdated || (force || thing->IsRoot())) {
|
if (thing->positionUpdated || (force && thing->IsRoot())) {
|
||||||
this->position = thing->GetPosition();
|
this->position = thing->GetPosition();
|
||||||
this->poseType |= Pose_Position;
|
this->poseType |= Pose_Position;
|
||||||
}
|
}
|
||||||
if (thing->orientationUpdated || (force || thing->IsRoot())) {
|
if (thing->orientationUpdated || (force && thing->IsRoot())) {
|
||||||
this->orientation = thing->GetOrientation();
|
this->orientation = thing->GetOrientation();
|
||||||
this->poseType |= Pose_Orientation;
|
this->poseType |= Pose_Orientation;
|
||||||
}
|
}
|
||||||
@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) {
|
|||||||
this->thingId = buffer[ix++];
|
this->thingId = buffer[ix++];
|
||||||
this->poseType = buffer[ix++];
|
this->poseType = buffer[ix++];
|
||||||
this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
|
this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
|
||||||
this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
|
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
|
||||||
// linearVelocity
|
// linearVelocity
|
||||||
// angularVelocity
|
// angularVelocity
|
||||||
}
|
}
|
||||||
@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
|
|||||||
if ((this->poseType & Pose_Position) != 0)
|
if ((this->poseType & Pose_Position) != 0)
|
||||||
LowLevelMessages::SendSpherical(buffer, &ix, this->position);
|
LowLevelMessages::SendSpherical(buffer, &ix, this->position);
|
||||||
if ((this->poseType & Pose_Orientation) != 0)
|
if ((this->poseType & Pose_Orientation) != 0)
|
||||||
LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
|
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
|
||||||
if ((this->poseType & Pose_LinearVelocity) != 0)
|
if ((this->poseType & Pose_LinearVelocity) != 0)
|
||||||
LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
|
LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
|
||||||
if ((this->poseType & Pose_AngularVelocity) != 0)
|
if ((this->poseType & Pose_AngularVelocity) != 0)
|
||||||
|
@ -1,5 +1,3 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "IMessage.h"
|
#include "IMessage.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
@ -1,5 +1,3 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "IMessage.h"
|
#include "IMessage.h"
|
||||||
#include "Thing.h"
|
#include "Thing.h"
|
||||||
|
|
||||||
|
240
Participant.cpp
240
Participant.cpp
@ -1,15 +1,13 @@
|
|||||||
#include "Participant.h"
|
#include "Participant.h"
|
||||||
|
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include "Arduino/ArduinoParticipant.h"
|
|
||||||
#include "EspIdf/EspIdfParticipant.h"
|
|
||||||
#include "Posix/PosixParticipant.h"
|
|
||||||
#include "Windows/WindowsParticipant.h"
|
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
#pragma region Participant
|
#pragma region Participant
|
||||||
|
|
||||||
|
ParticipantRegistry Participant::registry;
|
||||||
|
|
||||||
Participant* Participant::LocalParticipant = new Participant();
|
Participant* Participant::LocalParticipant = new Participant();
|
||||||
|
|
||||||
void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
|
void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
|
||||||
@ -18,14 +16,17 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
Participant::Participant() {
|
Participant::Participant() {
|
||||||
Thing::CreateRoot(this);
|
std::cout << "P\n";
|
||||||
//this->Add(this->root);
|
//this->root.name = "Isolated";
|
||||||
|
this->root = new Thing(this);
|
||||||
|
this->root->name = "Root";
|
||||||
|
this->Add(this->root);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
Participant::Participant(const char* ipAddress, int port) {
|
Participant::Participant(const char* ipAddress, int port) {
|
||||||
Thing::CreateRoot(this);
|
// Add the root thing to the list of things, because we could not do that
|
||||||
//this->Add(this->root);
|
// earlier (I think)
|
||||||
|
this->Add(this->root);
|
||||||
|
|
||||||
// make a copy of the ip address string
|
// make a copy of the ip address string
|
||||||
int addressLength = (int)strlen(ipAddress);
|
int addressLength = (int)strlen(ipAddress);
|
||||||
@ -42,62 +43,27 @@ Participant::Participant(const char* ipAddress, int port) {
|
|||||||
this->ipAddress = addressString;
|
this->ipAddress = addressString;
|
||||||
this->port = port;
|
this->port = port;
|
||||||
}
|
}
|
||||||
*/
|
|
||||||
Participant::~Participant() {
|
Participant::~Participant() {
|
||||||
// registry.Remove(this);
|
// registry.Remove(this);
|
||||||
// delete[] this->ipAddress;
|
delete[] this->ipAddress;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Participant::Update(bool recurse) {
|
void Participant::Update() {
|
||||||
for (Thing* thing : this->things) {
|
for (Thing* thing : this->things) {
|
||||||
if (thing != nullptr)
|
if (thing != nullptr)
|
||||||
thing->Update();
|
thing->Update(true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Participant::Send(IMessage* msg) {
|
Thing* Participant::Get(unsigned char thingId) {
|
||||||
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) {
|
|
||||||
for (Thing* thing : this->things) {
|
for (Thing* thing : this->things) {
|
||||||
if (thing->owner->networkId == networkId && thing->id == thingId)
|
if (thing->id == thingId)
|
||||||
return thing;
|
return thing;
|
||||||
}
|
}
|
||||||
std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port
|
// std::cout << "Could not find thing " << this->ipAddress << ":" <<
|
||||||
<< "[" << (int)thingId << "]\n";
|
// this->port
|
||||||
|
// << "[" << (int)thingId << "]\n";
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -119,24 +85,22 @@ void Participant::Add(Thing* thing, bool checkId) {
|
|||||||
thing->id = highestIx + 1;
|
thing->id = highestIx + 1;
|
||||||
this->things.push_back(thing);
|
this->things.push_back(thing);
|
||||||
#endif
|
#endif
|
||||||
std::cout << "Add thing with generated ID "
|
// std::cout << "Add thing with generated ID " << this->ipAddress << ":"
|
||||||
//<< this->ipAddress << ":" << this->port
|
// << this->port << "[" << (int)thing->id << "]\n";
|
||||||
<< "[" << (int)thing->id << "]\n";
|
|
||||||
} else {
|
} else {
|
||||||
Thing* foundThing = Get(thing->owner->networkId, thing->id);
|
Thing* foundThing = Get(thing->id);
|
||||||
if (foundThing == nullptr) {
|
if (foundThing == nullptr) {
|
||||||
#if defined(NO_STD)
|
#if defined(NO_STD)
|
||||||
this->things[this->thingCount++] = thing;
|
this->things[this->thingCount++] = thing;
|
||||||
#else
|
#else
|
||||||
this->things.push_back(thing);
|
this->things.push_back(thing);
|
||||||
#endif
|
#endif
|
||||||
std::cout << "Add thing " << //this->ipAddress << ":" << this->port <<
|
// std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
|
||||||
"["
|
// "["
|
||||||
<< (int)thing->id << "]\n";
|
// << (int)thing->id << "]\n";
|
||||||
} else {
|
} else {
|
||||||
std::cout << "Did not add, existing thing "
|
// std::cout << "Did not add, existing thing " << this->ipAddress << ":"
|
||||||
//<< this->ipAddress << ":" << this->port
|
// << this->port << "[" << (int)thing->id << "]\n";
|
||||||
<< "[" << (int)thing->id << "]\n";
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -164,88 +128,86 @@ void Participant::Remove(Thing* thing) {
|
|||||||
|
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
|
||||||
// #pragma region ParticipantRegistry
|
#pragma region ParticipantRegistry
|
||||||
|
|
||||||
// /*
|
Participant* ParticipantRegistry::Get(const char* ipAddress,
|
||||||
// Participant* ParticipantRegistry::Get(const char* ipAddress,
|
unsigned int port) {
|
||||||
// unsigned int port) {
|
#if !defined(NO_STD)
|
||||||
// #if !defined(NO_STD)
|
for (Participant* participant : ParticipantRegistry::participants) {
|
||||||
// for (Participant* participant : ParticipantRegistry::participants) {
|
if (participant == nullptr)
|
||||||
// if (participant == nullptr)
|
continue;
|
||||||
// continue;
|
if (strcmp(participant->ipAddress, ipAddress) == 0 &&
|
||||||
// if (strcmp(participant->ipAddress, ipAddress) == 0 &&
|
participant->port == port) {
|
||||||
// participant->port == port) {
|
// std::cout << "found participant " << participant->ipAddress << ":"
|
||||||
// // std::cout << "found participant " << participant->ipAddress << ":"
|
// << (int)participant->port << std::endl;
|
||||||
// // << (int)participant->port << std::endl;
|
return participant;
|
||||||
// return participant;
|
}
|
||||||
// }
|
}
|
||||||
// }
|
std::cout << "Could not find participant " << ipAddress << ":" << (int)port
|
||||||
// std::cout << "Could not find participant " << ipAddress << ":" << (int)port
|
<< std::endl;
|
||||||
// << std::endl;
|
#endif
|
||||||
// #endif
|
return nullptr;
|
||||||
// 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,
|
Participant* ParticipantRegistry::Get(unsigned char participantId) {
|
||||||
// // unsigned int port) {
|
#if !defined(NO_STD)
|
||||||
// // Participant* participant = new Participant(ipAddress, port);
|
for (Participant* participant : ParticipantRegistry::participants) {
|
||||||
// // Add(participant);
|
if (participant == nullptr)
|
||||||
// // return participant;
|
continue;
|
||||||
// // }
|
if (participant->networkId == participantId)
|
||||||
|
return participant;
|
||||||
|
}
|
||||||
|
std::cout << "Could not find participant " << (int)participantId << std::endl;
|
||||||
|
#endif
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
// void ParticipantRegistry::Add(Participant* participant) {
|
Participant* ParticipantRegistry::Add(const char* ipAddress,
|
||||||
// Participant* foundParticipant =
|
unsigned int port) {
|
||||||
// Get(participant->networkId);
|
Participant* participant = new Participant(ipAddress, port);
|
||||||
// //Get(participant->ipAddress, participant->port);
|
Add(participant);
|
||||||
|
return participant;
|
||||||
|
}
|
||||||
|
|
||||||
// if (foundParticipant == nullptr) {
|
void ParticipantRegistry::Add(Participant* participant) {
|
||||||
// #if defined(NO_STD)
|
Participant* foundParticipant =
|
||||||
// // this->things[this->thingCount++] = thing;
|
Get(participant->ipAddress, participant->port);
|
||||||
// #else
|
|
||||||
// ParticipantRegistry::participants.push_back(participant);
|
|
||||||
// #endif
|
|
||||||
// // std::cout << "Add participant " << participant->ipAddress << ":"
|
|
||||||
// // << participant->port << "[" << (int)participant->networkId
|
|
||||||
// // << "]\n";
|
|
||||||
// // std::cout << "participants " <<
|
|
||||||
// // ParticipantRegistry::participants.size()
|
|
||||||
// // << "\n";
|
|
||||||
// // } else {
|
|
||||||
// // std::cout << "Did not add, existing participant " <<
|
|
||||||
// // participant->ipAddress
|
|
||||||
// // << ":" << participant->port << "[" <<
|
|
||||||
// // (int)participant->networkId
|
|
||||||
// // << "]\n";
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
|
|
||||||
// void ParticipantRegistry::Remove(Participant* participant) {
|
if (foundParticipant == nullptr) {
|
||||||
// // participants.remove(participant);
|
#if defined(NO_STD)
|
||||||
// }
|
// this->things[this->thingCount++] = thing;
|
||||||
|
#else
|
||||||
|
ParticipantRegistry::participants.push_back(participant);
|
||||||
|
#endif
|
||||||
|
// std::cout << "Add participant " << participant->ipAddress << ":"
|
||||||
|
// << participant->port << "[" << (int)participant->networkId
|
||||||
|
// << "]\n";
|
||||||
|
// std::cout << "participants " <<
|
||||||
|
// ParticipantRegistry::participants.size()
|
||||||
|
// << "\n";
|
||||||
|
// } else {
|
||||||
|
// std::cout << "Did not add, existing participant " <<
|
||||||
|
// participant->ipAddress
|
||||||
|
// << ":" << participant->port << "[" <<
|
||||||
|
// (int)participant->networkId
|
||||||
|
// << "]\n";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// #if defined(NO_STD)
|
void ParticipantRegistry::Remove(Participant* participant) {
|
||||||
// Participant** ParticipantRegistry::GetAll() const {
|
// participants.remove(participant);
|
||||||
// return ParticipantRegistry::participants;
|
}
|
||||||
// }
|
|
||||||
// #else
|
|
||||||
// const std::list<Participant*>& ParticipantRegistry::GetAll() const {
|
|
||||||
// return ParticipantRegistry::participants;
|
|
||||||
// }
|
|
||||||
// #endif
|
|
||||||
|
|
||||||
// #pragma endregion ParticipantRegistry
|
#if defined(NO_STD)
|
||||||
|
Participant** ParticipantRegistry::GetAll() const {
|
||||||
|
return ParticipantRegistry::participants;
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
const std::list<Participant*>& ParticipantRegistry::GetAll() const {
|
||||||
|
return ParticipantRegistry::participants;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#pragma endregion ParticipantRegistry
|
||||||
|
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
|
@ -1,13 +1,11 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "Messages/IMessage.h"
|
|
||||||
#include "Thing.h"
|
#include "Thing.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
constexpr int MAX_THING_COUNT = 256;
|
constexpr int MAX_THING_COUNT = 256;
|
||||||
|
|
||||||
/*
|
|
||||||
/// @brief class which manages all known participants
|
/// @brief class which manages all known participants
|
||||||
class ParticipantRegistry {
|
class ParticipantRegistry {
|
||||||
public:
|
public:
|
||||||
@ -15,7 +13,7 @@ class ParticipantRegistry {
|
|||||||
/// @param ipAddress The IP address of the participant
|
/// @param ipAddress The IP address of the participant
|
||||||
/// @param port The port number of the participant
|
/// @param port The port number of the participant
|
||||||
/// @return The participant or a nullptr when it could not be found
|
/// @return The participant or a nullptr when it could not be found
|
||||||
//Participant* Get(const char* ipAddress, unsigned int port);
|
Participant* Get(const char* ipAddress, unsigned int port);
|
||||||
/// @brief Retrieve a participant by its network ID
|
/// @brief Retrieve a participant by its network ID
|
||||||
/// @param networkID The network ID of the participant
|
/// @param networkID The network ID of the participant
|
||||||
/// @return The participant or a nullptr when it could not be found
|
/// @return The participant or a nullptr when it could not be found
|
||||||
@ -25,8 +23,7 @@ class ParticipantRegistry {
|
|||||||
/// @param ipAddress The IP address of the participant
|
/// @param ipAddress The IP address of the participant
|
||||||
/// @param port The port number of the participant
|
/// @param port The port number of the participant
|
||||||
/// @return The added participant
|
/// @return The added participant
|
||||||
//Participant* Add(const char* ipAddress, unsigned int port);
|
Participant* Add(const char* ipAddress, unsigned int port);
|
||||||
|
|
||||||
/// @brief Add a participant
|
/// @brief Add a participant
|
||||||
/// @param participant The participant to add
|
/// @param participant The participant to add
|
||||||
void Add(Participant* participant);
|
void Add(Participant* participant);
|
||||||
@ -54,7 +51,6 @@ class ParticipantRegistry {
|
|||||||
std::list<Participant*> participants;
|
std::list<Participant*> participants;
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
*/
|
|
||||||
|
|
||||||
/// @brief A participant is a device which manages things.
|
/// @brief A participant is a device which manages things.
|
||||||
/// It can communicate with other participant to synchronise the state of
|
/// It can communicate with other participant to synchronise the state of
|
||||||
@ -63,39 +59,30 @@ class ParticipantRegistry {
|
|||||||
/// participant. It is used as a basis for the local participant, but also as a
|
/// participant. It is used as a basis for the local participant, but also as a
|
||||||
/// reference to remote participants.
|
/// reference to remote participants.
|
||||||
class Participant {
|
class Participant {
|
||||||
#pragma region Init
|
|
||||||
|
|
||||||
public:
|
|
||||||
/// @brief Create a generic participant
|
|
||||||
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
|
|
||||||
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);
|
|
||||||
|
|
||||||
#pragma endregion Init
|
|
||||||
|
|
||||||
#pragma region Properties
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// @brief The name of the participant
|
/// @brief The name of the participant
|
||||||
const char* name = "Participant";
|
const char* name = "Participant";
|
||||||
|
|
||||||
|
/// @brief The Ip Address of a participant.
|
||||||
|
const char* ipAddress = "0.0.0.0";
|
||||||
|
/// @brief The port number for UDP communication with the participant.
|
||||||
|
unsigned int port = 0;
|
||||||
|
|
||||||
/// @brief The network Id to identify the participant
|
/// @brief The network Id to identify the participant
|
||||||
unsigned char networkId = 0;
|
unsigned char networkId = 0;
|
||||||
|
|
||||||
/// @brief The root thing for this participant
|
Participant();
|
||||||
Thing* root = nullptr;
|
/// @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
|
||||||
|
Participant(const char* ipAddress, int port);
|
||||||
|
/// @brief Destructor for the participant
|
||||||
|
~Participant();
|
||||||
|
|
||||||
|
static Participant* LocalParticipant;
|
||||||
|
static void ReplaceLocalParticipant(Participant& newParticipant);
|
||||||
|
|
||||||
|
Thing* root = new Thing(this);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
#if defined(NO_STD)
|
#if defined(NO_STD)
|
||||||
@ -106,10 +93,9 @@ class Participant {
|
|||||||
std::list<Thing*> things;
|
std::list<Thing*> things;
|
||||||
#endif
|
#endif
|
||||||
/// @brief Find a thing managed by this participant
|
/// @brief Find a thing managed by this participant
|
||||||
/// @param networkId The network ID of the thing
|
|
||||||
/// @param thingId The ID of the thing
|
/// @param thingId The ID of the thing
|
||||||
/// @return The thing if found, nullptr when no thing has been found
|
/// @return The thing if found, nullptr when no thing has been found
|
||||||
Thing* Get(unsigned char networkId, unsigned char thingId);
|
Thing* Get(unsigned char thingId);
|
||||||
/// @brief Add a new thing for this participant.
|
/// @brief Add a new thing for this participant.
|
||||||
/// @param thing The thing to add
|
/// @param thing The thing to add
|
||||||
/// @param checkId If true, the thing.id is regenerated if it is zero
|
/// @param checkId If true, the thing.id is regenerated if it is zero
|
||||||
@ -118,31 +104,12 @@ class Participant {
|
|||||||
/// @param thing The thing to remove
|
/// @param thing The thing to remove
|
||||||
void Remove(Thing* thing);
|
void Remove(Thing* thing);
|
||||||
|
|
||||||
#pragma endregion Properties
|
|
||||||
|
|
||||||
#pragma region Update
|
|
||||||
|
|
||||||
public:
|
|
||||||
/// @brief Update all things for this participant
|
/// @brief Update all things for this participant
|
||||||
virtual void Update(bool recurse = true);
|
/// @param currentTimeMs The current time in milliseconds (optional)
|
||||||
|
virtual void Update();
|
||||||
#pragma endregion Update
|
|
||||||
|
|
||||||
#pragma region Send
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
char buffer[1024];
|
static ParticipantRegistry registry;
|
||||||
|
|
||||||
virtual bool Send(IMessage* msg);
|
|
||||||
|
|
||||||
#pragma endregion Send
|
|
||||||
|
|
||||||
// #pragma region Participant Registry
|
|
||||||
|
|
||||||
// public:
|
|
||||||
// static ParticipantRegistry registry;
|
|
||||||
|
|
||||||
// #pragma endregion Participant Registry
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
|
@ -1,16 +1,14 @@
|
|||||||
/*
|
|
||||||
#include "IsolatedParticipant.h"
|
#include "IsolatedParticipant.h"
|
||||||
#include "ParticipantUDP.h"
|
#include "ParticipantUDP.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
static ParticipantUDPGeneric* isolatedParticipant = nullptr;
|
static ParticipantUDP* isolatedParticipant = nullptr;
|
||||||
|
|
||||||
Participant* IsolatedParticipant::Isolated() {
|
Participant* IsolatedParticipant::Isolated() {
|
||||||
if (isolatedParticipant == nullptr)
|
if (isolatedParticipant == nullptr)
|
||||||
isolatedParticipant = new ParticipantUDPGeneric(0);
|
isolatedParticipant = new ParticipantUDP(0);
|
||||||
return isolatedParticipant;
|
return isolatedParticipant;
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
*/
|
|
@ -1,4 +1,3 @@
|
|||||||
/*
|
|
||||||
#include "Participant.h"
|
#include "Participant.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
@ -12,4 +11,3 @@ class IsolatedParticipant {
|
|||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
*/
|
|
@ -1,154 +0,0 @@
|
|||||||
#include "ParticipantMQTT.h"
|
|
||||||
|
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
|
||||||
#include <arpa/inet.h>
|
|
||||||
#include <fcntl.h> // For fcntl
|
|
||||||
#include <netinet/in.h>
|
|
||||||
#include <sys/socket.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "Participants/ParticipantUDP.h"
|
|
||||||
|
|
||||||
namespace RoboidControl {
|
|
||||||
|
|
||||||
MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port)
|
|
||||||
: RemoteParticipantUDP("127.0.0.1", port) {
|
|
||||||
this->name = "ParticipantUDP";
|
|
||||||
this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
|
|
||||||
// Participant::registry.Add(this);
|
|
||||||
|
|
||||||
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
|
|
||||||
this->root->owner = this;
|
|
||||||
this->root->name = "UDP Root";
|
|
||||||
this->Add(this->root);
|
|
||||||
|
|
||||||
Participant::ReplaceLocalParticipant(*this);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MQTTParticipantBase::SetupTCP() {
|
|
||||||
this->connected = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
#define MQTT_CONNECT 0x10
|
|
||||||
#define MQTT_CONNECT_ACK 0x20
|
|
||||||
|
|
||||||
void MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
|
|
||||||
MQTTConnectPacket packet;
|
|
||||||
packet.fixed_header = MQTT_CONNECT;
|
|
||||||
packet.remaining_length = // 14;
|
|
||||||
2 + 4 + 2 + strlen(client_id) +
|
|
||||||
3; // Protocol name + protocol level + connect
|
|
||||||
// flags + keep alive + client ID
|
|
||||||
packet.protocol_name_length = 4; // "MQTT"
|
|
||||||
packet.protocol_name = "MQTT";
|
|
||||||
packet.protocol_level = 4; // MQTT version 3.1.1
|
|
||||||
packet.connect_flags = 2; // Clean session
|
|
||||||
packet.keep_alive = htons(15); // Keep alive time in seconds
|
|
||||||
|
|
||||||
// Create the MQTT connect packet
|
|
||||||
int index = 0;
|
|
||||||
|
|
||||||
this->buffer[index++] = packet.fixed_header;
|
|
||||||
this->buffer[index++] = packet.remaining_length;
|
|
||||||
this->buffer[index++] = 0; // MSB protocol_name_length
|
|
||||||
this->buffer[index++] = packet.protocol_name_length;
|
|
||||||
memcpy(&this->buffer[index], packet.protocol_name,
|
|
||||||
packet.protocol_name_length);
|
|
||||||
index += packet.protocol_name_length;
|
|
||||||
this->buffer[index++] = packet.protocol_level;
|
|
||||||
this->buffer[index++] = packet.connect_flags;
|
|
||||||
this->buffer[index++] = packet.keep_alive & 0xFF; // LSB
|
|
||||||
this->buffer[index++] = (packet.keep_alive >> 8) & 0xFF; // MSB
|
|
||||||
size_t client_id_length = strlen(client_id);
|
|
||||||
this->buffer[index++] = (client_id_length >> 8) & 0xFF; // MSB
|
|
||||||
this->buffer[index++] = client_id_length & 0xFF; // LSB
|
|
||||||
memcpy(&this->buffer[index], client_id, client_id_length);
|
|
||||||
index += client_id_length;
|
|
||||||
|
|
||||||
for (int ix = 0; ix < index; ix++)
|
|
||||||
std::cout << std::hex << "0x" << (int)this->buffer[ix] << " ";
|
|
||||||
std::cout << std::dec << std::endl;
|
|
||||||
|
|
||||||
// Send the MQTT connect packet
|
|
||||||
SendTCP(index);
|
|
||||||
|
|
||||||
std::cout << "Send connect, client ID = " << client_id << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
void MQTTParticipantBase::sendSubscribe(const char* topic) {
|
|
||||||
// Packet Identifier (2 bytes)
|
|
||||||
static unsigned short packetId =
|
|
||||||
1; // Increment this for each new subscription
|
|
||||||
|
|
||||||
// Calculate the length of the topic name
|
|
||||||
size_t topicLength = strlen(topic);
|
|
||||||
|
|
||||||
// Remaining length = Packet Identifier (2 bytes) + Topic Length (2 bytes) +
|
|
||||||
// Topic Name + QoS (1 byte)
|
|
||||||
size_t remainingLength = 1 + 2 + 2 + topicLength + 1;
|
|
||||||
|
|
||||||
// Construct the SUBSCRIBE packet
|
|
||||||
// unsigned char subscribePacket[3 + topicLength]; // 3 bytes for fixed
|
|
||||||
// header
|
|
||||||
// // and packet ID
|
|
||||||
this->buffer[0] = (char)0x82; // Packet type and flags
|
|
||||||
this->buffer[1] = (char)remainingLength; // Remaining length
|
|
||||||
this->buffer[2] = (packetId >> 8) & 0xFF; // Packet Identifier MSB
|
|
||||||
this->buffer[3] = packetId & 0xFF; // Packet Identifier LSB
|
|
||||||
this->buffer[4] = (topicLength >> 8) & 0xFF; // Topic Length MSB
|
|
||||||
this->buffer[5] = topicLength & 0xFF; // Topic Length LSB
|
|
||||||
|
|
||||||
// Copy the topic name into the packet
|
|
||||||
memcpy(&this->buffer[6], topic, topicLength);
|
|
||||||
this->buffer[6 + topicLength] = 0x00; // QoS level (0)
|
|
||||||
|
|
||||||
int index = 7 + topicLength;
|
|
||||||
// for (int ix = 0; ix < index; ix++)
|
|
||||||
// std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n";
|
|
||||||
|
|
||||||
// Send the SUBSCRIBE packet
|
|
||||||
SendTCP(7 + topicLength);
|
|
||||||
|
|
||||||
std::cout << "Send subscribe to topic: " << topic << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
void MQTTParticipantBase::Update(bool recurse) {
|
|
||||||
int packetSize = ReceiveTCP();
|
|
||||||
if (packetSize > 0) {
|
|
||||||
ReceiveData(packetSize);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
void MQTTParticipantBase::SendTCP(int bufferSize) {}
|
|
||||||
|
|
||||||
int MQTTParticipantBase::ReceiveTCP() {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) {
|
|
||||||
std::cout << " receivemsg\n";
|
|
||||||
// std::cout << "receive msg " << (int)msgId << "\n";
|
|
||||||
// std::cout << " buffer size = " <<(int) bufferSize << "\n";
|
|
||||||
};
|
|
||||||
|
|
||||||
// void ParticipantMQTT::receiveMessages(int sock) {
|
|
||||||
// unsigned char buffer[1024];
|
|
||||||
// // Check for incoming messages without blocking
|
|
||||||
// int bytesRead = recv(sock, buffer, sizeof(buffer), MSG_DONTWAIT);
|
|
||||||
// if (bytesRead > 0) {
|
|
||||||
// // Process the incoming MQTT message
|
|
||||||
// std::cout << "Received message: ";
|
|
||||||
// for (int i = 0; i < bytesRead; ++i) {
|
|
||||||
// std::cout << std::hex << (int)buffer[i] << " ";
|
|
||||||
// }
|
|
||||||
// std::cout << std::dec << std::endl; // Reset to decimal output
|
|
||||||
// } else if (bytesRead < 0) {
|
|
||||||
// // If no messages are available, check for errors
|
|
||||||
// if (errno != EAGAIN && errno != EWOULDBLOCK) {
|
|
||||||
// std::cerr << "Error receiving message: " << strerror(errno) <<
|
|
||||||
// std::endl;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
|
|
||||||
} // namespace RoboidControl
|
|
@ -1,130 +0,0 @@
|
|||||||
#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/ThingMsg.h"
|
|
||||||
#include "Participant.h"
|
|
||||||
#include "Participants/ParticipantUDP.h"
|
|
||||||
|
|
||||||
#if !defined(NO_STD)
|
|
||||||
#include <functional>
|
|
||||||
#include <list>
|
|
||||||
// #include <unordered_map>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
|
||||||
#include "Windows/WindowsParticipant.h"
|
|
||||||
#elif defined(__unix__) || defined(__APPLE__)
|
|
||||||
#include <arpa/inet.h>
|
|
||||||
#include <netinet/in.h>
|
|
||||||
#include <sys/socket.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include "Posix/PosixParticipant.h"
|
|
||||||
#endif
|
|
||||||
|
|
||||||
namespace RoboidControl {
|
|
||||||
|
|
||||||
/// @brief A participant using UDP communication
|
|
||||||
/// A local participant is the local device which can communicate with
|
|
||||||
/// other participants It manages all local things and communcation with other
|
|
||||||
/// participants. Each application has a local participant which is usually
|
|
||||||
/// explicit in the code. An participant can be isolated. In that case it is
|
|
||||||
/// standalong and does not communicate with other participants.
|
|
||||||
///
|
|
||||||
/// It is possible to work with an hidden participant by creating things without
|
|
||||||
/// specifying a participant in the constructor. In that case an hidden isolated
|
|
||||||
/// participant is created which can be obtained using
|
|
||||||
/// RoboidControl::IsolatedParticipant::Isolated().
|
|
||||||
/// @sa RoboidControl::Thing::Thing()
|
|
||||||
class MQTTParticipantBase : public RemoteParticipantUDP {
|
|
||||||
#pragma region Init
|
|
||||||
|
|
||||||
public:
|
|
||||||
/// @brief Create a participant which will try to connect to a MQTT server.
|
|
||||||
/// @param ipAddress The IP address of the MQTT server
|
|
||||||
/// @param port The port used by the MQTT server
|
|
||||||
MQTTParticipantBase(const char* ipAddress, int port = 1883);
|
|
||||||
protected:
|
|
||||||
virtual void SetupTCP();
|
|
||||||
|
|
||||||
#pragma endregion Init
|
|
||||||
|
|
||||||
#pragma region Properties
|
|
||||||
|
|
||||||
public:
|
|
||||||
/// @brief The remote site when this participant is connected to a site
|
|
||||||
RemoteParticipantUDP* remoteSite = nullptr;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
|
|
||||||
public:
|
|
||||||
// void Begin();
|
|
||||||
bool connected = false;
|
|
||||||
|
|
||||||
#pragma endregion Properties
|
|
||||||
|
|
||||||
#pragma region Update
|
|
||||||
|
|
||||||
public:
|
|
||||||
virtual void Update(bool recurse = true) override;
|
|
||||||
|
|
||||||
#pragma endregion Update
|
|
||||||
|
|
||||||
#pragma region Send
|
|
||||||
|
|
||||||
protected:
|
|
||||||
// MQTT Connect Packet Structure
|
|
||||||
struct MQTTConnectPacket {
|
|
||||||
uint8_t fixed_header;
|
|
||||||
uint8_t remaining_length;
|
|
||||||
uint8_t protocol_name_length;
|
|
||||||
const char* protocol_name;
|
|
||||||
uint8_t protocol_level;
|
|
||||||
uint8_t connect_flags;
|
|
||||||
uint16_t keep_alive;
|
|
||||||
const char* client_id;
|
|
||||||
};
|
|
||||||
|
|
||||||
void send_mqtt_connect(const char* client_id);
|
|
||||||
void sendSubscribe(const char* topic);
|
|
||||||
virtual void SendTCP(int bufferSize);
|
|
||||||
|
|
||||||
#pragma endregion Send
|
|
||||||
|
|
||||||
#pragma region Receive
|
|
||||||
|
|
||||||
protected:
|
|
||||||
virtual int ReceiveTCP();
|
|
||||||
void ReceiveData(unsigned char bufferSize);
|
|
||||||
// void ReceiveData(unsigned char bufferSize, Participant*
|
|
||||||
// remoteParticipant);
|
|
||||||
|
|
||||||
// void SetupUDP(int localPort, const char* remoteIpAddress, int
|
|
||||||
// remotePort);
|
|
||||||
|
|
||||||
// void ReceiveUDP();
|
|
||||||
|
|
||||||
// virtual void Process(Participant* sender, ParticipantMsg* msg);
|
|
||||||
// virtual void Process(Participant* sender, NetworkIdMsg* msg);
|
|
||||||
// virtual void Process(Participant* sender, InvestigateMsg* msg);
|
|
||||||
// virtual void Process(Participant* sender, ThingMsg* msg);
|
|
||||||
// virtual void Process(Participant* sender, NameMsg* msg);
|
|
||||||
// virtual void Process(Participant* sender, ModelUrlMsg* msg);
|
|
||||||
// virtual void Process(Participant* sender, PoseMsg* msg);
|
|
||||||
// virtual void Process(Participant* sender, BinaryMsg* msg);
|
|
||||||
// virtual void Process(Participant* sender, TextMsg* msg);
|
|
||||||
// virtual void Process(Participant* sender, DestroyMsg* msg);
|
|
||||||
|
|
||||||
#pragma endregion Receive
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace RoboidControl
|
|
||||||
|
|
||||||
#include "Posix/PosixMQTT.h"
|
|
@ -8,167 +8,81 @@
|
|||||||
#include "Posix/PosixParticipant.h"
|
#include "Posix/PosixParticipant.h"
|
||||||
#include "Windows/WindowsParticipant.h"
|
#include "Windows/WindowsParticipant.h"
|
||||||
|
|
||||||
#include "Things/DifferentialDrive.h"
|
|
||||||
#include "Things/DistanceSensor.h"
|
|
||||||
#include "Things/TouchSensor.h"
|
|
||||||
|
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
#pragma region ParticipantRegistry
|
|
||||||
|
|
||||||
ParticipantRegistry ParticipantUDPGeneric::registry;
|
|
||||||
|
|
||||||
RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress,
|
|
||||||
unsigned int port) {
|
|
||||||
#if !defined(NO_STD)
|
|
||||||
for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
|
|
||||||
if (participant == nullptr)
|
|
||||||
continue;
|
|
||||||
if (strcmp(participant->ipAddress, ipAddress) == 0 &&
|
|
||||||
participant->port == port) {
|
|
||||||
// std::cout << "found participant " << participant->ipAddress << ":"
|
|
||||||
// << (int)participant->port << std::endl;
|
|
||||||
return participant;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
std::cout << "Could not find participant " << ipAddress << ":" << (int)port
|
|
||||||
<< std::endl;
|
|
||||||
#endif
|
|
||||||
return nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) {
|
|
||||||
#if !defined(NO_STD)
|
|
||||||
for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
|
|
||||||
if (participant == nullptr)
|
|
||||||
continue;
|
|
||||||
if (participant->networkId == participantId)
|
|
||||||
return participant;
|
|
||||||
}
|
|
||||||
std::cout << "Could not find participant " << (int)participantId << std::endl;
|
|
||||||
#endif
|
|
||||||
return nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress,
|
|
||||||
unsigned int port) {
|
|
||||||
RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port);
|
|
||||||
Add(participant);
|
|
||||||
return participant;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ParticipantRegistry::Add(RemoteParticipantUDP* participant) {
|
|
||||||
Participant* foundParticipant = Get(participant->networkId);
|
|
||||||
// Get(participant->ipAddress, participant->port);
|
|
||||||
|
|
||||||
if (foundParticipant == nullptr) {
|
|
||||||
#if defined(NO_STD)
|
|
||||||
// this->things[this->thingCount++] = thing;
|
|
||||||
#else
|
|
||||||
ParticipantRegistry::participants.push_back(participant);
|
|
||||||
#endif
|
|
||||||
// std::cout << "Add participant " << participant->ipAddress << ":"
|
|
||||||
// << participant->port << "[" << (int)participant->networkId
|
|
||||||
// << "]\n";
|
|
||||||
// std::cout << "participants " <<
|
|
||||||
// ParticipantRegistry::participants.size()
|
|
||||||
// << "\n";
|
|
||||||
// } else {
|
|
||||||
// std::cout << "Did not add, existing participant " <<
|
|
||||||
// participant->ipAddress
|
|
||||||
// << ":" << participant->port << "[" <<
|
|
||||||
// (int)participant->networkId
|
|
||||||
// << "]\n";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void ParticipantRegistry::Remove(RemoteParticipantUDP* participant) {
|
|
||||||
// participants.remove(participant);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(NO_STD)
|
|
||||||
RemoteParticipantUDP** ParticipantRegistry::GetAll() const {
|
|
||||||
return ParticipantRegistry::participants;
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
const std::list<RemoteParticipantUDP*>& ParticipantRegistry::GetAll() const {
|
|
||||||
return ParticipantRegistry::participants;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#pragma endregion ParticipantRegistry
|
|
||||||
|
|
||||||
RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) {
|
|
||||||
// make a copy of the ip address string
|
|
||||||
int addressLength = (int)strlen(ipAddress);
|
|
||||||
int stringLength = addressLength + 1;
|
|
||||||
char* addressString = new char[stringLength];
|
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
|
||||||
strncpy_s(addressString, stringLength, ipAddress,
|
|
||||||
addressLength); // Leave space for null terminator
|
|
||||||
#else
|
|
||||||
strncpy(addressString, ipAddress, addressLength);
|
|
||||||
#endif
|
|
||||||
addressString[addressLength] = '\0';
|
|
||||||
|
|
||||||
this->ipAddress = addressString;
|
|
||||||
this->port = port;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool RemoteParticipantUDP::Send(IMessage* msg) {
|
|
||||||
// No message is actually sent, because this class has no networking
|
|
||||||
// implementation
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
#pragma region Init
|
#pragma region Init
|
||||||
|
|
||||||
ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
|
ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
|
||||||
: RemoteParticipantUDP("127.0.0.1", port) {
|
|
||||||
this->name = "ParticipantUDP";
|
this->name = "ParticipantUDP";
|
||||||
this->remoteSite = nullptr;
|
this->remoteSite = nullptr;
|
||||||
if (this->port == 0)
|
if (this->port == 0)
|
||||||
this->isIsolated = true;
|
this->isIsolated = true;
|
||||||
registry.Add(this);
|
Participant::registry.Add(this);
|
||||||
|
|
||||||
this->root = Thing::LocalRoot(); //::LocalParticipant->root;
|
this->root = Thing::LocalRoot(); //::LocalParticipant->root;
|
||||||
this->root->owner = this;
|
this->root->owner = this;
|
||||||
this->root->name = "UDP Root";
|
this->root->name = "UDP Root";
|
||||||
std::cout << "P2 " << (long)this->root << std::endl;
|
|
||||||
this->Add(this->root);
|
this->Add(this->root);
|
||||||
|
|
||||||
Participant::ReplaceLocalParticipant(*this);
|
Participant::ReplaceLocalParticipant(*this);
|
||||||
}
|
}
|
||||||
|
|
||||||
ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
|
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
|
||||||
int port,
|
: Participant("127.0.0.1", localPort) {
|
||||||
int localPort)
|
|
||||||
: RemoteParticipantUDP("127.0.0.1", localPort) {
|
|
||||||
this->name = "ParticipantUDP";
|
this->name = "ParticipantUDP";
|
||||||
if (this->port == 0)
|
if (this->port == 0)
|
||||||
this->isIsolated = true;
|
this->isIsolated = true;
|
||||||
else
|
else
|
||||||
this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
|
this->remoteSite = new Participant(ipAddress, port);
|
||||||
registry.Add(this);
|
Participant::registry.Add(this);
|
||||||
|
|
||||||
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
|
this->root = Thing::LocalRoot(); // Participant::LocalParticipant->root;
|
||||||
this->root->owner = this;
|
this->root->owner = this;
|
||||||
this->root->name = "UDP Root";
|
this->root->name = "UDP Root";
|
||||||
std::cout << "P1 " << (long)this->root << std::endl;
|
|
||||||
this->Add(this->root);
|
this->Add(this->root);
|
||||||
|
|
||||||
Participant::ReplaceLocalParticipant(*this);
|
Participant::ReplaceLocalParticipant(*this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::begin() {
|
static ParticipantUDP* isolatedParticipant = nullptr;
|
||||||
|
|
||||||
|
ParticipantUDP* ParticipantUDP::Isolated() {
|
||||||
|
if (isolatedParticipant == nullptr)
|
||||||
|
isolatedParticipant = new ParticipantUDP(0);
|
||||||
|
return isolatedParticipant;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ParticipantUDP::begin() {
|
||||||
if (this->isIsolated || this->remoteSite == nullptr)
|
if (this->isIsolated || this->remoteSite == nullptr)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
|
SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void ParticipantUDP::SetupUDP(int localPort,
|
||||||
|
const char* remoteIpAddress,
|
||||||
|
int remotePort) {
|
||||||
|
#if defined(_WIN32) || defined(_WIN64)
|
||||||
|
Windows::ParticipantUDP* thisWindows =
|
||||||
|
static_cast<Windows::ParticipantUDP*>(this);
|
||||||
|
thisWindows->Setup(localPort, remoteIpAddress, remotePort);
|
||||||
|
#elif defined(__unix__) || defined(__APPLE__)
|
||||||
|
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
|
||||||
|
thisPosix->Setup(localPort, remoteIpAddress, remotePort);
|
||||||
|
#elif defined(ARDUINO)
|
||||||
|
Arduino::ParticipantUDP* thisArduino =
|
||||||
|
static_cast<Arduino::ParticipantUDP*>(this);
|
||||||
|
thisArduino->Setup();
|
||||||
|
#elif defined(IDF_VER)
|
||||||
|
EspIdf::ParticipantUDP* thisEspIdf =
|
||||||
|
static_cast<EspIdf::ParticipantUDP*>(this);
|
||||||
|
thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
|
||||||
|
#endif
|
||||||
|
this->connected = true;
|
||||||
|
}
|
||||||
|
|
||||||
#pragma endregion Init
|
#pragma endregion Init
|
||||||
|
|
||||||
#pragma region Update
|
#pragma region Update
|
||||||
@ -177,55 +91,105 @@ void ParticipantUDPGeneric::begin() {
|
|||||||
// 1. receive external messages
|
// 1. receive external messages
|
||||||
// 2. update the state
|
// 2. update the state
|
||||||
// 3. send out the updated messages
|
// 3. send out the updated messages
|
||||||
void ParticipantUDPGeneric::Update(bool recurse) {
|
void ParticipantUDP::Update() {
|
||||||
unsigned long currentTimeMs = Thing::GetTimeMs();
|
unsigned long currentTimeMs = Thing::GetTimeMs();
|
||||||
|
|
||||||
|
PrepMyThings();
|
||||||
|
|
||||||
if (this->isIsolated == false) {
|
if (this->isIsolated == false) {
|
||||||
if (this->connected == false)
|
if (this->connected == false)
|
||||||
begin();
|
begin();
|
||||||
|
|
||||||
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
|
if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
|
||||||
ParticipantMsg* msg = new ParticipantMsg(this->networkId);
|
ParticipantMsg* msg = new ParticipantMsg(this->networkId);
|
||||||
|
|
||||||
if (this->remoteSite == nullptr)
|
if (this->remoteSite == nullptr)
|
||||||
this->Publish(msg);
|
this->Publish(msg);
|
||||||
else
|
else
|
||||||
this->Send(msg);
|
this->Send(this->remoteSite, msg);
|
||||||
|
|
||||||
delete msg;
|
delete msg;
|
||||||
|
|
||||||
this->nextPublishMe = currentTimeMs + this->publishInterval;
|
this->nextPublishMe = currentTimeMs + this->publishInterval;
|
||||||
}
|
}
|
||||||
|
|
||||||
//this->ReceiveUDP();
|
this->ReceiveUDP();
|
||||||
}
|
}
|
||||||
|
|
||||||
UpdateMyThings();
|
UpdateMyThings();
|
||||||
UpdateOtherThings();
|
UpdateOtherThings();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::UpdateMyThings() {
|
void ParticipantUDP::PrepMyThings() {
|
||||||
|
for (Thing* thing : this->things) {
|
||||||
|
if (thing == nullptr)
|
||||||
|
continue;
|
||||||
|
|
||||||
|
thing->PrepareForUpdate();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ParticipantUDP::UpdateMyThings() {
|
||||||
|
// std::cout << this->things.size() << std::endl;
|
||||||
for (Thing* thing : this->things) {
|
for (Thing* thing : this->things) {
|
||||||
if (thing == nullptr) // || thing->GetParent() != nullptr)
|
if (thing == nullptr) // || thing->GetParent() != nullptr)
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
// Why don't we do recursive?
|
// std::cout << thing->name << "\n";
|
||||||
|
if (thing->hierarchyChanged) {
|
||||||
|
if (!(this->isIsolated || this->networkId == 0)) {
|
||||||
|
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
||||||
|
this->Send(this->remoteSite, thingMsg);
|
||||||
|
delete thingMsg;
|
||||||
|
|
||||||
|
if (thing->nameChanged) {
|
||||||
|
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
||||||
|
this->Send(this->remoteSite, nameMsg);
|
||||||
|
delete nameMsg;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// std::cout << "B\n";
|
||||||
|
// Why don't we do recursive?
|
||||||
// Because when a thing creates a thing in the update,
|
// Because when a thing creates a thing in the update,
|
||||||
// that new thing is not sent out (because of hierarchyChanged)
|
// that new thing is not sent out (because of hierarchyChanged)
|
||||||
// before it is updated itself: it is immediatedly updated!
|
// before it is updated itself: it is immediatedly updated!
|
||||||
thing->Update(false);
|
thing->Update(false);
|
||||||
|
|
||||||
|
// std::cout << "C\n";
|
||||||
|
if (!(this->isIsolated || this->networkId == 0)) {
|
||||||
|
if (thing->terminate) {
|
||||||
|
DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
|
||||||
|
this->Send(this->remoteSite, destroyMsg);
|
||||||
|
delete destroyMsg;
|
||||||
|
} else {
|
||||||
|
// Send to remote site
|
||||||
|
if (thing->nameChanged) {
|
||||||
|
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
||||||
|
this->Send(this->remoteSite, nameMsg);
|
||||||
|
delete nameMsg;
|
||||||
|
}
|
||||||
|
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
|
||||||
|
this->Send(this->remoteSite, poseMsg);
|
||||||
|
delete poseMsg;
|
||||||
|
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
|
||||||
|
this->Send(this->remoteSite, binaryMsg);
|
||||||
|
delete binaryMsg;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// std::cout << "D\n";
|
||||||
if (thing->terminate)
|
if (thing->terminate)
|
||||||
this->Remove(thing);
|
this->Remove(thing);
|
||||||
|
// std::cout << "E\n";
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::UpdateOtherThings() {
|
void ParticipantUDP::UpdateOtherThings() {
|
||||||
#if defined(NO_STD)
|
#if defined(NO_STD)
|
||||||
Participant** participants = Participant::registry.GetAll();
|
Participant** participants = Participant::registry.GetAll();
|
||||||
for (int ix = 0; ix < Participant::registry.count; ix++) {
|
for (int ix = 0; ix < Participant::registry.count; ix++) {
|
||||||
Participant* participant = participants[ix];
|
Participant* participant = participants[ix];
|
||||||
#else
|
#else
|
||||||
for (Participant* participant : registry.GetAll()) {
|
for (Participant* participant : Participant::registry.GetAll()) {
|
||||||
#endif
|
#endif
|
||||||
if (participant == nullptr || participant == this)
|
if (participant == nullptr || participant == this)
|
||||||
continue;
|
continue;
|
||||||
@ -239,10 +203,10 @@ void ParticipantUDPGeneric::UpdateOtherThings() {
|
|||||||
|
|
||||||
for (Thing* thing : participant->things) {
|
for (Thing* thing : participant->things) {
|
||||||
PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
|
PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
|
||||||
participant->Send(poseMsg);
|
this->Send(participant, poseMsg);
|
||||||
delete poseMsg;
|
delete poseMsg;
|
||||||
BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
|
BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
|
||||||
participant->Send(binaryMsg);
|
this->Send(participant, binaryMsg);
|
||||||
delete binaryMsg;
|
delete binaryMsg;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -253,34 +217,55 @@ void ParticipantUDPGeneric::UpdateOtherThings() {
|
|||||||
|
|
||||||
#pragma region Send
|
#pragma region Send
|
||||||
|
|
||||||
void ParticipantUDPGeneric::SendThingInfo(Participant* remoteParticipant,
|
void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
|
||||||
Thing* thing) {
|
Thing* thing) {
|
||||||
// std::cout << "Send thing info [" << (int)thing->id << "] \n";
|
// std::cout << "Send thing info [" << (int)thing->id << "] \n";
|
||||||
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
|
||||||
remoteParticipant->Send(thingMsg);
|
this->Send(remoteParticipant, thingMsg);
|
||||||
delete thingMsg;
|
delete thingMsg;
|
||||||
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
NameMsg* nameMsg = new NameMsg(this->networkId, thing);
|
||||||
remoteParticipant->Send(nameMsg);
|
this->Send(remoteParticipant, nameMsg);
|
||||||
delete nameMsg;
|
delete nameMsg;
|
||||||
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
|
ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
|
||||||
remoteParticipant->Send(modelMsg);
|
this->Send(remoteParticipant, modelMsg);
|
||||||
delete modelMsg;
|
delete modelMsg;
|
||||||
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
|
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
|
||||||
remoteParticipant->Send(poseMsg);
|
this->Send(remoteParticipant, poseMsg);
|
||||||
delete poseMsg;
|
delete poseMsg;
|
||||||
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
|
BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
|
||||||
remoteParticipant->Send(binaryMsg);
|
this->Send(remoteParticipant, customMsg);
|
||||||
delete binaryMsg;
|
delete customMsg;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ParticipantUDPGeneric::Send(IMessage* msg) {
|
bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
|
||||||
if (this->remoteSite != nullptr)
|
int bufferSize = msg->Serialize(this->buffer);
|
||||||
return this->remoteSite->Send(msg);
|
if (bufferSize <= 0)
|
||||||
|
return true;
|
||||||
|
|
||||||
return true;
|
// std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
|
||||||
|
// << " to " << remoteParticipant->ipAddress << std::endl;
|
||||||
|
|
||||||
|
#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";
|
// std::cout << "Publish thing info" << thing->networkId << "\n";
|
||||||
// Strange, when publishing, the network id is irrelevant, because it is
|
// Strange, when publishing, the network id is irrelevant, because it is
|
||||||
// connected to a specific site...
|
// connected to a specific site...
|
||||||
@ -301,31 +286,71 @@ void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) {
|
|||||||
delete customMsg;
|
delete customMsg;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool ParticipantUDP::Publish(IMessage* msg) {
|
||||||
|
// std::cout << "publish msg\n";
|
||||||
|
#if defined(_WIN32) || defined(_WIN64)
|
||||||
|
Windows::ParticipantUDP* thisWindows =
|
||||||
|
static_cast<Windows::ParticipantUDP*>(this);
|
||||||
|
return thisWindows->Publish(msg);
|
||||||
|
#elif defined(__unix__) || defined(__APPLE__)
|
||||||
|
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
|
||||||
|
return thisPosix->Publish(msg);
|
||||||
|
#elif defined(ARDUINO)
|
||||||
|
Arduino::ParticipantUDP* thisArduino =
|
||||||
|
static_cast<Arduino::ParticipantUDP*>(this);
|
||||||
|
return thisArduino->Publish(msg);
|
||||||
|
#elif defined(IDF_VER)
|
||||||
|
EspIdf::ParticipantUDP* thisEspIdf =
|
||||||
|
static_cast<EspIdf::ParticipantUDP*>(this);
|
||||||
|
return thisEspIdf->Publish(msg);
|
||||||
|
#else
|
||||||
|
return false;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
// Send
|
// Send
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
|
||||||
#pragma region Receive
|
#pragma region Receive
|
||||||
|
|
||||||
void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
|
void ParticipantUDP::ReceiveUDP() {
|
||||||
char* senderIpAddress,
|
#if defined(_WIN32) || defined(_WIN64)
|
||||||
unsigned int senderPort) {
|
Windows::ParticipantUDP* thisWindows =
|
||||||
|
static_cast<Windows::ParticipantUDP*>(this);
|
||||||
|
thisWindows->Receive();
|
||||||
|
#elif defined(__unix__) || defined(__APPLE__)
|
||||||
|
Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
|
||||||
|
thisPosix->Receive();
|
||||||
|
#elif defined(ARDUINO)
|
||||||
|
Arduino::ParticipantUDP* thisArduino =
|
||||||
|
static_cast<Arduino::ParticipantUDP*>(this);
|
||||||
|
thisArduino->Receive();
|
||||||
|
#elif defined(IDF_VER)
|
||||||
|
EspIdf::ParticipantUDP* thisEspIdf =
|
||||||
|
static_cast<EspIdf::ParticipantUDP*>(this);
|
||||||
|
thisEspIdf->Receive();
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void ParticipantUDP::ReceiveData(unsigned char packetSize,
|
||||||
|
char* senderIpAddress,
|
||||||
|
unsigned int senderPort) {
|
||||||
// std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
|
// std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
|
||||||
// << std::endl;
|
// << std::endl;
|
||||||
RemoteParticipantUDP* sender =
|
Participant* sender = this->registry.Get(senderIpAddress, senderPort);
|
||||||
this->registry.Get(senderIpAddress, senderPort);
|
|
||||||
if (sender == nullptr) {
|
if (sender == nullptr) {
|
||||||
sender = this->registry.Add(senderIpAddress, senderPort);
|
sender = this->registry.Add(senderIpAddress, senderPort);
|
||||||
#if !defined(NO_STD)
|
#if !defined(NO_STD)
|
||||||
// std::cout << "New remote participant " << sender->ipAddress << ":"
|
std::cout << "New remote participant " << sender->ipAddress << ":"
|
||||||
// << sender->port << std::endl;
|
<< sender->port << std::endl;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
ReceiveData(packetSize, sender);
|
ReceiveData(packetSize, sender);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
|
void ParticipantUDP::ReceiveData(unsigned char bufferSize,
|
||||||
RemoteParticipantUDP* sender) {
|
Participant* sender) {
|
||||||
unsigned char msgId = this->buffer[0];
|
unsigned char msgId = this->buffer[0];
|
||||||
// std::cout << "receive msg " << (int)msgId << "\n";
|
// std::cout << "receive msg " << (int)msgId << "\n";
|
||||||
// std::cout << " buffer size = " <<(int) bufferSize << "\n";
|
// std::cout << " buffer size = " <<(int) bufferSize << "\n";
|
||||||
@ -377,18 +402,6 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
|
|||||||
Process(sender, msg);
|
Process(sender, msg);
|
||||||
delete msg;
|
delete msg;
|
||||||
} break;
|
} 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
|
// Check if the buffer has been read completely
|
||||||
@ -398,16 +411,14 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
|
||||||
ParticipantMsg* msg) {
|
|
||||||
#if defined(DEBUG)
|
#if defined(DEBUG)
|
||||||
std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
|
std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
|
||||||
<< "\n";
|
<< "\n";
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
|
||||||
NetworkIdMsg* msg) {
|
|
||||||
#if defined(DEBUG)
|
#if defined(DEBUG)
|
||||||
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
|
std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
|
||||||
<< " -> " << (int)msg->networkId << "\n";
|
<< " -> " << (int)msg->networkId << "\n";
|
||||||
@ -422,68 +433,28 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
|
||||||
InvestigateMsg* msg) {
|
|
||||||
#if defined(DEBUG)
|
#if defined(DEBUG)
|
||||||
std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
|
std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
|
||||||
<< "/" << (int)msg->thingId << "]\n";
|
<< "/" << (int)msg->thingId << "]\n";
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
|
||||||
ThingMsg* msg) {
|
|
||||||
#if defined(DEBUG)
|
#if defined(DEBUG)
|
||||||
std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
|
std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
|
||||||
<< "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
|
<< "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
|
||||||
<< (int)msg->parentId << "\n";
|
<< (int)msg->parentId << "\n";
|
||||||
#endif
|
#endif
|
||||||
RemoteParticipantUDP* owner = registry.Get(msg->networkId);
|
|
||||||
if (owner == nullptr) {
|
|
||||||
owner = new RemoteParticipantUDP(sender->ipAddress, sender->port);
|
|
||||||
owner->networkId = msg->networkId;
|
|
||||||
registry.Add(owner);
|
|
||||||
}
|
|
||||||
|
|
||||||
Thing* thing = owner->Get(msg->networkId, msg->thingId);
|
|
||||||
if (thing == nullptr) {
|
|
||||||
bool isRemote = (sender->networkId != owner->networkId);
|
|
||||||
thing = ProcessNewThing(owner, msg, isRemote);
|
|
||||||
thing->id = msg->thingId;
|
|
||||||
thing->type = msg->thingType;
|
|
||||||
thing->isRemote = isRemote;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (msg->parentId != 0) {
|
|
||||||
thing->SetParent(owner->Get(msg->networkId, msg->parentId));
|
|
||||||
if (thing->GetParent() == nullptr)
|
|
||||||
std::cout << "Could not find parent" << std::endl;
|
|
||||||
} else
|
|
||||||
thing->SetParent(nullptr);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
|
void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
|
||||||
ThingMsg* msg,
|
|
||||||
bool isRemote) {
|
|
||||||
switch (msg->thingType) {
|
|
||||||
case Thing::Type::DistanceSensor:
|
|
||||||
return new DistanceSensor(owner->root);
|
|
||||||
case Thing::Type::TouchSensor:
|
|
||||||
return new TouchSensor(owner->root);
|
|
||||||
case Thing::Type::DifferentialDrive:
|
|
||||||
return new DifferentialDrive(owner->root);
|
|
||||||
default:
|
|
||||||
return new Thing(owner->root);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
|
||||||
NameMsg* msg) {
|
|
||||||
#if defined(DEBUG)
|
#if defined(DEBUG)
|
||||||
std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
|
std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
|
||||||
<< (int)msg->thingId << "] ";
|
<< (int)msg->thingId << "] ";
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
Thing* thing = sender->Get(msg->thingId);
|
||||||
if (thing != nullptr) {
|
if (thing != nullptr) {
|
||||||
int nameLength = msg->nameLength;
|
int nameLength = msg->nameLength;
|
||||||
int stringLen = nameLength + 1;
|
int stringLen = nameLength + 1;
|
||||||
@ -509,25 +480,23 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
|
||||||
ModelUrlMsg* msg) {
|
|
||||||
#if defined(DEBUG)
|
#if defined(DEBUG)
|
||||||
std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
|
std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
|
||||||
<< "/" << (int)msg->thingId << "]\n";
|
<< "/" << (int)msg->thingId << "]\n";
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
|
||||||
PoseMsg* msg) {
|
|
||||||
#if !defined(DEBUG) && !defined(NO_STD)
|
#if !defined(DEBUG) && !defined(NO_STD)
|
||||||
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
|
std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
|
||||||
<< "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
|
<< "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
|
||||||
#endif
|
#endif
|
||||||
Participant* owner = registry.Get(msg->networkId);
|
Participant* owner = Participant::registry.Get(msg->networkId);
|
||||||
if (owner == nullptr)
|
if (owner == nullptr)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
Thing* thing = owner->Get(msg->networkId, msg->thingId);
|
Thing* thing = owner->Get(msg->thingId);
|
||||||
if (thing == nullptr)
|
if (thing == nullptr)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
@ -541,16 +510,15 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
|||||||
thing->SetAngularVelocity(msg->angularVelocity);
|
thing->SetAngularVelocity(msg->angularVelocity);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
|
||||||
BinaryMsg* msg) {
|
|
||||||
#if defined(DEBUG)
|
#if defined(DEBUG)
|
||||||
std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
|
std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
|
||||||
<< "/" << (int)msg->thingId << "]\n";
|
<< "/" << (int)msg->thingId << "]\n";
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
Participant* owner = registry.Get(msg->networkId);
|
Participant* owner = Participant::registry.Get(msg->networkId);
|
||||||
if (owner != nullptr) {
|
if (owner != nullptr) {
|
||||||
Thing* thing = owner->Get(msg->networkId, msg->thingId);
|
Thing* thing = owner->Get(msg->thingId);
|
||||||
if (thing != nullptr)
|
if (thing != nullptr)
|
||||||
thing->ProcessBinary(msg->data);
|
thing->ProcessBinary(msg->data);
|
||||||
#if !defined(NO_STD)
|
#if !defined(NO_STD)
|
||||||
@ -564,26 +532,6 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
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);
|
|
||||||
if (thing != nullptr)
|
|
||||||
this->Remove(thing);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Receive
|
// Receive
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
|
||||||
|
@ -5,10 +5,9 @@
|
|||||||
#include "Messages/InvestigateMsg.h"
|
#include "Messages/InvestigateMsg.h"
|
||||||
#include "Messages/ModelUrlMsg.h"
|
#include "Messages/ModelUrlMsg.h"
|
||||||
#include "Messages/NameMsg.h"
|
#include "Messages/NameMsg.h"
|
||||||
#include "Messages/NetworkIdMsg.h"
|
|
||||||
#include "Messages/ParticipantMsg.h"
|
#include "Messages/ParticipantMsg.h"
|
||||||
#include "Messages/PoseMsg.h"
|
#include "Messages/PoseMsg.h"
|
||||||
#include "Messages/TextMsg.h"
|
#include "Messages/NetworkIdMsg.h"
|
||||||
#include "Messages/ThingMsg.h"
|
#include "Messages/ThingMsg.h"
|
||||||
#include "Participant.h"
|
#include "Participant.h"
|
||||||
|
|
||||||
@ -31,74 +30,6 @@ namespace RoboidControl {
|
|||||||
|
|
||||||
constexpr int MAX_SENDER_COUNT = 256;
|
constexpr int MAX_SENDER_COUNT = 256;
|
||||||
|
|
||||||
class RemoteParticipantUDP : public Participant {
|
|
||||||
public:
|
|
||||||
/// @brief Create a new participant with the given communcation info
|
|
||||||
/// @param ipAddress The IP address of the participant
|
|
||||||
/// @param port The UDP port of the participant
|
|
||||||
/// @remarks This does not belong here, it should move to ParticipantUDP or
|
|
||||||
/// something like that in the future
|
|
||||||
RemoteParticipantUDP(const char* ipAddress, int port);
|
|
||||||
|
|
||||||
/// @brief The Ip Address of a participant.
|
|
||||||
/// @remarks This does not belong here, it should move to ParticipantUDP or
|
|
||||||
/// something like that in the future
|
|
||||||
const char* ipAddress = "0.0.0.0";
|
|
||||||
/// @brief The port number for UDP communication with the participant.
|
|
||||||
/// @remarks This does not belong here, it should move to ParticipantUDP or
|
|
||||||
/// something like that in the future
|
|
||||||
unsigned int port = 0;
|
|
||||||
|
|
||||||
bool Send(IMessage* msg) override;
|
|
||||||
};
|
|
||||||
|
|
||||||
/// @brief class which manages all known participants
|
|
||||||
class ParticipantRegistry {
|
|
||||||
public:
|
|
||||||
/// @brief Retrieve a participant by its address
|
|
||||||
/// @param ipAddress The IP address of the participant
|
|
||||||
/// @param port The port number of the participant
|
|
||||||
/// @return The participant or a nullptr when it could not be found
|
|
||||||
RemoteParticipantUDP* Get(const char* ipAddress, unsigned int port);
|
|
||||||
/// @brief Retrieve a participant by its network ID
|
|
||||||
/// @param networkID The network ID of the participant
|
|
||||||
/// @return The participant or a nullptr when it could not be found
|
|
||||||
RemoteParticipantUDP* Get(unsigned char networkID);
|
|
||||||
|
|
||||||
/// @brief Add a participant with the given details
|
|
||||||
/// @param ipAddress The IP address of the participant
|
|
||||||
/// @param port The port number of the participant
|
|
||||||
/// @return The added participant
|
|
||||||
RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port);
|
|
||||||
|
|
||||||
/// @brief Add a participant
|
|
||||||
/// @param participant The participant to add
|
|
||||||
void Add(RemoteParticipantUDP* participant);
|
|
||||||
|
|
||||||
/// @brief Remove a participant
|
|
||||||
/// @param participant The participant to remove
|
|
||||||
void Remove(RemoteParticipantUDP* participant);
|
|
||||||
|
|
||||||
private:
|
|
||||||
#if defined(NO_STD)
|
|
||||||
public:
|
|
||||||
RemoteParticipantUDP** GetAll() const;
|
|
||||||
int count = 0;
|
|
||||||
|
|
||||||
private:
|
|
||||||
RemoteParticipantUDP** participants;
|
|
||||||
#else
|
|
||||||
public:
|
|
||||||
/// @brief Get all participants
|
|
||||||
/// @return All participants
|
|
||||||
const std::list<RemoteParticipantUDP*>& GetAll() const;
|
|
||||||
|
|
||||||
private:
|
|
||||||
/// @brief The list of known participants
|
|
||||||
std::list<RemoteParticipantUDP*> participants;
|
|
||||||
#endif
|
|
||||||
};
|
|
||||||
|
|
||||||
/// @brief A participant using UDP communication
|
/// @brief A participant using UDP communication
|
||||||
/// A local participant is the local device which can communicate with
|
/// A local participant is the local device which can communicate with
|
||||||
/// other participants It manages all local things and communcation with other
|
/// other participants It manages all local things and communcation with other
|
||||||
@ -111,7 +42,7 @@ class ParticipantRegistry {
|
|||||||
/// participant is created which can be obtained using
|
/// participant is created which can be obtained using
|
||||||
/// RoboidControl::IsolatedParticipant::Isolated().
|
/// RoboidControl::IsolatedParticipant::Isolated().
|
||||||
/// @sa RoboidControl::Thing::Thing()
|
/// @sa RoboidControl::Thing::Thing()
|
||||||
class ParticipantUDPGeneric : public RemoteParticipantUDP {
|
class ParticipantUDP : public Participant {
|
||||||
#pragma region Init
|
#pragma region Init
|
||||||
|
|
||||||
public:
|
public:
|
||||||
@ -120,32 +51,36 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
|
|||||||
/// These participant typically broadcast Participant messages to let site
|
/// These participant typically broadcast Participant messages to let site
|
||||||
/// servers on the local network know their presence. Alternatively they can
|
/// servers on the local network know their presence. Alternatively they can
|
||||||
/// broadcast information which can be used directly by other participants.
|
/// broadcast information which can be used directly by other participants.
|
||||||
ParticipantUDPGeneric(int port = 7681);
|
ParticipantUDP(int port = 7681);
|
||||||
/// @brief Create a participant which will try to connect to a site.
|
/// @brief Create a participant which will try to connect to a site.
|
||||||
/// @param ipAddress The IP address of the site
|
/// @param ipAddress The IP address of the site
|
||||||
/// @param port The port used by the site
|
/// @param port The port used by the site
|
||||||
/// @param localPort The port used by the local participant
|
/// @param localPort The port used by the local participant
|
||||||
ParticipantUDPGeneric(const char* ipAddress,
|
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
|
||||||
int port = 7681,
|
|
||||||
int localPort = 7681);
|
/// @brief Isolated participant is used when the application is run without
|
||||||
|
/// networking
|
||||||
|
/// @return A participant without networking support
|
||||||
|
static ParticipantUDP* Isolated();
|
||||||
|
|
||||||
|
/// @brief True if the participant is running isolated.
|
||||||
|
/// Isolated participants do not communicate with other participants
|
||||||
|
|
||||||
#pragma endregion Init
|
#pragma endregion Init
|
||||||
|
|
||||||
#pragma region Properties
|
|
||||||
|
|
||||||
public:
|
|
||||||
/// @brief True if the participant is running isolated.
|
/// @brief True if the participant is running isolated.
|
||||||
/// Isolated participants do not communicate with other participants
|
/// Isolated participants do not communicate with other participants
|
||||||
bool isIsolated = false;
|
bool isIsolated = false;
|
||||||
|
|
||||||
/// @brief The remote site when this participant is connected to a site
|
/// @brief The remote site when this participant is connected to a site
|
||||||
RemoteParticipantUDP* remoteSite = nullptr;
|
Participant* remoteSite = nullptr;
|
||||||
|
|
||||||
/// The interval in milliseconds for publishing (broadcasting) data on the
|
/// The interval in milliseconds for publishing (broadcasting) data on the
|
||||||
/// local network
|
/// local network
|
||||||
long publishInterval = 3000; // 3 seconds
|
long publishInterval = 3000; // 3 seconds
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
char buffer[1024];
|
||||||
|
|
||||||
#if !defined(ARDUINO)
|
#if !defined(ARDUINO)
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
#if defined(__unix__) || defined(__APPLE__)
|
||||||
int sock;
|
int sock;
|
||||||
@ -159,18 +94,16 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
|
|||||||
void begin();
|
void begin();
|
||||||
bool connected = false;
|
bool connected = false;
|
||||||
|
|
||||||
#pragma endregion Properties
|
|
||||||
|
|
||||||
#pragma region Update
|
#pragma region Update
|
||||||
|
|
||||||
public:
|
public:
|
||||||
virtual void Update(bool recurse = true) override;
|
virtual void Update() override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
unsigned long nextPublishMe = 0;
|
unsigned long nextPublishMe = 0;
|
||||||
|
|
||||||
/// @brief Prepare the local things for the next update
|
/// @brief Prepare the local things for the next update
|
||||||
// virtual void PrepMyThings();
|
virtual void PrepMyThings();
|
||||||
virtual void UpdateMyThings();
|
virtual void UpdateMyThings();
|
||||||
virtual void UpdateOtherThings();
|
virtual void UpdateOtherThings();
|
||||||
|
|
||||||
@ -181,47 +114,34 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
|
|||||||
void SendThingInfo(Participant* remoteParticipant, Thing* thing);
|
void SendThingInfo(Participant* remoteParticipant, Thing* thing);
|
||||||
void PublishThingInfo(Thing* thing);
|
void PublishThingInfo(Thing* thing);
|
||||||
|
|
||||||
virtual bool Send(IMessage* msg) override;
|
bool Send(Participant* remoteParticipant, IMessage* msg);
|
||||||
virtual bool Publish(IMessage* msg) = 0;
|
bool Publish(IMessage* msg);
|
||||||
|
|
||||||
#pragma endregion Send
|
#pragma endregion Send
|
||||||
|
|
||||||
#pragma region Receive
|
#pragma region Receive
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void ReceiveData(unsigned char bufferSize,
|
void ReceiveData(unsigned char bufferSize,
|
||||||
char* senderIpAddress,
|
char* senderIpAddress,
|
||||||
unsigned int senderPort);
|
unsigned int senderPort);
|
||||||
void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant);
|
void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
|
||||||
|
|
||||||
virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0;
|
void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
|
||||||
|
|
||||||
virtual void ReceiveUDP() = 0;
|
void ReceiveUDP();
|
||||||
|
|
||||||
virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg);
|
virtual void Process(Participant* sender, ParticipantMsg* msg);
|
||||||
virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg);
|
virtual void Process(Participant* sender, NetworkIdMsg* msg);
|
||||||
virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg);
|
virtual void Process(Participant* sender, InvestigateMsg* msg);
|
||||||
|
virtual void Process(Participant* sender, ThingMsg* msg);
|
||||||
virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg);
|
virtual void Process(Participant* sender, NameMsg* msg);
|
||||||
virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender,
|
virtual void Process(Participant* sender, ModelUrlMsg* msg);
|
||||||
ThingMsg* msg,
|
virtual void Process(Participant* sender, PoseMsg* msg);
|
||||||
bool isRemote);
|
virtual void Process(Participant* sender, BinaryMsg* msg);
|
||||||
|
|
||||||
virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg);
|
|
||||||
virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg);
|
|
||||||
virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg);
|
|
||||||
virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg);
|
|
||||||
virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg);
|
|
||||||
virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg);
|
|
||||||
|
|
||||||
#pragma endregion Receive
|
#pragma endregion Receive
|
||||||
|
|
||||||
public:
|
|
||||||
static ParticipantRegistry registry;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
|
|
||||||
#include "EspIdf/EspIdfParticipant.h"
|
|
||||||
#include "Posix/PosixParticipant.h"
|
|
||||||
|
@ -15,7 +15,7 @@ SiteServer::SiteServer(int port) : ParticipantUDP(port) {
|
|||||||
this->name = "Site Server";
|
this->name = "Site Server";
|
||||||
this->publishInterval = 0;
|
this->publishInterval = 0;
|
||||||
|
|
||||||
//SetupUDP(port, ipAddress, 0);
|
SetupUDP(port, ipAddress, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
#pragma endregion Init
|
#pragma endregion Init
|
||||||
@ -36,16 +36,16 @@ void SiteServer::UpdateMyThings() {
|
|||||||
for (int ix = 0; ix < Participant::registry.count; ix++) {
|
for (int ix = 0; ix < Participant::registry.count; ix++) {
|
||||||
Participant* participant = participants[ix];
|
Participant* participant = participants[ix];
|
||||||
#else
|
#else
|
||||||
for (Participant* participant : registry.GetAll()) {
|
for (Participant* participant : Participant::registry.GetAll()) {
|
||||||
#endif
|
#endif
|
||||||
if (participant == nullptr || participant == this)
|
if (participant == nullptr || participant == this)
|
||||||
continue;
|
continue;
|
||||||
|
|
||||||
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
|
PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
|
||||||
participant->Send(poseMsg);
|
this->Send(participant, poseMsg);
|
||||||
delete poseMsg;
|
delete poseMsg;
|
||||||
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
|
BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
|
||||||
participant->Send(binaryMsg);
|
this->Send(participant, binaryMsg);
|
||||||
delete binaryMsg;
|
delete binaryMsg;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -56,30 +56,30 @@ void SiteServer::UpdateMyThings() {
|
|||||||
|
|
||||||
#pragma region Receive
|
#pragma region Receive
|
||||||
|
|
||||||
void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
|
void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
|
||||||
if (msg->networkId != sender->networkId) {
|
if (msg->networkId != sender->networkId) {
|
||||||
// std::cout << this->name << " received New Client -> " <<
|
// std::cout << this->name << " received New Client -> " <<
|
||||||
// sender->ipAddress
|
// sender->ipAddress
|
||||||
// << ":" << (int)sender->port << "\n";
|
// << ":" << (int)sender->port << "\n";
|
||||||
NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
|
NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
|
||||||
sender->Send(msg);
|
this->Send(sender, msg);
|
||||||
delete msg;
|
delete msg;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {}
|
void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
|
||||||
|
|
||||||
void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) {
|
void SiteServer::Process(Participant* sender, ThingMsg* msg) {
|
||||||
Thing* thing = sender->Get(msg->networkId, msg->thingId);
|
Thing* thing = sender->Get(msg->thingId);
|
||||||
if (thing == nullptr) {
|
if (thing == nullptr)
|
||||||
// new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
|
// new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
|
||||||
// Thing::Reconstruct(sender, msg->thingType, msg->thingId);
|
// Thing::Reconstruct(sender, msg->thingType, msg->thingId);
|
||||||
//thing = new Thing(msg->thingType, sender->root);
|
//thing = new Thing(msg->thingType, sender->root);
|
||||||
}
|
;
|
||||||
thing->id = msg->thingId;
|
thing->id = msg->thingId;
|
||||||
|
|
||||||
if (msg->parentId != 0) {
|
if (msg->parentId != 0) {
|
||||||
thing->SetParent(Get(msg->networkId, msg->parentId));
|
thing->SetParent(Get(msg->parentId));
|
||||||
if (thing->IsRoot())
|
if (thing->IsRoot())
|
||||||
// if (thing->GetParent() != nullptr)
|
// if (thing->GetParent() != nullptr)
|
||||||
#if defined(NO_STD)
|
#if defined(NO_STD)
|
||||||
|
@ -2,11 +2,11 @@
|
|||||||
|
|
||||||
#include "ParticipantUDP.h"
|
#include "ParticipantUDP.h"
|
||||||
|
|
||||||
// #if !defined(NO_STD)
|
#if !defined(NO_STD)
|
||||||
// #include <functional>
|
#include <functional>
|
||||||
// #include <memory>
|
#include <memory>
|
||||||
// #include <unordered_map>
|
#include <unordered_map>
|
||||||
// #endif
|
#endif
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
@ -33,9 +33,9 @@ class SiteServer : public ParticipantUDP {
|
|||||||
protected:
|
protected:
|
||||||
unsigned long nextPublishMe = 0;
|
unsigned long nextPublishMe = 0;
|
||||||
|
|
||||||
virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) override;
|
virtual void Process(Participant* sender, ParticipantMsg* msg) override;
|
||||||
virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override;
|
virtual void Process(Participant* sender, NetworkIdMsg* msg) override;
|
||||||
virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override;
|
virtual void Process(Participant* sender, ThingMsg* msg) override;
|
||||||
|
|
||||||
#pragma endregion Receive
|
#pragma endregion Receive
|
||||||
|
|
||||||
|
@ -1,95 +0,0 @@
|
|||||||
#include "PosixMQTT.h"
|
|
||||||
|
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
|
||||||
|
|
||||||
#include <arpa/inet.h>
|
|
||||||
#include <fcntl.h> // For fcntl
|
|
||||||
#include <netinet/in.h>
|
|
||||||
#include <sys/socket.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
#include <chrono>
|
|
||||||
#include <thread>
|
|
||||||
|
|
||||||
|
|
||||||
namespace RoboidControl {
|
|
||||||
|
|
||||||
MQTTParticipant::MQTTParticipant(const char* remoteIpAddress, int port)
|
|
||||||
: MQTTParticipantBase(remoteIpAddress, port) {
|
|
||||||
SetupTCP();
|
|
||||||
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
|
||||||
send_mqtt_connect("RoboidControl1");
|
|
||||||
// sendSubscribe("domoticz/out");
|
|
||||||
}
|
|
||||||
|
|
||||||
void MQTTParticipant::SetupTCP() {
|
|
||||||
// Create a TCP socket
|
|
||||||
this->sock = socket(AF_INET, SOCK_STREAM, 0);
|
|
||||||
if (this->sock < 0) {
|
|
||||||
std::cerr << "Error creating socket" << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the socket to non-blocking mode
|
|
||||||
int flags = fcntl(this->sock, F_GETFL, 0);
|
|
||||||
fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
|
|
||||||
|
|
||||||
this->remote_addr.sin_family = AF_INET;
|
|
||||||
this->remote_addr.sin_port = htons(this->remoteSite->port);
|
|
||||||
int result =
|
|
||||||
inet_pton(AF_INET, this->remoteSite->ipAddress, &remote_addr.sin_addr);
|
|
||||||
if (result <= 0) {
|
|
||||||
std::cerr << "Invalid address/ Address not supported" << std::endl;
|
|
||||||
close(this->sock);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
int connect_result =
|
|
||||||
connect(this->sock, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
|
|
||||||
if (connect_result < 0 && errno != EINPROGRESS) {
|
|
||||||
std::cerr << "Error connecting to server:" << (int)errno << std::endl;
|
|
||||||
close(this->sock);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::cout << "TCP connection to " << this->remoteSite->ipAddress << ":"
|
|
||||||
<< this->remoteSite->port << "\n";
|
|
||||||
|
|
||||||
this->connected = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void MQTTParticipant::SendTCP(int bufferSize) {
|
|
||||||
send(this->sock, this->buffer, bufferSize, 0);
|
|
||||||
std::cout << "Posix: sent TCP\n";
|
|
||||||
}
|
|
||||||
|
|
||||||
int MQTTParticipant::ReceiveTCP() {
|
|
||||||
if (this->connected == false)
|
|
||||||
return 0;
|
|
||||||
|
|
||||||
int bytesReceived = recv(this->sock, this->buffer, sizeof(this->buffer) - 1, 0);
|
|
||||||
if (bytesReceived > 0) {
|
|
||||||
std::cout << " !\n";
|
|
||||||
buffer[bytesReceived] = '\0'; // Null-terminate the received data
|
|
||||||
std::cout << "Received: " << this->buffer << std::endl;
|
|
||||||
return bytesReceived;
|
|
||||||
} else if (bytesReceived == 0) {
|
|
||||||
this->connected = false;
|
|
||||||
// Connection has been gracefully closed
|
|
||||||
std::cout << "Connection closed by the server." << std::endl;
|
|
||||||
return 0;
|
|
||||||
} else {
|
|
||||||
if (errno == EAGAIN || errno == EWOULDBLOCK) {
|
|
||||||
// No data available to read, continue the loop
|
|
||||||
// std::cout << "No data available" << std::endl;
|
|
||||||
} else {
|
|
||||||
std::cerr << "Error receiving data: " << strerror(errno) << std::endl;
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace RoboidControl
|
|
||||||
#endif
|
|
@ -1,25 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
|
||||||
|
|
||||||
#include "Participants/ParticipantMQTT.h"
|
|
||||||
|
|
||||||
namespace RoboidControl {
|
|
||||||
|
|
||||||
class MQTTParticipant : public MQTTParticipantBase {
|
|
||||||
public:
|
|
||||||
MQTTParticipant(const char* ipAddress, int port = 1883);
|
|
||||||
|
|
||||||
protected:
|
|
||||||
void SetupTCP() override;
|
|
||||||
|
|
||||||
void SendTCP(int bufferSize) override;
|
|
||||||
|
|
||||||
int ReceiveTCP() override;
|
|
||||||
|
|
||||||
sockaddr_in remote_addr;
|
|
||||||
int sock;
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace RoboidControl
|
|
||||||
#endif
|
|
@ -1,5 +1,4 @@
|
|||||||
#include "PosixParticipant.h"
|
#include "PosixParticipant.h"
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
|
||||||
|
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
#if defined(__unix__) || defined(__APPLE__)
|
||||||
#include <arpa/inet.h>
|
#include <arpa/inet.h>
|
||||||
@ -10,28 +9,28 @@
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
namespace Posix {
|
||||||
|
|
||||||
ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}
|
void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
|
||||||
|
|
||||||
ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
|
|
||||||
: ParticipantUDPGeneric(ipAddress, port, localPort) {}
|
|
||||||
|
|
||||||
void ParticipantUDP::SetupUDP(int localPort,
|
|
||||||
const char* remoteIpAddress,
|
|
||||||
int remotePort) {
|
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
#if defined(__unix__) || defined(__APPLE__)
|
||||||
|
|
||||||
// Create a UDP socket
|
// Create a UDP socket
|
||||||
|
|
||||||
this->sock = socket(AF_INET, SOCK_DGRAM, 0);
|
this->sock = socket(AF_INET, SOCK_DGRAM, 0);
|
||||||
|
|
||||||
if (this->sock < 0) {
|
if (this->sock < 0) {
|
||||||
std::cerr << "Error creating socket" << std::endl;
|
std::cerr << "Error creating socket" << std::endl;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the socket to non-blocking mode
|
// Set the socket to non-blocking mode
|
||||||
|
#if defined(_WIN32) || defined(_WIN64)
|
||||||
|
u_long mode = 1; // 1 to enable non-blocking socket
|
||||||
|
ioctlsocket(this->sock, FIONBIO, &mode);
|
||||||
|
#elif defined(__unix__) || defined(__APPLE__)
|
||||||
int flags = fcntl(this->sock, F_GETFL, 0);
|
int flags = fcntl(this->sock, F_GETFL, 0);
|
||||||
fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
|
fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
|
||||||
|
#endif
|
||||||
|
|
||||||
if (remotePort != 0) {
|
if (remotePort != 0) {
|
||||||
// Set up the address to send to
|
// Set up the address to send to
|
||||||
@ -56,8 +55,7 @@ void ParticipantUDP::SetupUDP(int localPort,
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Bind the socket to the specified port
|
// Bind the socket to the specified port
|
||||||
if (bind(this->sock, (const struct sockaddr*)&server_addr,
|
if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
|
||||||
sizeof(server_addr)) < 0) {
|
|
||||||
std::cerr << "Bind failed" << std::endl;
|
std::cerr << "Bind failed" << std::endl;
|
||||||
close(sock);
|
close(sock);
|
||||||
}
|
}
|
||||||
@ -65,59 +63,23 @@ void ParticipantUDP::SetupUDP(int localPort,
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) {
|
void ParticipantUDP::Receive() {
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
|
||||||
|
|
||||||
// Create a UDP socket
|
|
||||||
|
|
||||||
this->sock = socket(AF_INET, SOCK_STREAM, 0);
|
|
||||||
if (this->sock < 0) {
|
|
||||||
std::cerr << "Error creating socket" << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
remote_addr.sin_family = AF_INET;
|
|
||||||
remote_addr.sin_port = htons(remotePort);
|
|
||||||
inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr);
|
|
||||||
|
|
||||||
int connect_result =
|
|
||||||
connect(this->sock, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
|
|
||||||
if (connect_result < 0) { //} && errno != EINPROGRESS) {
|
|
||||||
std::cerr << "Error connecting to server" << std::endl;
|
|
||||||
close(this->sock);
|
|
||||||
}
|
|
||||||
|
|
||||||
std::cout << "TCP connection to " << remoteIpAddress << ":" << remotePort
|
|
||||||
<< "\n";
|
|
||||||
|
|
||||||
// Set the socket to non-blocking mode
|
|
||||||
int flags = fcntl(this->sock, F_GETFL, 0);
|
|
||||||
fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
|
|
||||||
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
void ParticipantUDP::ReceiveUDP() {
|
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
#if defined(__unix__) || defined(__APPLE__)
|
||||||
sockaddr_in client_addr;
|
sockaddr_in client_addr;
|
||||||
socklen_t len = sizeof(client_addr);
|
socklen_t len = sizeof(client_addr);
|
||||||
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0,
|
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
|
||||||
(struct sockaddr*)&client_addr, &len);
|
|
||||||
if (packetSize > 0) {
|
if (packetSize > 0) {
|
||||||
char sender_ipAddress[INET_ADDRSTRLEN];
|
char sender_ipAddress[INET_ADDRSTRLEN];
|
||||||
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress,
|
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, INET_ADDRSTRLEN);
|
||||||
INET_ADDRSTRLEN);
|
|
||||||
unsigned int sender_port = ntohs(client_addr.sin_port);
|
unsigned int sender_port = ntohs(client_addr.sin_port);
|
||||||
|
|
||||||
ReceiveData(packetSize, sender_ipAddress, sender_port);
|
ReceiveData(packetSize, sender_ipAddress, sender_port);
|
||||||
// RoboidControl::Participant* remoteParticipant =
|
// RoboidControl::Participant* remoteParticipant = this->Get(sender_ipAddress, sender_port);
|
||||||
// this->Get(sender_ipAddress, sender_port); if (remoteParticipant ==
|
// if (remoteParticipant == nullptr) {
|
||||||
// nullptr) {
|
|
||||||
// remoteParticipant = this->Add(sender_ipAddress, sender_port);
|
// remoteParticipant = this->Add(sender_ipAddress, sender_port);
|
||||||
// // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
|
// // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
|
||||||
// // << "\n";
|
// // << "\n";
|
||||||
// // std::cout << "New remote participant " <<
|
// // std::cout << "New remote participant " << remoteParticipant->ipAddress
|
||||||
// remoteParticipant->ipAddress
|
|
||||||
// // << ":" << remoteParticipant->port << " "
|
// // << ":" << remoteParticipant->port << " "
|
||||||
// // << (int)remoteParticipant->networkId << "\n";
|
// // << (int)remoteParticipant->networkId << "\n";
|
||||||
// }
|
// }
|
||||||
@ -128,38 +90,12 @@ void ParticipantUDP::ReceiveUDP() {
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
// int ParticipantUDP::ReceiveTCP() {
|
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
|
||||||
// #if defined(__unix__) || defined(__APPLE__)
|
|
||||||
// int bytesReceived = recv(sock, buffer, sizeof(buffer) - 1, 0);
|
|
||||||
// if (bytesReceived > 0) {
|
|
||||||
// buffer[bytesReceived] = '\0'; // Null-terminate the received data
|
|
||||||
// std::cout << "Received: " << buffer << std::endl;
|
|
||||||
// return bytesReceived;
|
|
||||||
// } else if (bytesReceived == 0) {
|
|
||||||
// // Connection has been gracefully closed
|
|
||||||
// std::cout << "Connection closed by the server." << std::endl;
|
|
||||||
// return 0;
|
|
||||||
// } else {
|
|
||||||
// if (errno == EAGAIN || errno == EWOULDBLOCK) {
|
|
||||||
// // No data available to read, continue the loop
|
|
||||||
// // std::cout << "No data available" << std::endl;
|
|
||||||
// } else {
|
|
||||||
// std::cerr << "Error receiving data: " << strerror(errno) << std::endl;
|
|
||||||
// }
|
|
||||||
// return 0;
|
|
||||||
// }
|
|
||||||
// #endif // _WIN32 || _WIN64
|
|
||||||
// return 0;
|
|
||||||
// }
|
|
||||||
|
|
||||||
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
|
|
||||||
int bufferSize) {
|
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
#if defined(__unix__) || defined(__APPLE__)
|
||||||
// std::cout << "Send to " << remoteParticipant->ipAddress << ":" <<
|
// std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
|
||||||
// ntohs(remoteParticipant->port)
|
|
||||||
// << "\n";
|
// << "\n";
|
||||||
|
|
||||||
// Set up the destination address
|
// Set up the destination address
|
||||||
struct sockaddr_in dest_addr;
|
struct sockaddr_in dest_addr;
|
||||||
memset(&dest_addr, 0, sizeof(dest_addr));
|
memset(&dest_addr, 0, sizeof(dest_addr));
|
||||||
dest_addr.sin_family = AF_INET;
|
dest_addr.sin_family = AF_INET;
|
||||||
@ -167,11 +103,9 @@ bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
|
|||||||
dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);
|
dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);
|
||||||
|
|
||||||
// Send the message
|
// Send the message
|
||||||
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0,
|
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr));
|
||||||
(struct sockaddr*)&dest_addr, sizeof(dest_addr));
|
|
||||||
if (sent_bytes < 0) {
|
if (sent_bytes < 0) {
|
||||||
std::cerr << "sendto failed with error: " << sent_bytes << " "
|
std::cerr << "sendto failed with error: " << sent_bytes << " " << strerror(errno) << std::endl;
|
||||||
<< strerror(errno) << std::endl;
|
|
||||||
close(sock);
|
close(sock);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -179,19 +113,6 @@ bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// bool ParticipantUDP::SendTCP(int bufferSize) {
|
|
||||||
// #if defined(__unix__) || defined(__APPLE__)
|
|
||||||
// // send(sock, this->buffer, bufferSize, 0);
|
|
||||||
// ssize_t sent_bytes = send(this->sock, this->buffer, bufferSize, 0);
|
|
||||||
// if (sent_bytes < 0) {
|
|
||||||
// std::cerr << "Failed to send packet" << strerror(errno) << std::endl;
|
|
||||||
// close(sock);
|
|
||||||
// return -1;
|
|
||||||
// }
|
|
||||||
// #endif
|
|
||||||
// return false;
|
|
||||||
// }
|
|
||||||
|
|
||||||
bool ParticipantUDP::Publish(IMessage* msg) {
|
bool ParticipantUDP::Publish(IMessage* msg) {
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
#if defined(__unix__) || defined(__APPLE__)
|
||||||
int bufferSize = msg->Serialize(this->buffer);
|
int bufferSize = msg->Serialize(this->buffer);
|
||||||
@ -200,11 +121,8 @@ bool ParticipantUDP::Publish(IMessage* msg) {
|
|||||||
|
|
||||||
char ip_str[INET_ADDRSTRLEN];
|
char ip_str[INET_ADDRSTRLEN];
|
||||||
inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
|
inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
|
||||||
std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port)
|
std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) << "\n";
|
||||||
<< "\n";
|
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
|
||||||
int sent_bytes =
|
|
||||||
sendto(sock, this->buffer, bufferSize, 0,
|
|
||||||
(struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
|
|
||||||
if (sent_bytes < 0) {
|
if (sent_bytes < 0) {
|
||||||
std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
|
std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
|
||||||
close(sock);
|
close(sock);
|
||||||
@ -214,6 +132,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} // namespace Posix
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
|
|
||||||
#endif
|
|
@ -1,36 +1,16 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
|
||||||
|
|
||||||
#include "Participants/ParticipantUDP.h"
|
#include "Participants/ParticipantUDP.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
namespace Posix {
|
||||||
|
|
||||||
class ParticipantUDP : public ParticipantUDPGeneric {
|
class ParticipantUDP : public RoboidControl::ParticipantUDP {
|
||||||
public:
|
public:
|
||||||
/// @brief Create a participant without connecting to a site
|
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
|
||||||
/// @param port The port on which the participant communicates
|
void Receive();
|
||||||
/// These participant typically broadcast Participant messages to let site
|
bool Send(Participant* remoteParticipant, int bufferSize);
|
||||||
/// servers on the local network know their presence. Alternatively they can
|
bool Publish(IMessage* msg);
|
||||||
/// broadcast information which can be used directly by other participants.
|
|
||||||
ParticipantUDP(int port = 7681);
|
|
||||||
/// @brief Create a participant which will try to connect to a site.
|
|
||||||
/// @param ipAddress The IP address of the site
|
|
||||||
/// @param port The port used by the site
|
|
||||||
/// @param localPort The port used by the local participant
|
|
||||||
ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
|
|
||||||
|
|
||||||
void SetupUDP(int localPort,
|
|
||||||
const char* remoteIpAddress,
|
|
||||||
int remotePort) override;
|
|
||||||
void ReceiveUDP() override;
|
|
||||||
|
|
||||||
//void Setup(int localPort, const char* remoteIpAddress, int remotePort);
|
|
||||||
void SetupTCP(const char* remoteIpAddress, int remotePort);
|
|
||||||
|
|
||||||
// void Receive();
|
|
||||||
int ReceiveTCP();
|
|
||||||
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
|
|
||||||
bool Publish(IMessage* msg) override;
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
#if defined(__unix__) || defined(__APPLE__)
|
#if defined(__unix__) || defined(__APPLE__)
|
||||||
@ -40,5 +20,5 @@ class ParticipantUDP : public ParticipantUDPGeneric {
|
|||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
|
} // namespace Posix
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
#endif
|
|
||||||
|
56
README.md
56
README.md
@ -15,59 +15,3 @@ Supporting:
|
|||||||
|
|
||||||
- RoboidControl::Thing
|
- RoboidControl::Thing
|
||||||
- RoboidControl::Participant
|
- RoboidControl::Participant
|
||||||
|
|
||||||
# Installation
|
|
||||||
|
|
||||||
## Core code
|
|
||||||
|
|
||||||
The repository uses cmake for building. You can place it in a subfolder of your project and include it in you `CMakeLists.txt`.
|
|
||||||
For example if the library is placed in the subfolder `roboidcontrol`:
|
|
||||||
```
|
|
||||||
# Add the path to Roboid Control
|
|
||||||
add_subdirectory(roboidcontrol)
|
|
||||||
|
|
||||||
# Your source files/executable
|
|
||||||
add_executable(my_executable main.cpp)
|
|
||||||
|
|
||||||
# Link against RoboidControl
|
|
||||||
target_link_libraries(my_executable RoboidControl)
|
|
||||||
```
|
|
||||||
|
|
||||||
## Arduino (PlatformIO)
|
|
||||||
|
|
||||||
Arduino is only supported in combination with PlatformIO. The Arduino IDE is not (yet?) supported.
|
|
||||||
|
|
||||||
The best way to include support for Roboid Control in PlatformIO is
|
|
||||||
to clone the Roboid Control for C++ repository into a subfolder of the /lib folder.
|
|
||||||
Alternatively you can download the zip file and unpack it as a subfolder of the /lib folder.
|
|
||||||
|
|
||||||
## ESP-IDF
|
|
||||||
|
|
||||||
The best way to include support for Roboid Control in PlatformIO is
|
|
||||||
to clone the Roboid Control for C++ repository into a subfolder of the /components folder.
|
|
||||||
Alternatively you can download the zip file and unpack it as a subfolder of the /components folder.
|
|
||||||
|
|
||||||
Make sure you have included RoboidControl as a component in your top-level CMakeLists.txt, for example:
|
|
||||||
```
|
|
||||||
list(APPEND EXTRA_COMPONENT_DIRS
|
|
||||||
components/RoboidControl
|
|
||||||
)
|
|
||||||
```
|
|
||||||
|
|
||||||
# Get Started
|
|
||||||
|
|
||||||
## Core C++ Examples
|
|
||||||
|
|
||||||
This repository contains examples in the `examples` folder. You can build these using cmake.
|
|
||||||
|
|
||||||
For example, to build the BB2A example:
|
|
||||||
```
|
|
||||||
cmake -B build -D BUILD_EXAMPLE_BB2A=ON
|
|
||||||
cmake --build build
|
|
||||||
```
|
|
||||||
The resulting executable is then `build/examples/Debug/BB2A.exe`
|
|
||||||
|
|
||||||
## Arduino (PlatformIO) Examples
|
|
||||||
|
|
||||||
Specific examples for the Arduino platform are found in the `Arduino\examples` folder.
|
|
||||||
To use them you should create a new project in PlatformIO and then copy the example code to your project.
|
|
@ -1,3 +0,0 @@
|
|||||||
Start testing: Jun 17 17:17 W. Europe Summer Time
|
|
||||||
----------------------------------------------------------
|
|
||||||
End testing: Jun 17 17:17 W. Europe Summer Time
|
|
55
Thing.cpp
55
Thing.cpp
@ -28,8 +28,7 @@ Thing* Thing::LocalRoot() {
|
|||||||
|
|
||||||
// Only use this for root things
|
// Only use this for root things
|
||||||
Thing::Thing(Participant* owner) {
|
Thing::Thing(Participant* owner) {
|
||||||
this->type = Type::Root;
|
this->type = Type::Roboid; // should become root
|
||||||
this->name = "Root";
|
|
||||||
|
|
||||||
this->position = Spherical::zero;
|
this->position = Spherical::zero;
|
||||||
this->positionUpdated = true;
|
this->positionUpdated = true;
|
||||||
@ -41,16 +40,12 @@ Thing::Thing(Participant* owner) {
|
|||||||
this->angularVelocity = Spherical::zero;
|
this->angularVelocity = Spherical::zero;
|
||||||
|
|
||||||
this->owner = owner;
|
this->owner = owner;
|
||||||
this->owner->Add(this);
|
//this->owner->Add(this, true);
|
||||||
std::cout << this->owner->name << ": New root thing " << std::endl;
|
std::cout << this->owner->name << ": New root thing " << std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Thing::CreateRoot(Participant* owner) {
|
Thing::Thing(unsigned char thingType, Thing* parent) {
|
||||||
owner->root = new Thing(owner);
|
this->type = thingType;
|
||||||
}
|
|
||||||
|
|
||||||
Thing::Thing(Thing* parent) {
|
|
||||||
this->type = Type::Undetermined;
|
|
||||||
|
|
||||||
this->position = Spherical::zero;
|
this->position = Spherical::zero;
|
||||||
this->positionUpdated = true;
|
this->positionUpdated = true;
|
||||||
@ -73,6 +68,13 @@ Thing::~Thing() {
|
|||||||
std::cout << "Destroy thing " << this->name << std::endl;
|
std::cout << "Destroy thing " << this->name << std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
|
||||||
|
// unsigned char thingId) {
|
||||||
|
// Thing thing = Thing(owner, thingType);
|
||||||
|
// thing.id = thingId;
|
||||||
|
// return thing;
|
||||||
|
// }
|
||||||
|
|
||||||
#pragma endregion Init
|
#pragma endregion Init
|
||||||
|
|
||||||
void Thing::SetName(const char* name) {
|
void Thing::SetName(const char* name) {
|
||||||
@ -101,10 +103,25 @@ void Thing::SetParent(Thing* parent) {
|
|||||||
this->hierarchyChanged = true;
|
this->hierarchyChanged = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// void Thing::SetParent(Thing* parent) {
|
||||||
|
// parent->AddChild(this);
|
||||||
|
// this->hierarchyChanged = true;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// const Thing& Thing::GetParent() {
|
||||||
|
// return *this->parent;
|
||||||
|
// }
|
||||||
|
|
||||||
bool Thing::IsRoot() const {
|
bool Thing::IsRoot() const {
|
||||||
return this == LocalRoot() || this->parent == nullptr;
|
return this == LocalRoot() || this->parent == nullptr; //&Thing::Root;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// void Thing::SetParent(Thing* root, const char* name) {
|
||||||
|
// Thing* thing = root->FindChild(name);
|
||||||
|
// if (thing != nullptr)
|
||||||
|
// this->SetParent(thing);
|
||||||
|
// }
|
||||||
|
|
||||||
Thing* Thing::GetParent() {
|
Thing* Thing::GetParent() {
|
||||||
return this->parent;
|
return this->parent;
|
||||||
}
|
}
|
||||||
@ -253,16 +270,24 @@ unsigned long Thing::GetTimeMs() {
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// void Thing::Update(bool recursive) {
|
||||||
|
// Update(GetTimeMs(), recursive);
|
||||||
|
// }
|
||||||
|
|
||||||
|
void Thing::PrepareForUpdate() {}
|
||||||
|
|
||||||
void Thing::Update(bool recursive) {
|
void Thing::Update(bool recursive) {
|
||||||
|
// if (this->positionUpdated || this->orientationUpdated)
|
||||||
|
// OnPoseChanged callback
|
||||||
this->positionUpdated = false;
|
this->positionUpdated = false;
|
||||||
this->orientationUpdated = false;
|
this->orientationUpdated = false;
|
||||||
this->linearVelocityUpdated = false;
|
// this->linearVelocityUpdated = false;
|
||||||
this->angularVelocityUpdated = false;
|
// this->angularVelocityUpdated = false;
|
||||||
this->hierarchyChanged = false;
|
this->hierarchyChanged = false;
|
||||||
this->nameChanged = false;
|
this->nameChanged = false;
|
||||||
|
|
||||||
if (recursive) {
|
if (recursive) {
|
||||||
std::cout << "# children: " << (int)this->childCount << std::endl;
|
// std::cout << "# children: " << (int)this->childCount << std::endl;
|
||||||
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
|
for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
|
||||||
Thing* child = this->children[childIx];
|
Thing* child = this->children[childIx];
|
||||||
if (child == nullptr)
|
if (child == nullptr)
|
||||||
@ -272,6 +297,10 @@ void Thing::Update(bool recursive) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Thing::UpdateThings() {
|
||||||
|
IsolatedParticipant::Isolated()->Update();
|
||||||
|
}
|
||||||
|
|
||||||
#pragma endregion Update
|
#pragma endregion Update
|
||||||
|
|
||||||
int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
|
int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
|
||||||
|
103
Thing.h
103
Thing.h
@ -10,7 +10,7 @@
|
|||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
class Participant;
|
class Participant;
|
||||||
class ParticipantUDPGeneric;
|
class ParticipantUDP;
|
||||||
|
|
||||||
#define THING_STORE_SIZE 256
|
#define THING_STORE_SIZE 256
|
||||||
// IMPORTANT: values higher than 256 will need to change the Thing::id type
|
// IMPORTANT: values higher than 256 will need to change the Thing::id type
|
||||||
@ -20,57 +20,57 @@ class ParticipantUDPGeneric;
|
|||||||
class Thing {
|
class Thing {
|
||||||
public:
|
public:
|
||||||
/// @brief Predefined thing types
|
/// @brief Predefined thing types
|
||||||
struct Type {
|
enum Type : unsigned char {
|
||||||
static const unsigned char Undetermined = 0x00;
|
Undetermined,
|
||||||
// Sensor
|
// Sensor,
|
||||||
static const unsigned char Switch = 0x01;
|
Switch,
|
||||||
static const unsigned char DistanceSensor = 0x02;
|
DistanceSensor,
|
||||||
static const unsigned char DirectionalSensor = 0x03;
|
DirectionalSensor,
|
||||||
static const unsigned char TemperatureSensor = 0x04;
|
TemperatureSensor,
|
||||||
static const unsigned char TouchSensor = 0x05;
|
TouchSensor,
|
||||||
// Motor
|
// Motor,
|
||||||
static const unsigned char ControlledMotor = 0x06;
|
ControlledMotor,
|
||||||
static const unsigned char UncontrolledMotor = 0x07;
|
UncontrolledMotor,
|
||||||
static const unsigned char Servo = 0x08;
|
Servo,
|
||||||
static const unsigned char RelativeEncoder = 0x19;
|
IncrementalEncoder,
|
||||||
// Other
|
// Other
|
||||||
static const unsigned char Root = 0x10;
|
Roboid,
|
||||||
static const unsigned char Roboid = 0x09;
|
Humanoid,
|
||||||
static const unsigned char Humanoid = 0x0A;
|
ExternalSensor,
|
||||||
static const unsigned char ExternalSensor = 0x08;
|
DifferentialDrive
|
||||||
static const unsigned char Animator = 0x0C;
|
|
||||||
static const unsigned char DifferentialDrive = 0x0D;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#pragma region Init
|
#pragma region Init
|
||||||
|
static Thing* LocalRoot();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
// Special constructor to create a root thing
|
||||||
|
Thing(Participant* parent);
|
||||||
|
// Which can only be used by the Participant
|
||||||
|
friend class Participant;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// @brief Create a new Thing
|
/// @brief Create a new thing
|
||||||
|
/// @param thingType The type of thing (can use Thing::Type)
|
||||||
/// @param parent (optional) The parent thing
|
/// @param parent (optional) The parent thing
|
||||||
/// The owner will be the same as the owner of the parent thing, it will
|
/// 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
|
/// be Participant::LocalParticipant if the parent is not specified. A thing
|
||||||
/// without a parent will be connected to the root thing.
|
/// without a parent will be a root thing.
|
||||||
Thing(Thing* parent = LocalRoot());
|
Thing(unsigned char thingType = Thing::Type::Undetermined,
|
||||||
|
Thing* parent = LocalRoot());
|
||||||
|
|
||||||
private:
|
/// @brief Create a new child thing
|
||||||
/// @brief Constructor to create a root thing
|
/// @param parent The parent thing
|
||||||
/// @param owner The participant who will own this root thing
|
/// @param thingType The type of thing (can use Thing::Type)
|
||||||
/// @remarks This function is private because CreateRoot() should be used
|
/// @param thingId The ID of the thing, leave out or set to zero to generate
|
||||||
/// instead
|
/// an ID
|
||||||
Thing(Participant* owener);
|
/// @note The owner will be the same as the owner of the parent thing
|
||||||
|
|
||||||
public:
|
|
||||||
/// @brief Destructor for a Thing
|
|
||||||
~Thing();
|
~Thing();
|
||||||
|
|
||||||
/// @brief Create a root thing for a participant
|
static Thing Reconstruct(Participant* owner,
|
||||||
/// @param owner The participant who will own this root thing
|
unsigned char thingType,
|
||||||
static void CreateRoot(Participant* owner);
|
unsigned char thingId);
|
||||||
|
|
||||||
/// @brief The root thing for the local participant
|
|
||||||
/// @return The root thing for the local participant
|
|
||||||
static Thing* LocalRoot();
|
|
||||||
|
|
||||||
#pragma endregion Init
|
#pragma endregion Init
|
||||||
|
|
||||||
@ -80,7 +80,9 @@ class Thing {
|
|||||||
|
|
||||||
#pragma region Properties
|
#pragma region Properties
|
||||||
|
|
||||||
public:
|
/// @brief The participant managing this thing
|
||||||
|
Participant* owner = nullptr;
|
||||||
|
|
||||||
/// @brief The ID of the thing
|
/// @brief The ID of the thing
|
||||||
unsigned char id = 0;
|
unsigned char id = 0;
|
||||||
|
|
||||||
@ -88,17 +90,10 @@ class Thing {
|
|||||||
/// This can be either a Thing::Type of a byte value for custom types
|
/// This can be either a Thing::Type of a byte value for custom types
|
||||||
unsigned char type = Type::Undetermined;
|
unsigned char type = Type::Undetermined;
|
||||||
|
|
||||||
/// @brief Is this a remote thing?
|
|
||||||
/// A remote thing is owned by other participant
|
|
||||||
/// and is not simulated by the local participant
|
|
||||||
bool isRemote = false;
|
|
||||||
|
|
||||||
/// @brief The participant owning this thing
|
|
||||||
Participant* owner = nullptr;
|
|
||||||
|
|
||||||
/// @brief The name of the thing
|
/// @brief The name of the thing
|
||||||
const char* name = nullptr;
|
const char* name = nullptr;
|
||||||
|
|
||||||
|
public:
|
||||||
void SetName(const char* name);
|
void SetName(const char* name);
|
||||||
const char* GetName() const;
|
const char* GetName() const;
|
||||||
bool nameChanged = false;
|
bool nameChanged = false;
|
||||||
@ -107,12 +102,14 @@ class Thing {
|
|||||||
/// loaded from
|
/// loaded from
|
||||||
/// @param url The url of the model
|
/// @param url The url of the model
|
||||||
/// @remark Although the roboid implementation is not dependent on 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
|
/// the only official supported model format is .obj
|
||||||
void SetModel(const char* url);
|
void SetModel(const char* url);
|
||||||
|
|
||||||
/// @brief An URL pointing to the location where a model of the thing can be
|
/// @brief An URL pointing to the location where a model of the thing can be
|
||||||
/// found
|
/// found
|
||||||
const char* modelUrl = nullptr;
|
const char* modelUrl = nullptr;
|
||||||
|
/// @brief The scale of the model (deprecated I think)
|
||||||
|
float modelScale = 1;
|
||||||
|
|
||||||
#pragma endregion Properties
|
#pragma endregion Properties
|
||||||
|
|
||||||
@ -120,13 +117,13 @@ class Thing {
|
|||||||
|
|
||||||
/// @brief Sets the parent of this Thing
|
/// @brief Sets the parent of this Thing
|
||||||
/// @param parent The Thing which should become the parent
|
/// @param parent The Thing which should become the parent
|
||||||
|
// virtual void SetParent(Thing* parent);
|
||||||
void SetParent(Thing* parent);
|
void SetParent(Thing* parent);
|
||||||
/// @brief Gets the parent of this Thing
|
/// @brief Gets the parent of this Thing
|
||||||
/// @return The parent Thing
|
/// @return The parent Thing
|
||||||
|
// Thing* GetParent();
|
||||||
Thing* GetParent();
|
Thing* GetParent();
|
||||||
|
|
||||||
/// @brief Check if this is a root thing
|
|
||||||
/// @return True is this thing is a root
|
|
||||||
bool IsRoot() const;
|
bool IsRoot() const;
|
||||||
|
|
||||||
/// @brief The number of children
|
/// @brief The number of children
|
||||||
@ -228,12 +225,16 @@ class Thing {
|
|||||||
#pragma region Update
|
#pragma region Update
|
||||||
|
|
||||||
public:
|
public:
|
||||||
//virtual void PrepareForUpdate();
|
virtual void PrepareForUpdate();
|
||||||
|
|
||||||
/// @brief Updates the state of the thing
|
/// @brief Updates the state of the thing
|
||||||
|
/// @param currentTimeMs The current clock time in milliseconds; if this is
|
||||||
|
/// zero, the current time is retrieved automatically
|
||||||
/// @param recurse When true, this will Update the descendants recursively
|
/// @param recurse When true, this will Update the descendants recursively
|
||||||
virtual void Update(bool recurse = false);
|
virtual void Update(bool recurse = false);
|
||||||
|
|
||||||
|
static void UpdateThings();
|
||||||
|
|
||||||
/// @brief Get the current time in milliseconds
|
/// @brief Get the current time in milliseconds
|
||||||
/// @return The current time in milliseconds
|
/// @return The current time in milliseconds
|
||||||
static unsigned long GetTimeMs();
|
static unsigned long GetTimeMs();
|
||||||
|
@ -39,8 +39,8 @@ void ControlledMotor::Update(bool recurse) {
|
|||||||
this->lastError = error;
|
this->lastError = error;
|
||||||
|
|
||||||
float output = p_term + i_term + d_term;
|
float output = p_term + i_term + d_term;
|
||||||
// std::cout << "target " << this->targetVelocity << " actual "
|
std::cout << "target " << this->targetVelocity << " actual "
|
||||||
// << this->actualVelocity << " output = " << output << std::endl;
|
<< this->actualVelocity << " output = " << output << std::endl;
|
||||||
// float acceleration =
|
// float acceleration =
|
||||||
// error * timeStep * pidP; // Just P is used at this moment
|
// error * timeStep * pidP; // Just P is used at this moment
|
||||||
// std::cout << "motor acc. " << acceleration << std::endl;
|
// std::cout << "motor acc. " << acceleration << std::endl;
|
||||||
|
@ -4,8 +4,8 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent) {
|
DifferentialDrive::DifferentialDrive(Thing* parent)
|
||||||
this->type = Type::DifferentialDrive;
|
: Thing(Type::DifferentialDrive, parent) {
|
||||||
this->name = "Differential drive";
|
this->name = "Differential drive";
|
||||||
|
|
||||||
this->leftWheel = new Motor(this);
|
this->leftWheel = new Motor(this);
|
||||||
@ -18,8 +18,7 @@ DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent) {
|
|||||||
DifferentialDrive::DifferentialDrive(Motor* leftMotor,
|
DifferentialDrive::DifferentialDrive(Motor* leftMotor,
|
||||||
Motor* rightMotor,
|
Motor* rightMotor,
|
||||||
Thing* parent)
|
Thing* parent)
|
||||||
: Thing(parent) {
|
: Thing(Type::DifferentialDrive, parent) {
|
||||||
this->type = Type::DifferentialDrive;
|
|
||||||
this->name = "Differential drive";
|
this->name = "Differential drive";
|
||||||
this->leftWheel = leftMotor;
|
this->leftWheel = leftMotor;
|
||||||
this->rightWheel = rightMotor;
|
this->rightWheel = rightMotor;
|
||||||
|
@ -2,9 +2,18 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent) {
|
//DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}
|
||||||
this->type = Type::Switch;
|
|
||||||
}
|
// DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId)
|
||||||
|
// : Thing(owner, Type::Switch, thingId) {}
|
||||||
|
|
||||||
|
// DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId)
|
||||||
|
// : Thing(parent, Type::Switch) {}
|
||||||
|
|
||||||
|
// DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {}
|
||||||
|
|
||||||
|
// DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {}
|
||||||
|
DigitalSensor::DigitalSensor(Thing* parent) : Thing(Type::Switch, parent) {}
|
||||||
|
|
||||||
int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
|
int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
|
||||||
bytes[(*ix)++] = state ? 1 : 0;
|
bytes[(*ix)++] = state ? 1 : 0;
|
||||||
|
@ -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
|
|
@ -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
|
|
@ -1,29 +1,11 @@
|
|||||||
#include "Motor.h"
|
#include "Motor.h"
|
||||||
|
|
||||||
#include "Messages/BinaryMsg.h"
|
|
||||||
#include "Participant.h"
|
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
Motor::Motor(Thing* parent) : Thing(parent) {
|
Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {}
|
||||||
this->type = Type::UncontrolledMotor;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Motor::SetTargetVelocity(float targetSpeed) {
|
void Motor::SetTargetVelocity(float targetSpeed) {
|
||||||
if (targetSpeed != this->targetVelocity) {
|
|
||||||
this->targetVelocity = targetSpeed;
|
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) {
|
int Motor::GenerateBinary(char* data, unsigned char* ix) {
|
||||||
|
@ -14,11 +14,11 @@ class Motor : public Thing {
|
|||||||
Direction direction;
|
Direction direction;
|
||||||
|
|
||||||
virtual void SetTargetVelocity(float velocity); // -1..0..1
|
virtual void SetTargetVelocity(float velocity); // -1..0..1
|
||||||
virtual float GetTargetVelocity();
|
|
||||||
|
|
||||||
int GenerateBinary(char* bytes, unsigned char* ix) override;
|
int GenerateBinary(char* bytes, unsigned char* ix) override;
|
||||||
|
// virtual void ProcessBinary(char* bytes) override;
|
||||||
|
|
||||||
protected:
|
//protected:
|
||||||
float targetVelocity = 0;
|
float targetVelocity = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -2,9 +2,8 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(parent) {
|
RelativeEncoder::RelativeEncoder(Thing* parent)
|
||||||
this->type = Type::RelativeEncoder;
|
: Thing(Type::IncrementalEncoder, parent) {}
|
||||||
}
|
|
||||||
|
|
||||||
float RelativeEncoder::GetRotationSpeed() {
|
float RelativeEncoder::GetRotationSpeed() {
|
||||||
return rotationSpeed;
|
return rotationSpeed;
|
||||||
|
@ -4,9 +4,15 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent) {
|
// TemperatureSensor::TemperatureSensor(Participant* participant,
|
||||||
this->type = Type::TemperatureSensor;
|
// unsigned char thingId)
|
||||||
}
|
// : Thing(participant, Type::TemperatureSensor, thingId) {}
|
||||||
|
|
||||||
|
// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {}
|
||||||
|
|
||||||
|
TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {}
|
||||||
|
|
||||||
|
// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {}
|
||||||
|
|
||||||
void TemperatureSensor::SetTemperature(float temp) {
|
void TemperatureSensor::SetTemperature(float temp) {
|
||||||
this->temperature = temp;
|
this->temperature = temp;
|
||||||
|
@ -2,26 +2,20 @@
|
|||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
|
||||||
TouchSensor::TouchSensor(Thing* parent) : Thing(parent) {
|
TouchSensor::TouchSensor(Thing* parent) : Thing(Type::TouchSensor, parent) {
|
||||||
this->type = Type::TouchSensor;
|
|
||||||
this->name = "Touch sensor";
|
this->name = "Touch sensor";
|
||||||
}
|
}
|
||||||
|
|
||||||
bool TouchSensor::IsTouching() {
|
void TouchSensor::PrepareForUpdate() {
|
||||||
return this->internalTouch || this->externalTouch;
|
this->touchedSomething = this->externalTouch;
|
||||||
}
|
}
|
||||||
|
|
||||||
// void TouchSensor::PrepareForUpdate() {
|
|
||||||
// //this->internalTouch = this->externalTouch;
|
|
||||||
// }
|
|
||||||
|
|
||||||
void TouchSensor::Update(bool recursive) {
|
void TouchSensor::Update(bool recursive) {
|
||||||
Thing::Update(recursive);
|
Thing::Update(recursive);
|
||||||
}
|
}
|
||||||
|
|
||||||
int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
|
int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
|
||||||
std::cout << "BinaryMsg Touch " << this->internalTouch << std::endl;
|
bytes[(*ix)++] = this->touchedSomething ? 1 : 0;
|
||||||
bytes[(*ix)++] = this->internalTouch ? 1 : 0;
|
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -6,7 +6,7 @@ namespace RoboidControl {
|
|||||||
|
|
||||||
/// @brief A sensor which can detect touches
|
/// @brief A sensor which can detect touches
|
||||||
class TouchSensor : public Thing {
|
class TouchSensor : public Thing {
|
||||||
// When finishing this release (0.3), I notice that this is equivalent to a digital sensor
|
// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor
|
||||||
public:
|
public:
|
||||||
/// @brief Create a new child touch sensor
|
/// @brief Create a new child touch sensor
|
||||||
/// @param parent The parent thing
|
/// @param parent The parent thing
|
||||||
@ -16,9 +16,9 @@ class TouchSensor : public Thing {
|
|||||||
|
|
||||||
/// @brief Value which is true when the sensor is touching something, false
|
/// @brief Value which is true when the sensor is touching something, false
|
||||||
/// otherwise
|
/// otherwise
|
||||||
bool IsTouching();
|
bool touchedSomething = false;
|
||||||
|
|
||||||
//virtual void PrepareForUpdate() override;
|
virtual void PrepareForUpdate() override;
|
||||||
virtual void Update(bool recursive) override;
|
virtual void Update(bool recursive) override;
|
||||||
|
|
||||||
/// @brief Function used to generate binary data for this touch sensor
|
/// @brief Function used to generate binary data for this touch sensor
|
||||||
@ -30,7 +30,6 @@ class TouchSensor : public Thing {
|
|||||||
virtual void ProcessBinary(char* bytes) override;
|
virtual void ProcessBinary(char* bytes) override;
|
||||||
protected:
|
protected:
|
||||||
bool externalTouch = false;
|
bool externalTouch = false;
|
||||||
bool internalTouch = false;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
|
@ -1,5 +1,4 @@
|
|||||||
#include "WindowsParticipant.h"
|
#include "WindowsParticipant.h"
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
|
||||||
|
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
#if defined(_WIN32) || defined(_WIN64)
|
||||||
#include <winsock2.h>
|
#include <winsock2.h>
|
||||||
@ -8,12 +7,9 @@
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
namespace Windows {
|
||||||
|
|
||||||
ParticipantUDP::ParticipantUDP() {}
|
void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
|
||||||
|
|
||||||
void ParticipantUDP::Setup(int localPort,
|
|
||||||
const char* remoteIpAddress,
|
|
||||||
int remotePort) {
|
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
#if defined(_WIN32) || defined(_WIN64)
|
||||||
|
|
||||||
// Create a UDP socket
|
// Create a UDP socket
|
||||||
@ -23,28 +19,19 @@ void ParticipantUDP::Setup(int localPort,
|
|||||||
std::cerr << "WSAStartup failed" << std::endl;
|
std::cerr << "WSAStartup failed" << std::endl;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Create an UDP socket
|
|
||||||
this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
||||||
if (this->sock == INVALID_SOCKET) {
|
|
||||||
std::cerr << "UDP Socket creation failed: " << WSAGetLastError()
|
if (this->sock < 0) {
|
||||||
<< std::endl;
|
std::cerr << "Error creating socket" << std::endl;
|
||||||
WSACleanup();
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the socket to non-blocking mode
|
// Set the socket to non-blocking mode
|
||||||
u_long mode = 1; // 1 to enable non-blocking socket
|
u_long mode = 1; // 1 to enable non-blocking socket
|
||||||
if (ioctlsocket(sock, FIONBIO, &mode) != NO_ERROR) {
|
ioctlsocket(this->sock, FIONBIO, &mode);
|
||||||
std::cerr << "Failed to set non-blocking mode: " << WSAGetLastError()
|
|
||||||
<< std::endl;
|
|
||||||
closesocket(sock);
|
|
||||||
WSACleanup();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (remotePort != 0) {
|
if (remotePort != 0) {
|
||||||
// Define the server address
|
// Set up the address to send to
|
||||||
memset(&remote_addr, 0, sizeof(remote_addr));
|
memset(&remote_addr, 0, sizeof(remote_addr));
|
||||||
remote_addr.sin_family = AF_INET;
|
remote_addr.sin_family = AF_INET;
|
||||||
remote_addr.sin_port = htons((u_short)remotePort);
|
remote_addr.sin_port = htons((u_short)remotePort);
|
||||||
@ -68,64 +55,13 @@ void ParticipantUDP::Setup(int localPort,
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Bind the socket to the specified port
|
// Bind the socket to the specified port
|
||||||
if (bind(this->sock, (const struct sockaddr*)&server_addr,
|
if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
|
||||||
sizeof(server_addr)) < 0) {
|
|
||||||
std::cerr << "Bind failed" << std::endl;
|
std::cerr << "Bind failed" << std::endl;
|
||||||
closesocket(sock);
|
closesocket(sock);
|
||||||
WSACleanup();
|
WSACleanup();
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // _WIN32 || _WIN64
|
#endif // _WIN32 || _WIN64
|
||||||
}
|
|
||||||
|
|
||||||
void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) {
|
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
|
||||||
// Initialize Winsock
|
|
||||||
WSADATA wsaData;
|
|
||||||
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
|
|
||||||
std::cerr << "WSAStartup failed: " << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Create a TCP socket
|
|
||||||
this->sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
|
|
||||||
if (sock == INVALID_SOCKET) {
|
|
||||||
std::cerr << "TCP Socket creation failed: " << WSAGetLastError()
|
|
||||||
<< std::endl;
|
|
||||||
WSACleanup();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the socket to non-blocking mode
|
|
||||||
u_long mode = 1; // 1 to enable non-blocking socket
|
|
||||||
if (ioctlsocket(sock, FIONBIO, &mode) != NO_ERROR) {
|
|
||||||
std::cerr << "Failed to set non-blocking mode: " << WSAGetLastError()
|
|
||||||
<< std::endl;
|
|
||||||
closesocket(sock);
|
|
||||||
WSACleanup();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Define the server address
|
|
||||||
memset(&this->remote_addr, 0, sizeof(server_addr));
|
|
||||||
this->remote_addr.sin_family = AF_INET;
|
|
||||||
this->remote_addr.sin_port = htons((u_short)remotePort);
|
|
||||||
if (inet_pton(AF_INET, remoteIpAddress, &this->remote_addr.sin_addr) <= 0) {
|
|
||||||
std::cerr << "Invalid address" << std::endl;
|
|
||||||
closesocket(sock);
|
|
||||||
WSACleanup();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Connect to the server
|
|
||||||
if (connect(sock, (sockaddr*)&this->remote_addr, sizeof(this->remote_addr)) ==
|
|
||||||
SOCKET_ERROR) {
|
|
||||||
std::cerr << "Connection failed: " << WSAGetLastError() << std::endl;
|
|
||||||
closesocket(sock);
|
|
||||||
WSACleanup();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
#endif // _WIN32 || _WIN64
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParticipantUDP::Receive() {
|
void ParticipantUDP::Receive() {
|
||||||
@ -137,8 +73,7 @@ void ParticipantUDP::Receive() {
|
|||||||
|
|
||||||
sockaddr_in client_addr;
|
sockaddr_in client_addr;
|
||||||
int len = sizeof(client_addr);
|
int len = sizeof(client_addr);
|
||||||
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0,
|
int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
|
||||||
(struct sockaddr*)&client_addr, &len);
|
|
||||||
// std::cout << "received data " << packetSize << "\n";
|
// std::cout << "received data " << packetSize << "\n";
|
||||||
if (packetSize < 0) {
|
if (packetSize < 0) {
|
||||||
int error_code = WSAGetLastError(); // Get the error code on Windows
|
int error_code = WSAGetLastError(); // Get the error code on Windows
|
||||||
@ -146,14 +81,12 @@ void ParticipantUDP::Receive() {
|
|||||||
std::cerr << "recvfrom failed with error: " << error_code << std::endl;
|
std::cerr << "recvfrom failed with error: " << error_code << std::endl;
|
||||||
} else if (packetSize > 0) {
|
} else if (packetSize > 0) {
|
||||||
char sender_ipAddress[INET_ADDRSTRLEN];
|
char sender_ipAddress[INET_ADDRSTRLEN];
|
||||||
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress,
|
inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, INET_ADDRSTRLEN);
|
||||||
INET_ADDRSTRLEN);
|
|
||||||
unsigned int sender_port = ntohs(client_addr.sin_port);
|
unsigned int sender_port = ntohs(client_addr.sin_port);
|
||||||
|
|
||||||
ReceiveData(packetSize, sender_ipAddress, sender_port);
|
ReceiveData(packetSize, sender_ipAddress, sender_port);
|
||||||
// RoboidControl::ParticipantUDP* remoteParticipant =
|
// RoboidControl::ParticipantUDP* remoteParticipant = this->Get(sender_ipAddress, sender_port);
|
||||||
// this->Get(sender_ipAddress, sender_port); if (remoteParticipant ==
|
// if (remoteParticipant == nullptr) {
|
||||||
// nullptr) {
|
|
||||||
// remoteParticipant = this->Add(sender_ipAddress, sender_port);
|
// remoteParticipant = this->Add(sender_ipAddress, sender_port);
|
||||||
// // std::cout << "New sender " << sender_ipAddress << ":"
|
// // std::cout << "New sender " << sender_ipAddress << ":"
|
||||||
// // << sender_port << "\n";
|
// // << sender_port << "\n";
|
||||||
@ -166,43 +99,15 @@ void ParticipantUDP::Receive() {
|
|||||||
|
|
||||||
// ReceiveData(packetSize, remoteParticipant);
|
// ReceiveData(packetSize, remoteParticipant);
|
||||||
}
|
}
|
||||||
#endif // _WIN32 || _WIN64
|
#endif // _WIN32 || _WIN64
|
||||||
}
|
}
|
||||||
|
|
||||||
int ParticipantUDP::ReceiveTCP() {
|
bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
|
||||||
int bytesReceived = recv(sock, buffer, sizeof(buffer) - 1, 0);
|
|
||||||
if (bytesReceived > 0) {
|
|
||||||
buffer[bytesReceived] = '\0'; // Null-terminate the received data
|
|
||||||
std::cout << "Received: " << buffer << std::endl;
|
|
||||||
return bytesReceived;
|
|
||||||
} else if (bytesReceived == 0) {
|
|
||||||
// Connection has been gracefully closed
|
|
||||||
std::cout << "Connection closed by the server." << std::endl;
|
|
||||||
return 0;
|
|
||||||
} else {
|
|
||||||
int error = WSAGetLastError();
|
|
||||||
if (error == WSAEWOULDBLOCK) {
|
|
||||||
// No data available, continue with other tasks
|
|
||||||
std::cout << "No data available, continuing..." << std::endl;
|
|
||||||
// You can add a sleep or other logic here to avoid busy waiting
|
|
||||||
} else {
|
|
||||||
std::cerr << "Receive failed: " << error << std::endl;
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
#endif // _WIN32 || _WIN64
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
|
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
#if defined(_WIN32) || defined(_WIN64)
|
||||||
char ip_str[INET_ADDRSTRLEN];
|
char ip_str[INET_ADDRSTRLEN];
|
||||||
inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
|
inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
|
||||||
std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port)
|
std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port) << "\n";
|
||||||
<< "\n";
|
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
|
||||||
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0,
|
|
||||||
(struct sockaddr*)&remote_addr, sizeof(remote_addr));
|
|
||||||
|
|
||||||
if (sent_bytes <= SOCKET_ERROR) {
|
if (sent_bytes <= SOCKET_ERROR) {
|
||||||
int error_code = WSAGetLastError(); // Get the error code on Windows
|
int error_code = WSAGetLastError(); // Get the error code on Windows
|
||||||
@ -211,17 +116,10 @@ bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferS
|
|||||||
WSACleanup();
|
WSACleanup();
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
#endif // _WIN32 || _WIN64
|
#endif // _WIN32 || _WIN64
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ParticipantUDP::SendTCP(int bufferSize) {
|
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
|
||||||
send(sock, this->buffer, bufferSize, 0);
|
|
||||||
#endif
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ParticipantUDP::Publish(IMessage* msg) {
|
bool ParticipantUDP::Publish(IMessage* msg) {
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
#if defined(_WIN32) || defined(_WIN64)
|
||||||
int bufferSize = msg->Serialize(this->buffer);
|
int bufferSize = msg->Serialize(this->buffer);
|
||||||
@ -230,11 +128,8 @@ bool ParticipantUDP::Publish(IMessage* msg) {
|
|||||||
|
|
||||||
char ip_str[INET_ADDRSTRLEN];
|
char ip_str[INET_ADDRSTRLEN];
|
||||||
inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
|
inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
|
||||||
std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port)
|
std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) << "\n";
|
||||||
<< "\n";
|
int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
|
||||||
int sent_bytes =
|
|
||||||
sendto(sock, this->buffer, bufferSize, 0,
|
|
||||||
(struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
|
|
||||||
|
|
||||||
if (sent_bytes <= SOCKET_ERROR) {
|
if (sent_bytes <= SOCKET_ERROR) {
|
||||||
int error_code = WSAGetLastError(); // Get the error code on Windows
|
int error_code = WSAGetLastError(); // Get the error code on Windows
|
||||||
@ -243,10 +138,9 @@ bool ParticipantUDP::Publish(IMessage* msg) {
|
|||||||
WSACleanup();
|
WSACleanup();
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
#endif // _WIN32 || _WIN64
|
#endif // _WIN32 || _WIN64
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} // namespace Windows
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
|
|
||||||
#endif
|
|
@ -1,27 +1,22 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
|
||||||
|
|
||||||
#include "Participants/ParticipantUDP.h"
|
#include "Participants/ParticipantUDP.h"
|
||||||
|
|
||||||
namespace RoboidControl {
|
namespace RoboidControl {
|
||||||
|
namespace Windows {
|
||||||
|
|
||||||
class ParticipantUDP : public ParticipantUDPGeneric {
|
class ParticipantUDP : public RoboidControl::ParticipantUDP {
|
||||||
public:
|
public:
|
||||||
ParticipantUDP();
|
|
||||||
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
|
void Setup(int localPort, const char* remoteIpAddress, int remotePort);
|
||||||
void SetupTCP(const char* remoteIpAddres, int remotePort);
|
void Receive();
|
||||||
bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
|
bool Send(Participant* remoteParticipant, int bufferSize);
|
||||||
bool SendTCP(int bufferSize);
|
|
||||||
bool Publish(IMessage* msg);
|
bool Publish(IMessage* msg);
|
||||||
|
|
||||||
void Receive();
|
|
||||||
int ReceiveTCP();
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
#if defined(_WIN32) || defined(_WIN64)
|
#if defined(_WIN32) || defined(_WIN64)
|
||||||
SOCKET sock;
|
SOCKET sock;
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
|
} // namespace Windows
|
||||||
} // namespace RoboidControl
|
} // namespace RoboidControl
|
||||||
#endif
|
|
@ -27,10 +27,10 @@ int main() {
|
|||||||
while (true) {
|
while (true) {
|
||||||
// The left wheel turns forward when nothing is touched on the right side
|
// The left wheel turns forward when nothing is touched on the right side
|
||||||
// and turn backward when the roboid hits something on the right
|
// 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
|
// The right wheel does the same, but instead is controlled by
|
||||||
// touches on the left side
|
// 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
|
// When both sides are touching something, both wheels will turn backward
|
||||||
// and the roboid will move backwards
|
// and the roboid will move backwards
|
||||||
bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
|
bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
|
||||||
@ -40,9 +40,9 @@ int main() {
|
|||||||
|
|
||||||
// and sleep for 100ms
|
// and sleep for 100ms
|
||||||
#if defined(ARDUINO)
|
#if defined(ARDUINO)
|
||||||
delay(10);
|
delay(100);
|
||||||
#else
|
#else
|
||||||
sleep_for(milliseconds(10));
|
sleep_for(milliseconds(100));
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
@ -1,8 +1,25 @@
|
|||||||
# examples/CMakeLists.txt
|
# examples/CMakeLists.txt
|
||||||
|
|
||||||
# Check if the options are enabled and add the corresponding examples
|
# Specify the minimum CMake version
|
||||||
if(BUILD_EXAMPLE_BB2A)
|
cmake_minimum_required(VERSION 3.10)
|
||||||
add_executable(BB2A BB2A/main.cpp) # Adjust the path as necessary
|
|
||||||
|
|
||||||
target_link_libraries(BB2A RoboidControl)
|
# Specify the path to the main project directory
|
||||||
endif()
|
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
|
||||||
|
)
|
||||||
|
@ -1 +1 @@
|
|||||||
Important: this folder has to be named 'examples' exactly to maintain compatibility with Arduino
|
Important: this folder has to be names 'examples' exactly to maintain compatibility with Arduino
|
@ -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)
|
|
@ -5,7 +5,6 @@
|
|||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
#include "Participants/SiteServer.h"
|
#include "Participants/SiteServer.h"
|
||||||
#include "Participants/ParticipantMQTT.h"
|
|
||||||
#include "Thing.h"
|
#include "Thing.h"
|
||||||
|
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
@ -76,18 +75,4 @@ TEST(Participant, ThingMsg) {
|
|||||||
SUCCEED();
|
SUCCEED();
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(Participant, MQTT) {
|
|
||||||
MQTTParticipant* participant = new MQTTParticipant("192.168.77.11");
|
|
||||||
|
|
||||||
unsigned long milliseconds = Thing::GetTimeMs();
|
|
||||||
unsigned long startTime = milliseconds;
|
|
||||||
while (milliseconds < startTime + 10000) {
|
|
||||||
participant->Update();
|
|
||||||
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
||||||
milliseconds = Thing::GetTimeMs();
|
|
||||||
}
|
|
||||||
SUCCEED();
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
|
|||||||
unsigned long milliseconds = Thing::GetTimeMs();
|
unsigned long milliseconds = Thing::GetTimeMs();
|
||||||
unsigned long startTime = milliseconds;
|
unsigned long startTime = milliseconds;
|
||||||
while (milliseconds < startTime + 1000) {
|
while (milliseconds < startTime + 1000) {
|
||||||
thing->Update();
|
Thing::UpdateThings();
|
||||||
|
|
||||||
milliseconds = Thing::GetTimeMs();
|
milliseconds = Thing::GetTimeMs();
|
||||||
}
|
}
|
||||||
@ -23,7 +23,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(RoboidControlSuite, IsolatedParticipant) {
|
TEST(RoboidControlSuite, IsolatedParticipant) {
|
||||||
ParticipantUDP* participant = new ParticipantUDP(0);
|
ParticipantUDP* participant = ParticipantUDP::Isolated();
|
||||||
Thing* thing = new Thing();
|
Thing* thing = new Thing();
|
||||||
|
|
||||||
unsigned long milliseconds = Thing::GetTimeMs();
|
unsigned long milliseconds = Thing::GetTimeMs();
|
||||||
|
Loading…
x
Reference in New Issue
Block a user