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

Support simulated sensing

Browse Source
This commit is contained in:
Pascal Serrarens 2024-11-27 13:40:58 +01:00
parent 4ebe38eb39
commit ba009f79f7
15 changed files with 242 additions and 220 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
DifferentialDrive.cpp
View File

@ -69,43 +69,45 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
SetTwistSpeed(linear.Forward(), yaw);
}
void DifferentialDrive::SetVelocity(Polar velocity) {
SetTwistSpeed(velocity.distance, velocity.angle.InDegrees());
}
// void DifferentialDrive::SetVelocity(Polar velocity) {
// SetTwistSpeed(velocity.distance, velocity.angle.InDegrees());
// }
Spherical16 DifferentialDrive::GetVelocity() {
Motor *leftMotor = motors[0];
Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
// Spherical16 DifferentialDrive::GetVelocity() {
// Motor *leftMotor = motors[0];
// Motor *rightMotor = motors[1];
// float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per
// second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions
// per second
leftSpeed = leftSpeed * rpsToMs; // in meters per second
rightSpeed = rightSpeed * rpsToMs; // in meters per second
float speed = (leftSpeed + rightSpeed) / 2;
// leftSpeed = leftSpeed * rpsToMs; // in meters per second
// rightSpeed = rightSpeed * rpsToMs; // in meters per second
// float speed = (leftSpeed + rightSpeed) / 2;
float direction = speed >= 0 ? 0.0F : 180.0F;
float magnitude = fabsf(speed);
Spherical16 velocity =
Spherical16(magnitude, Angle16::Degrees(direction),
Angle16::Degrees(0)); // Polar(direction, magnitude);
return velocity;
}
// float direction = speed >= 0 ? 0.0F : 180.0F;
// float magnitude = fabsf(speed);
// Spherical16 velocity =
// Spherical16(magnitude, Angle16::Degrees(direction),
// Angle16::Degrees(0)); // Polar(direction, magnitude);
// return velocity;
// }
float DifferentialDrive::GetAngularVelocity() {
Motor *leftMotor = motors[0];
Motor *rightMotor = motors[1];
float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per second
float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
// float DifferentialDrive::GetAngularVelocity() {
// Motor *leftMotor = motors[0];
// Motor *rightMotor = motors[1];
// float leftSpeed = leftMotor->GetActualSpeed(); // in revolutions per
// second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions
// per second
leftSpeed = leftSpeed * rpsToMs; // in meters per second
rightSpeed = rightSpeed * rpsToMs; // in meters per second
// leftSpeed = leftSpeed * rpsToMs; // in meters per second
// rightSpeed = rightSpeed * rpsToMs; // in meters per second
float angularSpeed = (leftSpeed - rightSpeed) / 2;
float angularDistance = wheelSeparation / 2 * Passer::LinearAlgebra::pi;
float rotationsPerSecond = angularSpeed / angularDistance;
float degreesPerSecond =
Angle::Normalize(Angle::Degrees(360 * rotationsPerSecond)).InDegrees();
float angularVelocity = degreesPerSecond;
// float angularSpeed = (leftSpeed - rightSpeed) / 2;
// float angularDistance = wheelSeparation / 2 * Passer::LinearAlgebra::pi;
// float rotationsPerSecond = angularSpeed / angularDistance;
// float degreesPerSecond =
// Angle::Normalize(Angle::Degrees(360 * rotationsPerSecond)).InDegrees();
// float angularVelocity = degreesPerSecond;
return angularVelocity;
}
// return angularVelocity;
// }

6
DifferentialDrive.h
View File

@ -59,7 +59,7 @@ public:
virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
float pitch = 0.0F, float roll = 0.0F);
virtual void SetVelocity(Polar velocity);
// virtual void SetVelocity(Polar velocity);
/// @brief Calculate the linear velocity of the roboid based on the wheel
/// velocities
@ -68,7 +68,7 @@ public:
/// information
/// @remark This will be more expanded/detailed in a future version of Roboid
/// Control
virtual Spherical16 GetVelocity() override;
// virtual Spherical16 GetVelocity() override;
/// @brief Calculate the angular velocity of the roboid based on the wheel
/// velocities
/// @return The angular speed of the roboid in local space
@ -76,7 +76,7 @@ public:
/// information
/// @remark This will be more expanded/detailed in a future version of Roboid
/// Control
virtual float GetAngularVelocity() override;
// virtual float GetAngularVelocity() override;
protected:
float wheelDiameter = 1.0F; // in meters

6
DistanceSensor.cpp
View File

@ -1,5 +1,6 @@
#include "DistanceSensor.h"
#include "NetworkPerception.h"
#include <math.h>
DistanceSensor::DistanceSensor() {
@ -26,3 +27,8 @@ bool DistanceSensor::ObjectNearby() {
bool isOn = distance <= triggerDistance;
return isOn;
}
void DistanceSensor::ProcessBytes(unsigned char *bytes) {
unsigned char ix = 0;
this->distance = NetworkPerception::ReceiveFloat16(bytes, &ix);
}

2
DistanceSensor.h
View File

@ -31,6 +31,8 @@ public:
/// @return True when an object is nearby
bool ObjectNearby();
virtual void ProcessBytes(unsigned char *bytes) override;
protected:
/// @brief Distance to the closest object
float distance = 0;

15
NetworkPerception.cpp
View File

@ -1,6 +1,7 @@
#include "NetworkPerception.h"
#include "NetworkSync.h"
#include "float16/float16.h"
#define RC_DEBUG true
#if RC_DEBUG
@ -69,7 +70,8 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
if (thing == nullptr)
return;
roboid->networkSync->NewObject(thing);
// roboid->networkSync->NewObject(thing);
roboid->networkSync->SendThing(thing);
roboid->networkSync->SendModel(thing);
}
}
@ -327,3 +329,14 @@ Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) {
Vector3 v = Vector3(x, y, z);
return v;
}
float NetworkPerception::ReceiveFloat16(unsigned char *data,
unsigned char *startIndex) {
byte ix = *startIndex;
unsigned short value = data[ix++] << 8 | data[ix];
float16 f = float16();
f.setBinary(value);
*startIndex = ix;
return f.toDouble();
}

3
NetworkPerception.h
View File

@ -23,6 +23,9 @@ protected:
Int32 ReceiveInt32(unsigned char *data, int startIndex);
float ReceiveFloat100(unsigned char *data, int startIndex);
Vector3 ReceiveVector3(unsigned char *data, int startIndex);
public:
static float ReceiveFloat16(unsigned char *data, unsigned char *startIndex);
};
} // namespace RoboidControl

201
NetworkSync.cpp
View File

@ -30,6 +30,9 @@ void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
case NetworkIdMsg:
ReceiveNetworkId();
break;
case BytesMsg:
ReceiveBytes();
break;
}
}
@ -39,20 +42,32 @@ void NetworkSync::ReceiveNetworkId() {
#ifdef RC_DEBUG
SERIALPORT.printf("Received network Id %d\n", this->networkId);
#endif
SendThing(roboid);
SendName(roboid);
SendModel(roboid);
for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) {
Thing *child = roboid->GetChild(childIx);
if (child != nullptr)
PublishRelativeThing(child, true);
}
SendThingInfo(roboid, true);
}
void NetworkSync::PublishDevice() {
void NetworkSync::ReceiveBytes() {
unsigned char ix = 1; // first byte is the msgId
unsigned char networkId = buffer[ix++];
unsigned char thingId = buffer[ix++];
unsigned char len = buffer[ix++];
unsigned char *bytes = new unsigned char[len];
// printf("Received %d bytes for [%d/%d]\n", len, networkId, thingId);
for (unsigned char bytesIx = 0; bytesIx < len; bytesIx++)
bytes[bytesIx] = buffer[ix++];
// networkId is not used right now, we assume networkId == 0
Thing *thing = roboid->FindChild(thingId);
// printf("Found thing %d\n", thing);
if (thing != nullptr)
thing->ProcessBytes(bytes);
delete bytes;
}
void NetworkSync::PublishClient() {
unsigned char ix = 0;
buffer[ix++] = DeviceMsg;
buffer[ix++] = 0; // No network ID
buffer[ix++] = this->networkId;
PublishBuffer(ix);
#ifdef RC_DEBUG
@ -65,6 +80,20 @@ void NetworkSync::PublishState(Roboid *roboid) {
PublishPerception(roboid);
}
void NetworkSync::SendThingInfo(Thing *thing, bool recurse) {
SendThing(thing);
SendName(thing);
SendModel(thing);
if (recurse) {
for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
Thing *child = thing->GetChild(childIx);
if (child != nullptr)
SendThingInfo(child, true);
}
}
}
void NetworkSync::SendThing(Thing *thing) {
if (thing == nullptr)
return;
@ -73,57 +102,16 @@ void NetworkSync::SendThing(Thing *thing) {
buffer[ix++] = CreateMsg;
buffer[ix++] = thing->id;
buffer[ix++] = thing->type;
SendBuffer(ix);
#ifdef RC_DEBUG
printf("Sent Thing [%d/%d] %d\n", networkId, buffer[1], buffer[2]);
#endif
}
void NetworkSync::NewObject(InterestingThing *thing) {
if (thing == nullptr || thing->networkId != 0x00)
return;
unsigned char ix = 0;
buffer[ix++] = CreateMsg;
buffer[ix++] = thing->id;
buffer[ix++] = thing->type;
SendBuffer(ix);
#ifdef RC_DEBUG
printf("Sent CreateMsg [%d/%d] %d\n", networkId, buffer[1], buffer[2]);
#endif
thing->updated = true;
PublishTrackedObject(roboid, thing);
}
void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) {
Thing *parentThing = thing->GetParent();
unsigned char ix = 0;
buffer[ix++] = RelativePoseMsg;
buffer[ix++] = thing->id;
if (parentThing != nullptr)
buffer[ix++] = parentThing->id;
else
buffer[ix++] = 0x00;
SendSpherical16(
buffer, &ix,
thing->position); // Do we need this if we already send a pose?
SendBuffer(ix);
SendName(thing);
SendModel(thing);
if (recurse) {
for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
Thing *child = thing->GetChild(childIx);
if (child != nullptr)
PublishRelativeThing(child, true);
}
}
#ifdef RC_DEBUG
printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, thing->type);
#endif
}
void NetworkSync::SendName(Thing *thing) {
@ -150,28 +138,6 @@ void NetworkSync::SendName(Thing *thing) {
#endif
}
void NetworkSync::SendModel(Roboid *roboid) {
if (roboid->modelUrl == nullptr)
return;
unsigned char len = strlen(roboid->modelUrl);
if (len > 255)
return;
unsigned char ix = 0;
buffer[ix++] = ModelMsg;
buffer[ix++] = 0x00; // objectId
Spherical16 s = Spherical16::zero; //(roboid->modelPosition);
SendSpherical16(buffer, &ix, s);
SendFloat16(buffer, &ix, roboid->modelScale);
buffer[ix++] = len;
for (int urlIx = 0; urlIx < len; urlIx++)
buffer[ix++] = roboid->modelUrl[urlIx];
SendBuffer(ix);
}
void NetworkSync::SendModel(Thing *thing) {
if (thing->modelUrl == nullptr)
return;
@ -185,7 +151,7 @@ void NetworkSync::SendModel(Thing *thing) {
buffer[ix++] = thing->id; // objectId
Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
SendSpherical16(buffer, &ix, s);
SendFloat16(buffer, &ix, 1); // thing->modelScale);
SendFloat16(buffer, &ix, thing->modelScale);
buffer[ix++] = len;
for (int urlIx = 0; urlIx < len; urlIx++)
@ -208,26 +174,41 @@ void NetworkSync::SendDestroyThing(InterestingThing *thing) {
#endif
}
#include <Arduino.h>
void NetworkSync::SendPose(Thing *thing, bool recurse) {
if (this->networkId == 0) // We're not connected to a site yet
return;
// if (thing->GetLinearVelocity().distance > 0 ||
// thing->GetAngularVelocity().distance > 0) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg;
buffer[ix++] = thing->id;
buffer[ix++] = Pose_Position | Pose_Orientation;
SendSpherical16(buffer, &ix, thing->position);
SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
SendBuffer(ix);
thing->positionUpdated |= thing->GetLinearVelocity().distance > 0;
thing->orientationUpdated |= thing->GetAngularVelocity().distance > 0;
if (thing->positionUpdated || thing->orientationUpdated) {
unsigned char ix = 0;
buffer[ix++] = PoseMsg;
buffer[ix++] = thing->id;
unsigned char pattern = 0;
if (thing->positionUpdated)
pattern |= Pose_Position;
if (thing->orientationUpdated)
pattern |= Pose_Orientation;
buffer[ix++] = pattern;
SendSpherical16(buffer, &ix, thing->position);
SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
SendBuffer(ix);
#if RC_DEBUG
// if (thing->id == 0)
SERIALPORT.printf("Sent PoseMsg Thing [%d/%d] %f\n", this->networkId,
buffer[1], thing->position.distance);
#endif
// }
thing->positionUpdated = false;
thing->orientationUpdated = false;
// #if RC_DEBUG
if (thing->id == 0) {
Vector3 v = thing->position.ToVector3();
printf("Sent PoseMsg Thing [%d/%d] %f(%f)-(%f %f %f) %f\n",
this->networkId, thing->id, thing->position.distance,
thing->position.direction.horizontal.InDegrees(), v.Right(),
v.Up(), v.Forward(),
thing->orientation.swing.horizontal.InDegrees());
}
// #endif
}
if (recurse) {
for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
@ -333,42 +314,6 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
object->updated = false;
}
void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
Spherical16 velocity = roboid->propulsion->GetVelocity();
Spherical16 worldVelocity = roboid->orientation * velocity;
Vector3 worldVelocity3 = worldVelocity.ToVector3();
float angularVelocity = roboid->propulsion->GetAngularVelocity();
Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);
#ifdef RC_DEBUG
Serial.print("Send Pose 0 LinearVelocity ");
Serial.print(worldVelocity3.Right());
Serial.print(", ");
Serial.print(worldVelocity3.Up());
Serial.print(", ");
Serial.print(worldVelocity3.Forward());
Serial.print(" AngularVelocity ");
Serial.print(worldAngularVelocity.Right());
Serial.print(", ");
Serial.print(worldAngularVelocity.Up());
Serial.print(", ");
Serial.println(worldAngularVelocity.Forward());
#else
const unsigned int bufferSize = 3 + 12 + 12;
unsigned char buffer[bufferSize] = {
PoseMsg,
0, // objectId;
Pose_LinearVelocity | Pose_AngularVelocity,
};
unsigned char ix = 3;
SendVector3(buffer, &ix, worldVelocity3);
SendVector3(buffer, &ix, worldAngularVelocity);
sendBuffer(buffer, bufferSize);
#endif
}
void NetworkSync::SendInvestigate(InterestingThing *thing) {
unsigned char ix = 0;
buffer[ix++] = InvestigateMsg;

17
NetworkSync.h
View File

@ -22,10 +22,10 @@ public:
/// @brief Inform that the given object is no longer being tracked
/// @param obj
virtual void SendDestroyThing(InterestingThing *obj);
virtual void NewObject(InterestingThing *obj);
void SendThingInfo(Thing *thing, bool recurse = false);
void SendThing(Thing *thing);
void SendName(Thing *roboid);
virtual void SendModel(Roboid *obj);
void SendModel(Thing *thing);
/// @brief The id of a Pose message
@ -57,22 +57,16 @@ public:
static const unsigned char NameMsg = 0x91;
static const unsigned char DeviceMsg = 0xA0;
static const unsigned char NetworkIdMsg = 0xA1;
static const unsigned char BytesMsg = 0xB1;
typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
void ReceiveMessage(Roboid *roboid, unsigned char bytecount);
void ReceiveNetworkId();
void PublishState(Roboid *roboid);
void SendInvestigate(InterestingThing *thing);
void SendPoseMsg(Buffer sendBuffer, Roboid *roboid);
// void SendDestroyThing(Buffer sendBuffer, InterestingThing* obj);
// void PublishNewObject();
void PublishRelativeThing(Thing *thing, bool recurse = false);
void PublishPerception(Roboid *roboid);
void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects);
@ -91,6 +85,9 @@ protected:
Roboid *roboid;
NetworkPerception *networkPerception;
void ReceiveNetworkId();
void ReceiveBytes();
void PublishState(Sensor *sensor);
void PublishTrackedObject(Roboid *roboid, InterestingThing *object);
@ -125,7 +122,7 @@ protected:
virtual void SendBuffer(unsigned char bufferSize);
virtual void PublishBuffer(unsigned char bufferSize);
void PublishDevice();
void PublishClient();
};
} // namespace RoboidControl

6
Propulsion.cpp
View File

@ -39,6 +39,10 @@ void Propulsion::SetVelocity(Spherical16 velocity) {
this->linearVelocity = velocity;
}
void Propulsion::SetAngularVelocity(Spherical16 velocity) {
this->angularVelocity = velocity;
}
Spherical16 Propulsion::GetVelocity() { return this->linearVelocity; }
float Propulsion::GetAngularVelocity() { return 0; }
Spherical16 Propulsion::GetAngularVelocity() { return this->angularVelocity; }

4
Propulsion.h
View File

@ -60,6 +60,7 @@ public:
float pitch = 0.0F, float roll = 0.0F);
virtual void SetVelocity(Spherical16 velocity);
virtual void SetAngularVelocity(Spherical16 velocity);
/// @brief Retrieve the current velocity of the roboid
/// @return The velocity in polar coordinates
@ -68,7 +69,7 @@ public:
/// @brief Retrieve the current angular velocity of the roboid
/// @return The angular velocity
/// The actual unit of the angular velocity depend on the implementation
virtual float GetAngularVelocity();
virtual Spherical16 GetAngularVelocity();
/// @brief The roboid of this propulsion system
Roboid *roboid = nullptr;
@ -81,6 +82,7 @@ protected:
Motor **motors = nullptr;
Spherical16 linearVelocity = Spherical16::zero;
Spherical16 angularVelocity = Spherical16::zero;
};
} // namespace RoboidControl

34
Roboid.cpp
View File

@ -30,6 +30,7 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() {
propulsion->roboid = this;
}
#include <Arduino.h>
void Roboid::Update(unsigned long currentTimeMs) {
if (perception != nullptr)
perception->Update(currentTimeMs);
@ -39,33 +40,20 @@ void Roboid::Update(unsigned long currentTimeMs) {
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
// Conversion from old units
// Polar polarVelocity = this->propulsion->GetVelocity();
// this->linearVelocity = Spherical16(
// polarVelocity.distance,
// Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16());
this->linearVelocity = this->propulsion->GetVelocity();
float oldAngular = this->propulsion->GetAngularVelocity();
this->angularVelocity =
Spherical16(oldAngular, Angle16(), Angle16::Degrees(90));
this->angularVelocity = this->propulsion->GetAngularVelocity();
// SetPosition(this->position + this->orientation * Spherical16::forward *
// this->linearVelocity.distance *
// deltaTime);
// this->roboidPosition = this->position; // assuming the roboid is the
// root
this->position += this->orientation * Spherical16::forward *
this->linearVelocity.distance * deltaTime;
Spherical16 translation =
this->orientation * this->linearVelocity * deltaTime;
SetPosition(this->position + translation);
SwingTwist16 rotation = SwingTwist16::AngleAxis(
this->angularVelocity.distance * deltaTime, Direction16::up);
SwingTwist16 rotation =
SwingTwist16::AngleAxis(this->angularVelocity.distance * deltaTime,
this->angularVelocity.direction);
if (perception != nullptr)
perception->UpdatePose(rotation);
this->orientation = this->orientation * rotation;
this->roboidOrientation =
this->orientation; // assuming the roboid is the root
SetOrientation(this->orientation * rotation);
}
if (childCount > 0 && children != nullptr) {
@ -79,10 +67,6 @@ void Roboid::Update(unsigned long currentTimeMs) {
lastUpdateTimeMs = currentTimeMs;
}
// Spherical16 Roboid::GetPosition() { return this->roboidPosition; }
// SwingTwist16 Roboid::GetOrientation() { return this->roboidOrientation; }
/*
void Roboid::SetPosition(Spherical16 newWorldPosition) {
SwingTwist16 roboidOrientation = this->GetOrientation();

20
Sensor.cpp
View File

@ -12,4 +12,22 @@ void Sensor::SetParent(Thing *parent) {
Roboid *roboidParent = (Roboid *)this->parent;
roboidParent->perception->AddSensor(this);
}
}
}
void Sensor::ConnectTo(Thing *thing) {
this->name = thing->name;
this->id = thing->id;
Thing *thingParent = thing->GetParent();
thingParent->RemoveChild(thing);
thingParent->AddChild(this);
this->children = thing->GetChildren();
this->position = thing->position;
this->orientation = thing->orientation;
// delete (thing); // can we do this?
}
void Sensor::ConnectTo(Thing *rootThing, const char *thingName) {
Thing *thing = rootThing->FindChild(thingName);
this->ConnectTo(thing);
}

7
Sensor.h
View File

@ -10,14 +10,17 @@ class Sensor : public Thing {
public:
/// @brief Default Constructor for a Sensor
Sensor();
/// @brief Sets the parent Thing
/// @param parent The Thing which should become the parent
virtual void SetParent(Thing *parent) override;
virtual void PublishState() {};
virtual void* GetValue() { return nullptr; };
virtual void *GetValue() { return nullptr; };
void ConnectTo(Thing *thing);
void ConnectTo(Thing *rootThing, const char *thingName);
};
} // namespace RoboidControl

54
Thing.cpp
View File

@ -65,13 +65,6 @@ void Thing::AddChild(Thing *child) {
this->childCount++;
}
Thing *Thing::GetChild(unsigned char childIx) {
if (childIx >= 0 && childIx < this->childCount) {
return this->children[childIx];
} else
return nullptr;
}
Thing *Passer::RoboidControl::Thing::RemoveChild(Thing *child) {
Thing **newChildren = new Thing *[this->childCount - 1];
unsigned char newChildIx = 0;
@ -95,7 +88,16 @@ Thing *Passer::RoboidControl::Thing::RemoveChild(Thing *child) {
return child;
}
Thing *Thing::FindChild(const char *name) {
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)
@ -103,13 +105,43 @@ Thing *Thing::FindChild(const char *name) {
if (strcmp(child->name, name) == 0)
return child;
Thing *foundChild = child->FindChild(name);
if (foundChild != nullptr)
return foundChild;
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; }

19
Thing.h
View File

@ -54,7 +54,7 @@ public:
/// @brief The position in roboid space
/// @remark This is the position relative to the root of the roboid,
/// or the Roboid itself.
Spherical16 roboidPosition;
// Spherical16 roboidPosition;
/// @brief The orientation in local space
/// @remark When this Thing has a parent, the orientation is relative to the
/// parent's orientation
@ -62,7 +62,10 @@ public:
/// @brief The orientation in roboid space
/// @remark This is the orientation relative to the root of the roboid,
/// or the Roboid itself.
SwingTwist16 roboidOrientation;
// SwingTwist16 roboidOrientation;
void SetPosition(Spherical16 position);
void SetOrientation(SwingTwist16 orientation);
virtual Spherical16 GetLinearVelocity();
virtual Spherical16 GetAngularVelocity();
@ -79,13 +82,15 @@ public:
/// @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 *RemoveChild(Thing *child);
Thing *FindChild(const char *name);
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
@ -103,6 +108,8 @@ public:
const char *modelUrl = nullptr;
float modelScale = 1;
virtual void ProcessBytes(unsigned char *bytes) {}
protected:
/// @brief Bitmask for Motor type
static const unsigned int MotorType = 0x8000;
@ -131,10 +138,14 @@ protected:
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;