From 701316d687975595a1739c06ac97e6397cd0bd52 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 18 Dec 2024 16:53:54 +0100 Subject: [PATCH] Thing -> ControlCore --- ControlCore/CoreThing.cpp | 64 ------------- ControlCore/CoreThing.h | 75 --------------- ControlCore/Messages.cpp | 24 +++-- ControlCore/Messages.h | 13 ++- ControlCore/Thing.cpp | 195 ++++++++++++++++++++++++++++++++++++++ ControlCore/Thing.h | 133 ++++++++++++++++++++++++++ ControlledMotor.cpp | 4 +- DifferentialDrive.cpp | 20 ++-- DirectionalSensor.cpp | 2 +- DirectionalSensor.h | 3 +- DistanceSensor.cpp | 6 +- Motor.cpp | 4 +- Motor.h | 3 +- NetworkSync.cpp | 30 +++--- Perception.cpp | 89 +++++++++-------- Quadcopter.h | 2 +- Roboid.cpp | 21 ++-- Roboid.h | 2 +- Sensor.cpp | 29 +++--- Sensor.h | 4 +- ServoMotor.cpp | 6 +- Switch.cpp | 2 +- TemperatureSensor.cpp | 14 +-- Thing.cpp | 166 -------------------------------- Thing.h | 121 ----------------------- 25 files changed, 485 insertions(+), 547 deletions(-) delete mode 100644 ControlCore/CoreThing.cpp delete mode 100644 ControlCore/CoreThing.h create mode 100644 ControlCore/Thing.cpp create mode 100644 ControlCore/Thing.h delete mode 100644 Thing.cpp delete mode 100644 Thing.h diff --git a/ControlCore/CoreThing.cpp b/ControlCore/CoreThing.cpp deleted file mode 100644 index 93a081b..0000000 --- a/ControlCore/CoreThing.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "CoreThing.h" - -#include "Participant.h" -#include - -CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) { - this->type = thingType; - this->networkId = networkId; - this->Init(); - - int thingId = CoreThing::Add(this); - - if (thingId < 0) { - std::cout << "ERROR: Thing store is full\n"; - this->id = 0; // what to do when we cannot store any more things? - } else - this->id = thingId; -} - -void CoreThing::Terminate() { CoreThing::Remove(this); } - -void CoreThing::Init() {} - -CoreThing *CoreThing::GetParent() { return this->parent; } - -// All things -CoreThing *CoreThing::allThings[THING_STORE_SIZE] = {nullptr}; - -CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) { - for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) { - CoreThing *thing = allThings[ix]; - if (thing == nullptr) - continue; - if (thing->networkId == networkId && thing->id == thingId) - return thing; - } - return nullptr; -} - -int CoreThing::Add(CoreThing *newThing) { - for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) { - CoreThing *thing = allThings[ix]; - if (thing == nullptr) { - allThings[ix] = newThing; - - // std::cout << " Add new thing " << (int)ix << "\n"; - return ix; - } - } - return -1; -} - -void CoreThing::Remove(CoreThing *thing) { allThings[thing->id] = nullptr; } - -void CoreThing::UpdateAll(unsigned long currentTimeMs) { - // Not very efficient, but it works for now. - for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) { - CoreThing *thing = allThings[ix]; - if (thing != nullptr && - thing->parent == nullptr) { // update all root things - thing->Update(currentTimeMs); - } - } -} \ No newline at end of file diff --git a/ControlCore/CoreThing.h b/ControlCore/CoreThing.h deleted file mode 100644 index bf441b4..0000000 --- a/ControlCore/CoreThing.h +++ /dev/null @@ -1,75 +0,0 @@ -#pragma once -#include - -namespace Passer { -namespace Control { - -#define THING_STORE_SIZE 256 -// IMPORTANT: values higher than 256 will need to change the CoreThing::id type -// to 16-bit or higher, breaking the networking protocol! - -class CoreThing { -public: - // Participant *client; - unsigned char networkId; - /// @char The id of the thing - unsigned char id; - CoreThing *parent; - /// @brief The type of Thing - unsigned char type; - const char *name = nullptr; - const char *modelUrl = nullptr; - float modelScale = 1; - // protected Sensor sensor; - - /// @brief Basic Thing types - enum class Type { - Undetermined, - // Sensor, - Switch, - DistanceSensor, - DirectionalSensor, - TemperatureSensor, - // Motor, - ControlledMotor, - UncontrolledMotor, - Servo, - // Other - Humanoid, - ExternalSensor, - }; - -public: - CoreThing(unsigned char networkId = 0, - unsigned char thingType = (unsigned char)Type::Undetermined); - /// @brief Terminated thins are no longer updated - void Terminate(); - - /// @brief Gets the parent Thing - /// @return The parent Thing - CoreThing *GetParent(); - - /// @brief Updates the state of the thing - /// @param currentTimeMs The current clock time in milliseconds - virtual void Update(unsigned long currentTimeMs) {}; - - virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {}; - virtual void ProcessBytes(unsigned char *bytes) {}; - -protected: - virtual void Init(); - - //------------ All things -public: - static CoreThing *Get(unsigned char networkId, unsigned char thingId); - static int Add(CoreThing *thing); - static void Remove(CoreThing *thing); - static void UpdateAll(unsigned long currentTimeMs); - -private: - static CoreThing *allThings[]; -}; - -} // namespace Control -} // namespace Passer -using namespace Passer::Control; \ No newline at end of file diff --git a/ControlCore/Messages.cpp b/ControlCore/Messages.cpp index 67e1269..78c6da1 100644 --- a/ControlCore/Messages.cpp +++ b/ControlCore/Messages.cpp @@ -158,7 +158,7 @@ unsigned char NameMsg::Serialize(unsigned char *buffer) { return ix; } -// bool NameMsg::Send(Participant *participant, CoreThing *thing, +// bool NameMsg::Send(Participant *participant, Thing *thing, // unsigned char nameLength) { // if (thing->name == nullptr) // return true; // nothing sent, but still a success! @@ -204,12 +204,16 @@ unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) { PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId, unsigned char poseType, Spherical16 position, - SwingTwist16 orientation) { + SwingTwist16 orientation, Spherical16 linearVelocity, + Spherical16 angularVelocity) { this->networkId = networkId; this->thingId = thingId; + + this->poseType = poseType; this->position = position; this->orientation = orientation; - this->poseType = poseType; + this->linearVelocity = linearVelocity; + this->angularVelocity = angularVelocity; } PoseMsg::PoseMsg(const unsigned char *buffer) { unsigned char ix = 1; // First byte is msg id @@ -226,8 +230,14 @@ unsigned char PoseMsg::Serialize(unsigned char *buffer) { buffer[ix++] = this->networkId; buffer[ix++] = this->thingId; buffer[ix++] = this->poseType; - LowLevelMessages::SendSpherical16(buffer, &ix, this->position); - LowLevelMessages::SendQuat32(buffer, &ix, this->orientation); + if ((this->poseType & Pose_Position) != 0) + LowLevelMessages::SendSpherical16(buffer, &ix, this->position); + if ((this->poseType & Pose_Orientation) != 0) + LowLevelMessages::SendQuat32(buffer, &ix, this->orientation); + if ((this->poseType & Pose_LinearVelocity) != 0) + LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity); + if ((this->poseType & Pose_AngularVelocity) != 0) + LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity); return ix; } @@ -245,7 +255,7 @@ CustomMsg::CustomMsg(unsigned char *buffer) { // lifetime is shorter than the buffer lifetime... } -CustomMsg::CustomMsg(unsigned char networkId, CoreThing *thing) { +CustomMsg::CustomMsg(unsigned char networkId, Thing *thing) { this->networkId = networkId; this->thingId = thing->id; this->thing = thing; @@ -273,7 +283,7 @@ CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) { #pragma region DestroyMsg -DestroyMsg::DestroyMsg(unsigned char networkId, CoreThing *thing) { +DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) { this->networkId = networkId; this->thingId = thing->id; } diff --git a/ControlCore/Messages.h b/ControlCore/Messages.h index f45c3b0..5fcfefa 100644 --- a/ControlCore/Messages.h +++ b/ControlCore/Messages.h @@ -2,7 +2,7 @@ #include "../LinearAlgebra/Spherical.h" #include "../LinearAlgebra/SwingTwist.h" -#include "CoreThing.h" +#include "Thing.h" #include "float16.h" namespace Passer { @@ -118,10 +118,13 @@ public: Spherical16 position; SwingTwist16 orientation; + Spherical16 linearVelocity; + Spherical16 angularVelocity; PoseMsg(unsigned char networkId, unsigned char thingId, unsigned char poseType, Spherical16 position, - SwingTwist16 orientation); + SwingTwist16 orientation, Spherical16 linearVelocity = Spherical16(), + Spherical16 angularVelocity = Spherical16()); PoseMsg(const unsigned char *buffer); virtual unsigned char Serialize(unsigned char *buffer) override; @@ -134,13 +137,13 @@ public: unsigned char networkId; unsigned char thingId; - CoreThing *thing; + Thing *thing; unsigned char dataSize; unsigned char *data; CustomMsg(unsigned char *buffer); - CustomMsg(unsigned char networkId, CoreThing *thing); + CustomMsg(unsigned char networkId, Thing *thing); virtual unsigned char Serialize(unsigned char *buffer) override; @@ -154,7 +157,7 @@ public: unsigned char networkId; unsigned char thingId; - DestroyMsg(unsigned char networkId, CoreThing *thing); + DestroyMsg(unsigned char networkId, Thing *thing); virtual unsigned char Serialize(unsigned char *buffer) override; }; diff --git a/ControlCore/Thing.cpp b/ControlCore/Thing.cpp new file mode 100644 index 0000000..2f9dd73 --- /dev/null +++ b/ControlCore/Thing.cpp @@ -0,0 +1,195 @@ +#include "Thing.h" + +#include "Participant.h" +#include +#include + +Thing::Thing(unsigned char networkId, unsigned char thingType) { + this->type = thingType; + this->networkId = networkId; + this->Init(); + + int thingId = Thing::Add(this); + + if (thingId < 0) { + std::cout << "ERROR: Thing store is full\n"; + this->id = 0; // what to do when we cannot store any more things? + } else + this->id = thingId; + + this->linearVelocity = Spherical16::zero; + this->angularVelocity = Spherical16::zero; +} + +void Thing::Terminate() { Thing::Remove(this); } + +void Thing::Init() {} + +Thing *Thing::FindThing(const char *name) { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing *child = this->children[childIx]; + if (child == nullptr || child->name == nullptr) + continue; + + if (strcmp(child->name, name) == 0) + return child; + + Thing *foundChild = child->FindThing(name); + if (foundChild != nullptr) + return foundChild; + } + return nullptr; +} + +void Thing::SetParent(Thing *parent) { + if (parent == nullptr) { + Thing *parentThing = this->parent; + if (parentThing != nullptr) + parentThing->RemoveChild(this); + this->parent = nullptr; + } else + parent->AddChild(this); +} + +void Thing::SetParent(Thing *root, const char *name) { + Thing *thing = root->FindThing(name); + if (thing != nullptr) + this->SetParent(thing); +} + +Thing *Thing::GetParent() { return this->parent; } + +void Thing::AddChild(Thing *child) { + + unsigned char newChildCount = this->childCount + 1; + Thing **newChildren = new Thing *[newChildCount]; + + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + newChildren[childIx] = this->children[childIx]; + if (this->children[childIx] == child) { + // child is already present, stop copying do not update the children + delete[] newChildren; + return; + } + } + + newChildren[this->childCount] = child; + child->parent = this; + + if (this->children != nullptr) + delete[] this->children; + + this->children = newChildren; + this->childCount = newChildCount; +} + +Thing *Thing::RemoveChild(Thing *child) { + unsigned char newChildCount = this->childCount - 1; + Thing **newChildren = new Thing *[newChildCount]; + + unsigned char newChildIx = 0; + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + if (this->children[childIx] != child) { + if (newChildIx == newChildCount) { // We did not find the child + // stop copying and return nothing + delete[] newChildren; + return nullptr; + } else + newChildren[newChildIx++] = this->children[childIx]; + } + } + + child->parent = nullptr; + + delete[] this->children; + this->children = newChildren; + this->childCount = newChildCount; + + return child; +} + +Thing *Passer::Control::Thing::GetChild(unsigned char id, bool recursive) { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { + Thing *child = this->children[childIx]; + if (child == nullptr) + continue; + if (child->id == id) + return child; + + if (recursive) { + Thing *foundChild = child->GetChild(id, recursive); + if (foundChild != nullptr) + return foundChild; + } + } + return nullptr; +} + +Thing *Passer::Control::Thing::GetChildByIndex(unsigned char ix) { + return this->children[ix]; +} + +void Thing::SetModel(const char *url) { this->modelUrl = url; } + +void Thing::SetPosition(Spherical16 position) { + this->position = position; + this->positionUpdated = true; +} +Spherical16 Thing::GetPosition() { return this->position; } + +void Thing::SetOrientation(SwingTwist16 orientation) { + this->orientation = orientation; + this->orientationUpdated = true; +} + +SwingTwist16 Thing::GetOrientation() { return this->orientation; } + +Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; } + +Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; } + +// All things +Thing *Thing::allThings[THING_STORE_SIZE] = {nullptr}; + +Thing *Thing::Get(unsigned char networkId, unsigned char thingId) { + for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) { + Thing *thing = allThings[ix]; + if (thing == nullptr) + continue; + if (thing->networkId == networkId && thing->id == thingId) + return thing; + } + return nullptr; +} + +int Thing::Add(Thing *newThing) { + // Exclude 0 because that value is reserved for 'no thing' + for (uint16_t ix = 1; ix < THING_STORE_SIZE; ix++) { + Thing *thing = allThings[ix]; + if (thing == nullptr) { + allThings[ix] = newThing; + + // std::cout << " Add new thing " << (int)ix << "\n"; + return ix; + } + } + return -1; +} + +void Thing::Remove(Thing *thing) { + // std::cout << " remove " << (int)thing->id << "\n"; + allThings[thing->id] = nullptr; +} + +void Thing::UpdateAll(unsigned long currentTimeMs) { + // Not very efficient, but it works for now. + for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) { + Thing *thing = allThings[ix]; + if (thing != nullptr && + thing->parent == nullptr) { // update all root things + // std::cout << " update " << (int)ix << " thingid " << (int)thing->id + // << "\n"; + thing->Update(currentTimeMs); + } + } +} \ No newline at end of file diff --git a/ControlCore/Thing.h b/ControlCore/Thing.h new file mode 100644 index 0000000..239361c --- /dev/null +++ b/ControlCore/Thing.h @@ -0,0 +1,133 @@ +#pragma once +#include "../LinearAlgebra/Spherical.h" +#include "../LinearAlgebra/SwingTwist.h" +#include + +namespace Passer { +namespace Control { + +#define THING_STORE_SIZE 256 +// IMPORTANT: values higher than 256 will need to change the Thing::id type +// to 16-bit or higher, breaking the networking protocol! + +class Thing { +public: + // Participant *client; + unsigned char networkId = 0; + /// @char The id of the thing + unsigned char id = 0; + + Thing *FindThing(const char *name); + // Thing *FindChild(unsigned char id); + + /// @brief Sets the parent Thing + /// @param parent The Thing which should become the parnet + /// @remark This is equivalent to calling parent->AddChild(this); + virtual void SetParent(Thing *parent); + void SetParent(Thing *root, const char *name); + /// @brief Gets the parent Thing + /// @return The parent Thing + Thing *GetParent(); + + /// @brief Add a child Thing to this Thing + /// @param child The Thing which should become a child + /// @remark When the Thing is already a child, it will not be added again + virtual void AddChild(Thing *child); + Thing *RemoveChild(Thing *child); + + unsigned char childCount = 0; + Thing *GetChild(unsigned char id, bool recursive = false); + Thing *GetChildByIndex(unsigned char ix); + +protected: + Thing *parent = nullptr; + Thing **children = nullptr; + +public: + /// @brief The type of Thing + unsigned char type = 0; + const char *name = nullptr; + const char *modelUrl = nullptr; + float modelScale = 1; + // protected Sensor sensor; + + /// @brief Basic Thing types + enum class Type { + Undetermined, + // Sensor, + Switch, + DistanceSensor, + DirectionalSensor, + TemperatureSensor, + // Motor, + ControlledMotor, + UncontrolledMotor, + Servo, + // Other + Roboid, + Humanoid, + ExternalSensor, + }; + + void SetPosition(Spherical16 position); + Spherical16 GetPosition(); + void SetOrientation(SwingTwist16 orientation); + SwingTwist16 GetOrientation(); + float scale = 1; // assuming uniform scale + + bool positionUpdated = false; + bool orientationUpdated = false; + +protected: + /// @brief The position in local space + /// @remark When this Thing has a parent, the position is relative to the + /// parent's position and orientation + Spherical16 position; + /// @brief The orientation in local space + /// @remark When this Thing has a parent, the orientation is relative to the + /// parent's orientation + SwingTwist16 orientation; + +public: + Spherical16 linearVelocity; + Spherical16 angularVelocity; + virtual Spherical16 GetLinearVelocity(); + virtual Spherical16 GetAngularVelocity(); + +public: + Thing(unsigned char networkId = 0, + unsigned char thingType = (unsigned char)Type::Undetermined); + /// @brief Terminated thins are no longer updated + void Terminate(); + + /// @brief Sets the location from where the 3D model of this Thing can be + /// loaded from + /// @param url The url of the model + /// @remark Although the roboid implementation is not dependent on the model, + /// the only official supported model format is .obj + void SetModel(const char *url); + + /// @brief Updates the state of the thing + /// @param currentTimeMs The current clock time in milliseconds + virtual void Update(unsigned long currentTimeMs) {}; + + virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {}; + virtual void ProcessBytes(unsigned char *bytes) {}; + +protected: + virtual void Init(); + + //------------ All things +public: + static Thing *Get(unsigned char networkId, unsigned char thingId); + static int Add(Thing *thing); + static void Remove(Thing *thing); + static void UpdateAll(unsigned long currentTimeMs); + +private: + static Thing *allThings[]; +}; + +} // namespace Control +} // namespace Passer +using namespace Passer::Control; \ No newline at end of file diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp index bec227e..d999a73 100644 --- a/ControlledMotor.cpp +++ b/ControlledMotor.cpp @@ -1,6 +1,8 @@ #include "ControlledMotor.h" -ControlledMotor::ControlledMotor() { this->type = Thing::ControlledMotorType; } +ControlledMotor::ControlledMotor() { + this->type = (unsigned char)Type::ControlledMotor; +} ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder) : ControlledMotor() { diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 316d23f..3498dbd 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -14,11 +14,13 @@ DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) { float distance = this->wheelSeparation / 2; leftMotor->direction = Motor::Direction::CounterClockwise; - leftMotor->position.direction.horizontal = Angle16::Degrees(-90); - leftMotor->position.distance = distance; + leftMotor->SetPosition(Spherical16(distance, Direction16::left)); + // leftMotor->position.direction.horizontal = Angle16::Degrees(-90); + // leftMotor->position.distance = distance; rightMotor->direction = Motor::Direction::Clockwise; - rightMotor->position.direction.horizontal = Angle16::Degrees(90); - rightMotor->position.distance = distance; + rightMotor->SetPosition(Spherical16(distance, Direction16::right)); + // rightMotor->position.direction.horizontal = Angle16::Degrees(90); + // rightMotor->position.distance = distance; } void DifferentialDrive::SetDimensions(float wheelDiameter, @@ -28,8 +30,12 @@ void DifferentialDrive::SetDimensions(float wheelDiameter, this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi; float distance = this->wheelSeparation / 2; - this->motors[0]->position.distance = distance; - this->motors[1]->position.distance = distance; + Spherical16 motor0Position = this->motors[0]->GetPosition(); + motor0Position.distance = distance; + this->motors[0]->SetPosition(motor0Position); + Spherical16 motor1Position = this->motors[0]->GetPosition(); + motor1Position.distance = distance; + this->motors[1]->SetPosition(motor1Position); } void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, @@ -40,7 +46,7 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, continue; float xPosition = - motors[motorIx]->position.direction.horizontal.InDegrees(); + motors[motorIx]->GetPosition().direction.horizontal.InDegrees(); if (xPosition < 0) motor->SetTargetSpeed(leftSpeed); else if (xPosition > 0) diff --git a/DirectionalSensor.cpp b/DirectionalSensor.cpp index b590705..6896cca 100644 --- a/DirectionalSensor.cpp +++ b/DirectionalSensor.cpp @@ -3,7 +3,7 @@ #include "ControlCore/LowLevelMessages.h" DirectionalSensor::DirectionalSensor() : Sensor() { - this->type = DirectionalSensor::Type; + this->type = (unsigned char)Type::DirectionalSensor; this->vector = Spherical16::zero; } diff --git a/DirectionalSensor.h b/DirectionalSensor.h index 90a866f..fd570c9 100644 --- a/DirectionalSensor.h +++ b/DirectionalSensor.h @@ -11,7 +11,8 @@ public: /// @brief Default constructor DirectionalSensor(); - const unsigned int Type = SensorType | (unsigned int)Type::DirectionalSensor; + // const unsigned int Type = SensorType | (unsigned + // int)Type::DirectionalSensor; /// @brief Determine the distance to the nearest object /// @return the measured distance in meters to the nearest object diff --git a/DistanceSensor.cpp b/DistanceSensor.cpp index a2a759b..5c888fb 100644 --- a/DistanceSensor.cpp +++ b/DistanceSensor.cpp @@ -1,11 +1,10 @@ #include "DistanceSensor.h" -// #include "NetworkPerception.h" #include "ControlCore/LowLevelMessages.h" #include -DistanceSensor::DistanceSensor() { - this->type = Thing::DistanceSensorType; +DistanceSensor::DistanceSensor() : Sensor() { + this->type = (unsigned char)Type::DistanceSensor; this->distance = INFINITY; this->triggerDistance = 1.0F; } @@ -34,4 +33,5 @@ bool DistanceSensor::ObjectNearby() { void DistanceSensor::ProcessBytes(unsigned char *bytes) { unsigned char ix = 0; this->distance = LowLevelMessages::ReceiveFloat16(bytes, &ix); + // std::cout << "received distance " << this->distance << "\n"; } diff --git a/Motor.cpp b/Motor.cpp index 373641f..7b9ae6e 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -2,7 +2,9 @@ #include -Motor::Motor() : Thing() { this->type = (int)Thing::UncontrolledMotorType; } +Motor::Motor() : Thing() { + this->type = (unsigned char)Type::UncontrolledMotor; +} float Motor::GetActualSpeed() { return this->currentTargetSpeed; } diff --git a/Motor.h b/Motor.h index 8563c1b..aa0f2db 100644 --- a/Motor.h +++ b/Motor.h @@ -1,10 +1,11 @@ #pragma once -#include "Thing.h" +#include "ControlCore/Thing.h" #include namespace Passer { +using namespace Control; namespace RoboidControl { /// @brief A Motor is a Thing which can move parts of the Roboid diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 2fec8d5..2f9376c 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -68,7 +68,7 @@ void NetworkSync::ProcessInvestigateMsg(InvestigateMsg msg) { Thing *thing = (Thing *)Thing::Get(0, msg.thingId); if (thing != nullptr) - this->SendThingInfo(thing); + this->SendThingInfo(thing, true); } void NetworkSync::ProcessThingMsg(ThingMsg msg) { @@ -104,7 +104,7 @@ void NetworkSync::SendThingInfo(Thing *thing, bool recurse) { if (recurse) { for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) { - Thing *child = thing->GetChild(childIx); + Thing *child = (Thing *)thing->GetChildByIndex(childIx); if (child != nullptr) SendThingInfo(child, true); } @@ -115,7 +115,7 @@ void NetworkSync::SendThing(Thing *thing) { if (thing == nullptr) return; - Thing *parent = thing->GetParent(); + Thing *parent = (Thing *)thing->GetParent(); ThingMsg msg = ThingMsg(this->networkId, thing->id, thing->type, parent == nullptr ? 0 : parent->id); msg.SendTo(this); @@ -171,7 +171,7 @@ void NetworkSync::SendDestroy(Thing *thing) { #include void NetworkSync::ProcessCustomMsg(CustomMsg msg) { // we assume networkId == 0 as custom messages are intended for my things - Thing *thing = roboid->FindChild(msg.thingId); + Thing *thing = (Thing *)roboid->GetChild(msg.thingId, true); if (thing != nullptr) thing->ProcessBytes(msg.data); } @@ -188,8 +188,13 @@ void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) { poseType |= PoseMsg::Pose_Position; if (force || thing->orientationUpdated) poseType |= PoseMsg::Pose_Orientation; - PoseMsg msg = PoseMsg(this->networkId, thing->id, poseType, thing->position, - thing->orientation); + if (thing->linearVelocity.distance > 0) + poseType |= PoseMsg::Pose_LinearVelocity; + if (thing->angularVelocity.distance > 0) + poseType |= PoseMsg::Pose_AngularVelocity; + PoseMsg msg = PoseMsg(this->networkId, thing->id, poseType, + thing->GetPosition(), thing->GetOrientation(), + thing->linearVelocity, thing->angularVelocity); msg.SendTo(this); thing->positionUpdated = false; @@ -209,9 +214,9 @@ void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) { if (recurse) { for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) { - Thing *child = thing->GetChild(childIx); + Thing *child = (Thing *)thing->GetChildByIndex(childIx); if (child != nullptr) - SendPose(thing->GetChild(childIx), force, recurse); + SendPose(child, force, recurse); } } } @@ -260,17 +265,18 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid, originPosition = Spherical16::zero; } else { inv_originOrientation = - SwingTwist16::Inverse(roboid->worldOrigin->orientation); - originPosition = roboid->worldOrigin->position; + SwingTwist16::Inverse(roboid->worldOrigin->GetOrientation()); + originPosition = roboid->worldOrigin->GetPosition(); } // SwingTwist16 inv_originOrientation = // SwingTwist16::Inverse(roboid->worldOrigin->orientation); // Spherical16 originPosition = roboid->worldOrigin->position; - SwingTwist16 worldOrientation = inv_originOrientation * thing->orientation; + SwingTwist16 worldOrientation = + inv_originOrientation * thing->GetOrientation(); Spherical16 worldPosition = - inv_originOrientation * (thing->position - originPosition); + inv_originOrientation * (thing->GetPosition() - originPosition); PoseMsg msg = PoseMsg(this->networkId, thing->id, PoseMsg::Pose_Position | PoseMsg::Pose_Orientation, diff --git a/Perception.cpp b/Perception.cpp index 0a07b38..d5b9120 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -1,4 +1,5 @@ #include "Perception.h" +#include "ControLCore/Thing.h" #include "DistanceSensor.h" #include "LinearAlgebra/Angle.h" #include "NetworkSync.h" @@ -74,10 +75,10 @@ Sensor *Perception::FindSensorOfType(unsigned int sensorType) { float GetPlaneDistance(InterestingThing *plane, float horizontalAngle, float range) { - float distance = plane->position.distance; + float distance = plane->GetPosition().distance; float deltaAngle = Angle::Normalize( - Angle::Degrees(plane->position.direction.horizontal.InDegrees() - + Angle::Degrees(plane->GetPosition().direction.horizontal.InDegrees() - horizontalAngle)) .InDegrees(); if (fabsf(deltaAngle) < fabsf(range)) { @@ -123,11 +124,11 @@ float Perception::GetDistance(float horizontalDirection, float range) { if (obj->type == 0x080) { // plane float planeDistance = GetPlaneDistance(obj, horizontalDirection, range); minDistance = fminf(minDistance, planeDistance); - } else if (obj->position.direction.horizontal.InDegrees() > + } else if (obj->GetPosition().direction.horizontal.InDegrees() > horizontalDirection - range && - obj->position.direction.horizontal.InDegrees() < + obj->GetPosition().direction.horizontal.InDegrees() < horizontalDirection + range) { - minDistance = fminf(minDistance, obj->position.distance); + minDistance = fminf(minDistance, obj->GetPosition().distance); } } return minDistance; @@ -149,11 +150,11 @@ float Perception::GetDistanceOfType(unsigned char thingType, if (thing->type == 0x080) { // plane float planeDistance = GetPlaneDistance(thing, horizontalAngle, range); minDistance = fminf(minDistance, planeDistance); - } else if (thing->position.direction.horizontal.InDegrees() > + } else if (thing->GetPosition().direction.horizontal.InDegrees() > horizontalAngle - range && - thing->position.direction.horizontal.InDegrees() < + thing->GetPosition().direction.horizontal.InDegrees() < horizontalAngle + range) { - minDistance = fminf(minDistance, thing->position.distance); + minDistance = fminf(minDistance, thing->GetPosition().distance); } } return minDistance; @@ -169,11 +170,11 @@ float Perception::GetDistance(float horizontalDirection, InterestingThing *obj = trackedObjects[objIx]; if (obj == nullptr) continue; - if (obj->position.direction.horizontal.InDegrees() > + if (obj->GetPosition().direction.horizontal.InDegrees() > horizontalDirection - range && - obj->position.direction.horizontal.InDegrees() < + obj->GetPosition().direction.horizontal.InDegrees() < horizontalDirection + range) { - minDistance = fminf(minDistance, obj->position.distance); + minDistance = fminf(minDistance, obj->GetPosition().distance); } } @@ -188,9 +189,11 @@ bool Perception::ObjectNearby(float direction, float range) { if (obj == nullptr) continue; - if (obj->position.direction.horizontal.InDegrees() > direction - range && - obj->position.direction.horizontal.InDegrees() < direction + range) { - if (obj->position.distance <= nearbyDistance) + if (obj->GetPosition().direction.horizontal.InDegrees() > + direction - range && + obj->GetPosition().direction.horizontal.InDegrees() < + direction + range) { + if (obj->GetPosition().distance <= nearbyDistance) return true; } } @@ -216,15 +219,15 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical16 position, else { if (thing->IsTheSameAs(this->trackedObjects[thingIx])) { this->trackedObjects[thingIx]->Refresh( - thing->position, thing->orientation); //.ToQuaternion()); + thing->GetPosition(), thing->GetOrientation()); //.ToQuaternion()); delete thing; return this->trackedObjects[thingIx]; } // Is this the fartest object we see? else if (this->trackedObjects[farthestObjIx] == nullptr || - (this->trackedObjects[thingIx]->position.distance > - this->trackedObjects[farthestObjIx]->position.distance)) { + (this->trackedObjects[thingIx]->GetPosition().distance > + this->trackedObjects[farthestObjIx]->GetPosition().distance)) { farthestObjIx = thingIx; } } @@ -241,8 +244,8 @@ Perception::AddTrackedObject(Sensor *sensor, Spherical16 position, return thing; } // If this object is closer than the farthest object, then replace it - else if (thing->position.distance < - this->trackedObjects[farthestObjIx]->position.distance) { + else if (thing->GetPosition().distance < + this->trackedObjects[farthestObjIx]->GetPosition().distance) { delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = thing; thing->networkId = networkId; @@ -275,8 +278,8 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor, InterestingThing *Perception::AddTrackedObject(Sensor *sensor, Thing *orgThing) { - InterestingThing *thing = - new InterestingThing(sensor, orgThing->position, orgThing->orientation); + InterestingThing *thing = new InterestingThing( + sensor, orgThing->GetPosition(), orgThing->GetOrientation()); thing->id = orgThing->id; thing->networkId = 0; thing->type = orgThing->type; @@ -294,15 +297,15 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor, else { if (thing->IsTheSameAs(this->trackedObjects[thingIx])) { this->trackedObjects[thingIx]->Refresh( - thing->position, thing->orientation); //.ToQuaternion()); + thing->GetPosition(), thing->GetOrientation()); //.ToQuaternion()); delete thing; return this->trackedObjects[thingIx]; } // Is this the fartest object we see? else if (this->trackedObjects[farthestObjIx] == nullptr || - (this->trackedObjects[thingIx]->position.distance > - this->trackedObjects[farthestObjIx]->position.distance)) { + (this->trackedObjects[thingIx]->GetPosition().distance > + this->trackedObjects[farthestObjIx]->GetPosition().distance)) { farthestObjIx = thingIx; } } @@ -318,8 +321,8 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor, return thing; } // If this object is closer than the farthest object, then replace it - else if (thing->position.distance < - this->trackedObjects[farthestObjIx]->position.distance) { + else if (thing->GetPosition().distance < + this->trackedObjects[farthestObjIx]->GetPosition().distance) { delete this->trackedObjects[farthestObjIx]; this->trackedObjects[farthestObjIx] = thing; if (thing->id == 0x00) @@ -335,7 +338,7 @@ bool Perception::IsInteresting(float distance) { InterestingThing *thing = this->trackedObjects[objIx]; if (thing == nullptr) return true; - if (thing->position.distance > distance) + if (thing->GetPosition().distance > distance) return true; } return false; @@ -424,9 +427,9 @@ InterestingThing *Perception::GetMostInterestingThing() { for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) { InterestingThing *obj = this->trackedObjects[objIx]; if (obj != nullptr) { - if (obj->position.distance < closestDistance) { + if (obj->GetPosition().distance < closestDistance) { closestObject = obj; - closestDistance = obj->position.distance; + closestDistance = obj->GetPosition().distance; } } } @@ -446,7 +449,7 @@ void Perception::Update(unsigned long currentTimeMs) { if (sensor == nullptr) continue; - if (sensor->type == Thing::DistanceSensorType) { + if (sensor->type == (unsigned char)Thing::Type::DistanceSensor) { DistanceSensor *distanceSensor = (DistanceSensor *)sensor; float distance = distanceSensor->GetDistance(); @@ -456,11 +459,11 @@ void Perception::Update(unsigned long currentTimeMs) { // Polar position = Polar(distance, angle.ToFloat()); // AddTrackedObject(distanceSensor, position); Spherical16 position = - Spherical16(distance, sensor->position.direction); + Spherical16(distance, sensor->GetPosition().direction); AddTrackedObject(distanceSensor, position); } - } else if (sensor->type == Thing::SwitchType) { + } else if (sensor->type == (unsigned char)Thing::Type::Switch) { Switch *switchSensor = (Switch *)sensor; if (switchSensor->IsOn()) { // Polar position = Polar(sensor->position.angle, nearbyDistance); @@ -502,15 +505,18 @@ void Perception::UpdatePose(Polar16 translation) { // (float)thing->position.horizontalAngle, // (float)thing->position.verticalAngle); // Update the closest point to the plane - float angle = (float)thing->position.direction.horizontal.InDegrees() + - translation.angle.InDegrees(); + float angle = + (float)thing->GetPosition().direction.horizontal.InDegrees() + + translation.angle.InDegrees(); angle = fabsf(angle); float deltaDistance = translation.distance * cosf(angle * Passer::LinearAlgebra::Deg2Rad); // printf(" | translate %f %f %f | ", (float)translation.distance, // (float)angle, deltaDistance); - thing->position.distance -= deltaDistance; + Spherical16 position = thing->GetPosition(); + position.distance -= deltaDistance; + thing->SetPosition(position); //.distance -= deltaDistance; // printf("-> %f (%f %f)\n", thing->position.distance, // (float)thing->position.horizontalAngle, // (float)thing->position.verticalAngle); @@ -523,15 +529,15 @@ void Perception::UpdatePose(Polar16 translation) { Spherical16 translationS = Spherical16( translation.distance, Angle16::Degrees(translation.angle.InDegrees()), Angle16::Degrees(0)); - Spherical16 newPosition = thing->position + translationS; + Spherical16 newPosition = thing->GetPosition() + translationS; - Vector3 oldPos = thing->position.ToVector3(); + Vector3 oldPos = thing->GetPosition().ToVector3(); Vector3 newPos = newPosition.ToVector3(); // printf(" update percepted position (%f 0 %f) -> (%f 0 %f)\n", // oldPos.Right(), oldPos.Forward(), newPos.Right(), // newPos.Forward()); - thing->position = newPosition; + thing->SetPosition(newPosition); } } } @@ -556,8 +562,11 @@ void Perception::UpdatePose(SwingTwist16 rotation) { // (float)thing->position.verticalAngle); // printf("| rotate %f | ", rotationAngle); - thing->position.direction.horizontal = - thing->position.direction.horizontal - rotationAngle; + Spherical16 newPosition = thing->GetPosition(); + newPosition.direction.horizontal - rotationAngle; + thing->SetPosition(newPosition); + // thing->position.direction.horizontal = + // thing->position.direction.horizontal - rotationAngle; // printf("-> %f (%f %f) \n", thing->position.distance, // (float)thing->position.horizontalAngle, diff --git a/Quadcopter.h b/Quadcopter.h index 113c240..3a10bc9 100644 --- a/Quadcopter.h +++ b/Quadcopter.h @@ -1,8 +1,8 @@ #pragma once +#include "ControlCore/Thing.h" #include "LinearAlgebra/Vector3.h" #include "Propulsion.h" -#include "Thing.h" namespace Passer { namespace RoboidControl { diff --git a/Roboid.cpp b/Roboid.cpp index 0a3ee67..fd39498 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -15,7 +15,7 @@ Roboid::Roboid() : Thing() { #ifdef RC_DEBUG Serial.begin(115200); #endif - this->type = (unsigned char)RoboidType; + this->type = (unsigned char)Type::Roboid; this->perception = new Perception(); this->perception->roboid = this; this->propulsion = new Propulsion(); @@ -66,18 +66,19 @@ void Roboid::Update(unsigned long currentTimeMs) { lastUpdateTimeMs = currentTimeMs; } -void Roboid::AddChild(Thing *child) { - Thing::AddChild(child); - if (child->IsSensor()) { - Sensor *childSensor = (Sensor *)child; - this->perception->AddSensor(childSensor); - } -} +// void Roboid::AddChild(Thing *child) { +// std::cout << "Roboid add child"; +// Thing::AddChild(child); +// if (child->IsSensor()) { +// Sensor *childSensor = (Sensor *)child; +// this->perception->AddSensor(childSensor); +// } +// } void Passer::RoboidControl::Roboid::Release(Thing *child) { if (RemoveChild(child) != nullptr) { - child->position = this->position; - child->orientation = this->orientation; + child->SetPosition(this->position); + child->SetOrientation(this->orientation); // this creates an new thing, I wish I could avoid this. this->perception->AddTrackedObject(nullptr, child); } diff --git a/Roboid.h b/Roboid.h index 7164170..06a9dcc 100644 --- a/Roboid.h +++ b/Roboid.h @@ -63,7 +63,7 @@ public: /// as these are local to the roboid' orientation. // virtual void SetOrientation(SwingTwist16 worldOrientation); - virtual void AddChild(Thing *child) override; + // virtual void AddChild(Thing *child) override; void Release(Thing *child); Thing *worldOrigin = diff --git a/Sensor.cpp b/Sensor.cpp index 0649cfb..2450b3b 100644 --- a/Sensor.cpp +++ b/Sensor.cpp @@ -2,18 +2,19 @@ #include "Roboid.h" -Sensor::Sensor() : Thing() { // for now, id should be set properly later - this->type = (unsigned char)Thing::SensorType; +Sensor::Sensor() : Thing() { + // this->type = (unsigned char)Type::Sensor; } -void Sensor::SetParent(Thing *parent) { - this->parent = parent; - if (this->parent->parent == nullptr) { // Is the parent a root object? - // Then it is a roboid - Roboid *roboidParent = (Roboid *)this->parent; - roboidParent->perception->AddSensor(this); - } -} +// void Sensor::SetParent(Thing *parent) { +// this->parent = parent; +// if (this->parent != nullptr && +// this->parent->GetParent() == nullptr) { // Is the parent a root object? +// // Then it is a roboid +// Roboid *roboidParent = (Roboid *)this->parent; +// roboidParent->perception->AddSensor(this); +// } +// } void Sensor::ConnectTo(Thing *oldThing) { this->name = oldThing->name; @@ -25,17 +26,17 @@ void Sensor::ConnectTo(Thing *oldThing) { oldParent->AddChild(this); for (int childIx = 0; childIx < oldThing->childCount; childIx++) { - Thing *child = oldThing->GetChild(childIx); + Thing *child = oldThing->GetChildByIndex(childIx); this->AddChild(child); } - this->position = oldThing->position; - this->orientation = oldThing->orientation; + this->position = oldThing->GetPosition(); + this->orientation = oldThing->GetOrientation(); // delete (thing); // can we do this? } void Sensor::ConnectTo(Thing *rootThing, const char *thingName) { - Thing *thing = rootThing->FindChild(thingName); + Thing *thing = rootThing->FindThing(thingName); if (thing != nullptr) this->ConnectTo(thing); } diff --git a/Sensor.h b/Sensor.h index c669038..3e06b2e 100644 --- a/Sensor.h +++ b/Sensor.h @@ -1,6 +1,6 @@ #pragma once -#include "Thing.h" +#include "ControlCore/Thing.h" namespace Passer { namespace RoboidControl { @@ -13,7 +13,7 @@ public: /// @brief Sets the parent Thing /// @param parent The Thing which should become the parent - virtual void SetParent(Thing *parent) override; + // virtual void SetParent(Thing *parent) override; virtual void PublishState() {}; diff --git a/ServoMotor.cpp b/ServoMotor.cpp index 6b0a9a5..59c56d6 100644 --- a/ServoMotor.cpp +++ b/ServoMotor.cpp @@ -3,7 +3,7 @@ #include "LinearAlgebra/FloatSingle.h" ServoMotor::ServoMotor() : Thing() { - this->type = Thing::ServoType; + this->type = (unsigned char)Type::Servo; this->controlMode = ControlMode::Position; this->targetAngle = Angle16(); this->hasTargetAngle = false; @@ -40,8 +40,8 @@ float ServoMotor::GetTargetVelocity() { return this->targetVelocity; } void ServoMotor::Update(unsigned long currentTimeMs) { for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing *child = this->GetChild(childIx); - if (child != nullptr && child->type == Thing::ServoType) { + Thing *child = this->GetChildByIndex(childIx); + if (child != nullptr && child->type == (unsigned char)Type::Servo) { ServoMotor *servo = (ServoMotor *)child; servo->Update(currentTimeMs); } diff --git a/Switch.cpp b/Switch.cpp index 8a6bd38..3180b6f 100644 --- a/Switch.cpp +++ b/Switch.cpp @@ -1,6 +1,6 @@ #include "Switch.h" -Switch::Switch() { this->type = Thing::SwitchType; } +Switch::Switch() { this->type = (unsigned char)Type::Switch; } bool Switch::IsOn() { return false; } diff --git a/TemperatureSensor.cpp b/TemperatureSensor.cpp index 009317a..569e978 100644 --- a/TemperatureSensor.cpp +++ b/TemperatureSensor.cpp @@ -1,15 +1,9 @@ #include "TemperatureSensor.h" -TemperatureSensor::TemperatureSensor() -{ - this->type = Thing::TemperatureSensorType; +TemperatureSensor::TemperatureSensor() { + this->type = (unsigned char)Type::TemperatureSensor; } -float TemperatureSensor::InCelsius() -{ - return this->temperature; -} +float TemperatureSensor::InCelsius() { return this->temperature; } -void* TemperatureSensor::GetValue() { - return &this->temperature; -} +void *TemperatureSensor::GetValue() { return &this->temperature; } diff --git a/Thing.cpp b/Thing.cpp deleted file mode 100644 index 92cc5fa..0000000 --- a/Thing.cpp +++ /dev/null @@ -1,166 +0,0 @@ -#include "Thing.h" - -#include - -using namespace Passer::RoboidControl; - -// int Thing::lastThingId = 1; - -Thing::Thing() : CoreThing() { - // this->id = lastThingId++; - // this->type = (unsigned int)Type::Undetermined; - this->childCount = 0; - this->parent = nullptr; - this->children = nullptr; -} - -const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch; -const unsigned int Thing::DistanceSensorType = - SensorType | (unsigned int)Type::DistanceSensor; -const unsigned int Thing::TemperatureSensorType = - SensorType | (unsigned int)Type::TemperatureSensor; - -const unsigned int Thing::ControlledMotorType = - MotorType | (unsigned int)Type::ControlledMotor; -const unsigned int Thing::UncontrolledMotorType = - MotorType | (unsigned int)Type::UncontrolledMotor; -const unsigned int Thing::ServoType = (unsigned int)Type::Servo; -const unsigned int Thing::HumanoidType = (unsigned int)Type::Humanoid; - -bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; } - -bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; } - -bool Thing::IsRoboid() { return (type & Thing::RoboidType) != 0; } - -void Thing::SetModel(const char *url) { this->modelUrl = url; } - -void Thing::SetParent(Thing *parent) { - if (parent == nullptr) - return; - - parent->AddChild(this); -} - -Thing *Thing::GetParent() { return (Thing *)this->parent; } - -void Thing::AddChild(Thing *child) { - Thing **newChildren = new Thing *[this->childCount + 1]; - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - newChildren[childIx] = this->children[childIx]; - if (this->children[childIx] == child) { - // child is already present, stop copying do not update the children - delete[] newChildren; - return; - } - } - - newChildren[this->childCount] = child; - child->parent = this; - - if (this->children != nullptr) - delete[] this->children; - - this->children = newChildren; - this->childCount++; -} - -Thing *Thing::RemoveChild(Thing *child) { - unsigned char newChildCount = this->childCount - 1; - Thing **newChildren = new Thing *[newChildCount]; - - unsigned char newChildIx = 0; - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - if (this->children[childIx] != child) { - if (newChildIx == newChildCount) { // We did not find the child - // stop copying and return nothing - delete[] newChildren; - return nullptr; - } else - newChildren[newChildIx++] = this->children[childIx]; - } - } - - child->parent = nullptr; - - delete[] this->children; - this->children = newChildren; - this->childCount = newChildCount; - - return child; -} - -Thing *Thing::GetChild(unsigned char childIx) { - if (childIx >= 0 && childIx < this->childCount) { - return this->children[childIx]; - } else - return nullptr; -} - -Thing **Passer::RoboidControl::Thing::GetChildren() { return this->children; } - -Thing *Thing::FindChild(const char *name, bool recursive) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing *child = this->children[childIx]; - if (child == nullptr) - continue; - if (strcmp(child->name, name) == 0) - return child; - - if (recursive) { - Thing *foundChild = child->FindChild(name); - if (foundChild != nullptr) - return foundChild; - } - } - return nullptr; -} - -Thing *Passer::RoboidControl::Thing::FindChild(unsigned char thingId, - bool recursive) { - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) { - Thing *child = this->children[childIx]; - if (child == nullptr) - continue; - if (child->id == thingId) - return child; - - if (recursive) { - Thing *foundChild = child->FindChild(thingId); - if (foundChild != nullptr) - return foundChild; - } - } - return nullptr; -} - -void Passer::RoboidControl::Thing::SetPosition(Spherical16 position) { - this->position = position; - this->positionUpdated = true; -} - -void Passer::RoboidControl::Thing::SetOrientation(SwingTwist16 orientation) { - this->orientation = orientation; - this->orientationUpdated = true; -} - -Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; } - -Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; } - -// Thing *Thing::Get(unsigned char networkId, unsigned char thingId) { -// CoreThing *coreThing = CoreThing::Get(networkId, thingId); -// // All CoreThings are Things in this context, so we can cast directly -// Thing *thing = (Thing *)coreThing; -// return thing; -// } - -// int Thing::Add(Thing *thing) { return CoreThing::Add(thing); } - -// void Thing::UpdateAll(unsigned long currentTimeMs) { -// for (unsigned char ix = 0; ix < allThingsSize; ix++) { -// Thing *thing = Get(0, ix); -// if (thing != nullptr) -// thing->Update(currentTimeMs); -// } -// } \ No newline at end of file diff --git a/Thing.h b/Thing.h deleted file mode 100644 index 5bf9b80..0000000 --- a/Thing.h +++ /dev/null @@ -1,121 +0,0 @@ -#pragma once - -#include "ControlCore/CoreThing.h" -#include "LinearAlgebra/AngleAxis.h" -#include "LinearAlgebra/Quaternion.h" -#include "LinearAlgebra/Spherical.h" -#include "LinearAlgebra/SwingTwist.h" -#include - -namespace Passer { -namespace RoboidControl { - -/// @brief A thing is a functional component on a robot -class Thing : public CoreThing { -public: - /// @brief Default constructor for a Thing - Thing(); - - // I hate this, better is to have an additional field stating the thing - // classificaton Motor, Sensor etc. - /// @brief The type of a switch sensor - static const unsigned int SwitchType; - /// @brief The type of a distance sensor - static const unsigned int DistanceSensorType; - static const unsigned int TemperatureSensorType; - /// @brief The type of a controlled motor - static const unsigned int ControlledMotorType; - /// @brief The type of an uncontrolled motor - static const unsigned int UncontrolledMotorType; - /// @brief The type of an object received from the network - static const unsigned int ServoType; - static const unsigned int HumanoidType; - - /// @brief Check if the Thing is a Motor - /// @return True when the Thing is a Motor and False otherwise - bool IsMotor(); - /// @brief Check if the Thing is a Sensor - /// @return True when the Thing is a Sensor and False otherwise - bool IsSensor(); - /// @brief Check if the Thing is a Roboid - /// @return True when the Thing is a Roboid and False otherwise - bool IsRoboid(); - - /// @brief The position in local space - /// @remark When this Thing has a parent, the position is relative to the - /// parent's position and orientation - Spherical16 position; - /// @brief The position in roboid space - /// @remark This is the position relative to the root of the roboid, - /// or the Roboid itself. - // Spherical16 roboidPosition; - /// @brief The orientation in local space - /// @remark When this Thing has a parent, the orientation is relative to the - /// parent's orientation - SwingTwist16 orientation; - /// @brief The orientation in roboid space - /// @remark This is the orientation relative to the root of the roboid, - /// or the Roboid itself. - // SwingTwist16 roboidOrientation; - - void SetPosition(Spherical16 position); - void SetOrientation(SwingTwist16 orientation); - - virtual Spherical16 GetLinearVelocity(); - virtual Spherical16 GetAngularVelocity(); - - /// @brief Sets the parent Thing - /// @param parent The Thing which should become the parnet - /// @remark This is equivalent to calling parent->AddChild(this); - virtual void SetParent(Thing *parent); - /// @brief Gets the parent Thing - /// @return The parent Thing - Thing *GetParent(); - - /// @brief Add a child Thing to this Thing - /// @param child The Thing which should become a child - /// @remark When the Thing is already a child, it will not be added again - virtual void AddChild(Thing *child); - Thing *RemoveChild(Thing *child); - /// @brief Get the child at the given index - /// @param childIx The index of the child - /// @return The child at the given index or nullptr when the index is invalid - /// or the child could not be found - Thing *GetChild(unsigned char childIx); - Thing **GetChildren(); - Thing *FindChild(const char *name, bool recursive = true); - Thing *FindChild(unsigned char thingId, bool recursive = true); - - /// @brief Sets the location from where the 3D model of this Thing can be - /// loaded from - /// @param url The url of the model - /// @remark Although the roboid implementation is not dependent on the model, - /// the only official supported model format is .obj - void SetModel(const char *url); - - unsigned char childCount = 0; - -protected: - /// @brief Bitmask for Motor type - static const unsigned int MotorType = 0x8000; - /// @brief Bitmap for Sensor type - static const unsigned int SensorType = 0x4000; - /// @brief Bitmap for Roboid type - static const unsigned int RoboidType = 0x2000; - static const unsigned char CustomType = 0x80; - - // Thing *parent = nullptr; - Thing **children = nullptr; - -public: - bool positionUpdated = false; - bool orientationUpdated = false; - - Spherical16 angularVelocity = Spherical16::zero; - Spherical16 linearVelocity = Spherical16::zero; -}; - -} // namespace RoboidControl -} // namespace Passer - -using namespace Passer::RoboidControl; \ No newline at end of file