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

Thing -> ControlCore

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-18 16:53:54 +01:00
parent 9fb690a71b
commit 701316d687
25 changed files with 485 additions and 547 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
ControlCore/CoreThing.cpp
View File

@ -1,64 +0,0 @@
#include "CoreThing.h"
#include "Participant.h"
#include <iostream>
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);
}
}
}

75
ControlCore/CoreThing.h
View File

@ -1,75 +0,0 @@
#pragma once
#include <iostream>
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;

24
ControlCore/Messages.cpp
View File

@ -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;
}

13
ControlCore/Messages.h
View File

@ -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;
};

195
ControlCore/Thing.cpp Normal file
View File

@ -0,0 +1,195 @@
#include "Thing.h"
#include "Participant.h"
#include <iostream>
#include <string.h>
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);
}
}
}

133
ControlCore/Thing.h Normal file
View File

@ -0,0 +1,133 @@
#pragma once
#include "../LinearAlgebra/Spherical.h"
#include "../LinearAlgebra/SwingTwist.h"
#include <iostream>
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;

4
ControlledMotor.cpp
View File

@ -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() {

20
DifferentialDrive.cpp
View File

@ -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)

2
DirectionalSensor.cpp
View File

@ -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;
}

3
DirectionalSensor.h
View File

@ -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

6
DistanceSensor.cpp
View File

@ -1,11 +1,10 @@
#include "DistanceSensor.h"
// #include "NetworkPerception.h"
#include "ControlCore/LowLevelMessages.h"
#include <math.h>
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";
}

4
Motor.cpp
View File

@ -2,7 +2,9 @@
#include <time.h>
Motor::Motor() : Thing() { this->type = (int)Thing::UncontrolledMotorType; }
Motor::Motor() : Thing() {
this->type = (unsigned char)Type::UncontrolledMotor;
}
float Motor::GetActualSpeed() { return this->currentTargetSpeed; }

3
Motor.h
View File

@ -1,10 +1,11 @@
#pragma once
#include "Thing.h"
#include "ControlCore/Thing.h"
#include <time.h>
namespace Passer {
using namespace Control;
namespace RoboidControl {
/// @brief A Motor is a Thing which can move parts of the Roboid

30
NetworkSync.cpp
View File

@ -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 <iostream>
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,

89
Perception.cpp
View File

@ -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,

2
Quadcopter.h
View File

@ -1,8 +1,8 @@
#pragma once
#include "ControlCore/Thing.h"
#include "LinearAlgebra/Vector3.h"
#include "Propulsion.h"
#include "Thing.h"
namespace Passer {
namespace RoboidControl {

21
Roboid.cpp
View File

@ -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);
}

2
Roboid.h
View File

@ -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 =

29
Sensor.cpp
View File

@ -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);
}

4
Sensor.h
View File

@ -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() {};

6
ServoMotor.cpp
View File

@ -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);
}

2
Switch.cpp
View File

@ -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; }

14
TemperatureSensor.cpp
View File

@ -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; }

166
Thing.cpp
View File

@ -1,166 +0,0 @@
#include "Thing.h"
#include <string.h>
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);
// }
// }

121
Thing.h
View File

@ -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 <iostream>
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;