From 0e2f628e3ea15ed8289431bb2760ad582ed15481 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 21 Oct 2024 17:54:27 +0200 Subject: [PATCH] Improve use of position/orientation --- DifferentialDrive.cpp | 44 +++++++-------- NetworkSync.cpp | 128 ++++++++++++++++++++---------------------- Roboid.cpp | 104 ++++++++++++++-------------------- Roboid.h | 28 ++++----- Thing.h | 27 ++++----- 5 files changed, 148 insertions(+), 183 deletions(-) diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 03e7bfd..344f6d6 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -6,9 +6,9 @@ DifferentialDrive::DifferentialDrive() {}; -DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) { +DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) { this->motorCount = 2; - this->motors = new Motor*[2]; + this->motors = new Motor *[2]; this->motors[0] = leftMotor; this->motors[1] = rightMotor; @@ -35,7 +35,7 @@ void DifferentialDrive::SetDimensions(float wheelDiameter, void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, float rightSpeed) { for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) { - Motor* motor = motors[motorIx]; + Motor *motor = motors[motorIx]; if (motor == nullptr) continue; @@ -50,12 +50,12 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed, void DifferentialDrive::SetTwistSpeed(float forward, float yaw) { float leftSpeed = - Float::Clamp(forward + yaw, -1, 1); // revolutions per second + Float::Clamp(forward + yaw, -1, 1); // revolutions per second float rightSpeed = - Float::Clamp(forward - yaw, -1, 1); // revolutions per second + Float::Clamp(forward - yaw, -1, 1); // revolutions per second - float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second - float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second + float leftMotorSpeed = leftSpeed / rpsToMs; // meters per second + float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed); } @@ -64,9 +64,7 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) { SetTwistSpeed(linear.y, yaw); } -void DifferentialDrive::SetTwistSpeed(Vector3 linear, - float yaw, - float pitch, +void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch, float roll) { SetTwistSpeed(linear.Forward(), yaw); } @@ -76,30 +74,30 @@ void DifferentialDrive::SetVelocity(Polar velocity) { } Polar 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 + 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 speed = (leftSpeed + rightSpeed) / 2; float direction = speed >= 0 ? 0.0F : 180.0F; float magnitude = fabsf(speed); Polar velocity = Polar( - magnitude, Angle::Degrees(direction)); // Polar(direction, magnitude); + magnitude, Angle::Degrees(direction)); // 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 + 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; diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 00be150..4222f83 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -5,7 +5,7 @@ #ifdef RC_DEBUG #include #if ESP32 -#define SERIALPORT Serial0 +#define SERIALPORT Serial #else #define SERIALPORT Serial #endif @@ -18,18 +18,18 @@ #include -NetworkSync::NetworkSync(Roboid* roboid) { +NetworkSync::NetworkSync(Roboid *roboid) { this->roboid = roboid; this->networkId = 0; } -void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) { +void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) { networkPerception->ProcessPacket(roboid, buffer, bytecount); switch (buffer[0]) { - case NetworkIdMsg: - ReceiveNetworkId(); - break; + case NetworkIdMsg: + ReceiveNetworkId(); + break; } } @@ -42,7 +42,7 @@ void NetworkSync::ReceiveNetworkId() { SendName(roboid); SendModel(roboid); for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) { - Thing* child = roboid->GetChild(childIx); + Thing *child = roboid->GetChild(childIx); if (child != nullptr) PublishRelativeThing(child, true); } @@ -51,7 +51,7 @@ void NetworkSync::ReceiveNetworkId() { void NetworkSync::PublishDevice() { unsigned char ix = 0; buffer[ix++] = DeviceMsg; - buffer[ix++] = 0; // No network ID + buffer[ix++] = 0; // No network ID PublishBuffer(ix); #ifdef RC_DEBUG @@ -59,12 +59,12 @@ void NetworkSync::PublishDevice() { #endif } -void NetworkSync::PublishState(Roboid* roboid) { +void NetworkSync::PublishState(Roboid *roboid) { SendPose(roboid); PublishPerception(roboid); } -void NetworkSync::NewObject(InterestingThing* thing) { +void NetworkSync::NewObject(InterestingThing *thing) { if (thing == nullptr || thing->networkId != 0x00) return; @@ -82,8 +82,8 @@ void NetworkSync::NewObject(InterestingThing* thing) { PublishTrackedObject(roboid, thing); } -void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) { - Thing* parentThing = thing->GetParent(); +void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) { + Thing *parentThing = thing->GetParent(); unsigned char ix = 0; buffer[ix++] = RelativePoseMsg; @@ -99,14 +99,14 @@ void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) { if (recurse) { for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) { - Thing* child = thing->GetChild(childIx); + Thing *child = thing->GetChild(childIx); if (child != nullptr) PublishRelativeThing(child, true); } } } -void NetworkSync::SendName(Roboid* roboid) { +void NetworkSync::SendName(Roboid *roboid) { if (roboid->name == nullptr) return; @@ -116,7 +116,7 @@ void NetworkSync::SendName(Roboid* roboid) { unsigned char ix = 0; buffer[ix++] = NameMsg; - buffer[ix++] = 0x00; // objectId + buffer[ix++] = 0x00; // objectId buffer[ix++] = len; for (unsigned char nameIx = 0; nameIx < len; nameIx++) @@ -130,7 +130,7 @@ void NetworkSync::SendName(Roboid* roboid) { #endif } -void NetworkSync::SendModel(Roboid* roboid) { +void NetworkSync::SendModel(Roboid *roboid) { if (roboid->modelUrl == nullptr) return; @@ -140,10 +140,10 @@ void NetworkSync::SendModel(Roboid* roboid) { unsigned char ix = 0; buffer[ix++] = ModelMsg; - buffer[ix++] = 0x00; // objectId - Spherical16 s = Spherical16::zero; //(roboid->modelPosition); + buffer[ix++] = 0x00; // objectId + Spherical16 s = Spherical16::zero; //(roboid->modelPosition); SendSpherical16(buffer, &ix, s); - SendFloat16(buffer, &ix, 1); // roboid->modelScale); + SendFloat16(buffer, &ix, 1); // roboid->modelScale); buffer[ix++] = len; for (int urlIx = 0; urlIx < len; urlIx++) @@ -152,7 +152,7 @@ void NetworkSync::SendModel(Roboid* roboid) { SendBuffer(ix); } -void NetworkSync::SendModel(Thing* thing) { +void NetworkSync::SendModel(Thing *thing) { if (thing->modelUrl == nullptr) return; @@ -162,10 +162,10 @@ void NetworkSync::SendModel(Thing* thing) { unsigned char ix = 0; buffer[ix++] = ModelMsg; - buffer[ix++] = thing->id; // objectId - Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition); + 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, 1); // thing->modelScale); buffer[ix++] = len; for (int urlIx = 0; urlIx < len; urlIx++) @@ -174,8 +174,8 @@ void NetworkSync::SendModel(Thing* thing) { SendBuffer(ix); } -void NetworkSync::SendDestroyThing(InterestingThing* thing) { - if (networkId == 0) // We're not connected to a site yet +void NetworkSync::SendDestroyThing(InterestingThing *thing) { + if (networkId == 0) // We're not connected to a site yet return; unsigned char ix = 0; @@ -188,8 +188,8 @@ void NetworkSync::SendDestroyThing(InterestingThing* thing) { #endif } -void NetworkSync::SendPose(Thing* thing, bool recurse) { - if (networkId == 0) // We're not connected to a site yet +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 || @@ -204,21 +204,22 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) { #if RC_DEBUG if (thing->id == 0) - SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]); + SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", this->networkId, + buffer[1]); #endif } if (recurse) { for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) { - Thing* child = thing->GetChild(childIx); + Thing *child = thing->GetChild(childIx); if (child != nullptr) SendPose(thing->GetChild(childIx), true); } } } -void NetworkSync::PublishState(Sensor* sensor) { - float* value = (float*)sensor->GetValue(); +void NetworkSync::PublishState(Sensor *sensor) { + float *value = (float *)sensor->GetValue(); if (value == nullptr) return; @@ -229,14 +230,14 @@ void NetworkSync::PublishState(Sensor* sensor) { PublishBuffer(ix); } -void NetworkSync::PublishPerception(Roboid* roboid) { - Perception* perception = roboid->perception; +void NetworkSync::PublishPerception(Roboid *roboid) { + Perception *perception = roboid->perception; if (perception == nullptr) return; for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount; sensorIx++) { - Sensor* sensor = perception->sensors[sensorIx]; + Sensor *sensor = perception->sensors[sensorIx]; if (sensor == nullptr) continue; PublishState(sensor); @@ -244,14 +245,14 @@ void NetworkSync::PublishPerception(Roboid* roboid) { PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects()); } -void NetworkSync::PublishTrackedObjects(Roboid* roboid, - InterestingThing** objects) { - if (networkId == 0) // We're not connected to a site yet +void NetworkSync::PublishTrackedObjects(Roboid *roboid, + InterestingThing **objects) { + if (networkId == 0) // We're not connected to a site yet return; int n = 0; for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) { - InterestingThing* obj = objects[objIx]; + InterestingThing *obj = objects[objIx]; if (obj == nullptr) continue; @@ -262,8 +263,8 @@ void NetworkSync::PublishTrackedObjects(Roboid* roboid, } } -void NetworkSync::PublishTrackedObject(Roboid* roboid, - InterestingThing* object) { +void NetworkSync::PublishTrackedObject(Roboid *roboid, + InterestingThing *object) { if (object == nullptr || object->updated == false || object->networkId != 0x00) { return; @@ -277,8 +278,8 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid, SwingTwist16 worldOrientation = roboidOrientation * object->orientation; unsigned char ix = 0; - buffer[ix++] = PoseMsg; // Position2DMsg; - buffer[ix++] = object->id; // objectId; + buffer[ix++] = PoseMsg; // Position2DMsg; + buffer[ix++] = object->id; // objectId; buffer[ix++] = Pose_Position | Pose_Orientation; SendSpherical16(buffer, &ix, worldPosition); SendSwingTwist(buffer, &ix, worldOrientation); @@ -291,7 +292,8 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid, object->updated = false; } -void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) { +void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { + // SERIALPORT.printf("old send pose?\n"); Polar velocity = roboid->propulsion->GetVelocity(); Vector2 worldVelocity2 = Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle); @@ -317,7 +319,7 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) { const unsigned int bufferSize = 3 + 12 + 12; unsigned char buffer[bufferSize] = { PoseMsg, - 0, // objectId; + 0, // objectId; Pose_LinearVelocity | Pose_AngularVelocity, }; unsigned char ix = 3; @@ -339,7 +341,7 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) { // #endif // } -void NetworkSync::SendInvestigate(InterestingThing* thing) { +void NetworkSync::SendInvestigate(InterestingThing *thing) { unsigned char ix = 0; buffer[ix++] = InvestigateMsg; buffer[ix++] = thing->networkId; @@ -350,7 +352,7 @@ void NetworkSync::SendInvestigate(InterestingThing* thing) { #endif } -void NetworkSync::SendText(const char* s) { +void NetworkSync::SendText(const char *s) { unsigned char length; for (length = 0; length < 253; length++) { if (s[length] == '\0') @@ -389,8 +391,7 @@ void NetworkSync::SendInt(const int x) { // Low-level functions -void NetworkSync::SendVector3(unsigned char* data, - unsigned char* startIndex, +void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, const Vector3 v) { SendSingle100(data, *startIndex, v.Right()); (*startIndex) += 4; @@ -400,8 +401,7 @@ void NetworkSync::SendVector3(unsigned char* data, (*startIndex) += 4; } -void NetworkSync::SendQuaternion(unsigned char* data, - const int startIndex, +void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex, const Quaternion q) { Vector3 angles = Quaternion::ToAngles(q); int ix = startIndex; @@ -410,30 +410,26 @@ void NetworkSync::SendQuaternion(unsigned char* data, SendAngle8(data, ix++, angles.Forward()); } -void NetworkSync::SendPolar(unsigned char* data, - unsigned char* startIndex, +void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex, Polar p) { SendAngle8(data, *startIndex, (const float)p.angle.InDegrees()); SendSingle100(data, (*startIndex) + 1, p.distance); } -void NetworkSync::SendSpherical16(unsigned char* data, - unsigned char* startIndex, - Spherical16 s) { +void NetworkSync::SendSpherical16(unsigned char *data, + unsigned char *startIndex, Spherical16 s) { SendAngle8(data, (*startIndex)++, s.direction.horizontal.InDegrees()); SendAngle8(data, (*startIndex)++, s.direction.vertical.InDegrees()); SendFloat16(data, startIndex, s.distance); } -void NetworkSync::SendSwingTwist(unsigned char* data, - unsigned char* ix, +void NetworkSync::SendSwingTwist(unsigned char *data, unsigned char *ix, const SwingTwist16 r) { Quaternion q = r.ToQuaternion(); SendQuat32(buffer, ix, q); } -void NetworkSync::SendQuat32(unsigned char* data, - unsigned char* startIndex, +void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex, const Quaternion q) { unsigned char qx = (char)(q.x * 127 + 128); unsigned char qy = (char)(q.y * 127 + 128); @@ -452,8 +448,7 @@ void NetworkSync::SendQuat32(unsigned char* data, data[(*startIndex)++] = qw; } -void NetworkSync::SendAngle8(unsigned char* data, - unsigned int startIndex, +void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex, const float angle) { Angle8 packedAngle2 = Angle8::Degrees(angle); data[startIndex] = packedAngle2.GetBinary(); @@ -480,16 +475,14 @@ void NetworkSync::SendAngle8(unsigned char* data, // // data[startIndex + 3]); // } -void NetworkSync::SendSingle100(unsigned char* data, - unsigned int startIndex, +void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex, float value) { // Sends a float with truncated 2 decimal precision Int32 intValue = value * 100; SendInt32(data, startIndex, intValue); } -void NetworkSync::SendFloat16(unsigned char* data, - unsigned char* startIndex, +void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex, float value) { float16 value16 = float16(value); short binary = value16.getBinary(); @@ -498,11 +491,10 @@ void NetworkSync::SendFloat16(unsigned char* data, data[(*startIndex)++] = binary & 0xFF; } -void NetworkSync::SendInt32(unsigned char* data, - unsigned int startIndex, +void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex, Int32 value) { for (unsigned char ix = 0; ix < 4; ix++) { - data[startIndex++] = ((unsigned char*)&value)[ix]; + data[startIndex++] = ((unsigned char *)&value)[ix]; } } diff --git a/Roboid.cpp b/Roboid.cpp index cb1d6ae..e687f4d 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -20,23 +20,20 @@ Roboid::Roboid() : Thing(0) { this->perception->roboid = this; this->propulsion = nullptr; this->networkSync = nullptr; - // this->actuation = nullptr; + this->worldPosition = Spherical16::zero; this->worldOrientation = SwingTwist16::identity; - // this->worldOrientation = Quaternion::identity; - // this->worldAngleAxis = AngleAxisOf(); } -Roboid::Roboid(Propulsion* propulsion) : Roboid() { +Roboid::Roboid(Propulsion *propulsion) : Roboid() { this->propulsion = propulsion; if (propulsion != nullptr) propulsion->roboid = this; } -void Passer::RoboidControl::Roboid::SetName(char* name) { - this->name = name; -} +void Passer::RoboidControl::Roboid::SetName(char *name) { this->name = name; } +#include void Roboid::Update(unsigned long currentTimeMs) { if (perception != nullptr) perception->Update(currentTimeMs); @@ -46,13 +43,28 @@ void Roboid::Update(unsigned long currentTimeMs) { float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000; - SetPosition(this->worldPosition + - this->worldOrientation * Spherical16::forward * - this->propulsion->GetVelocity().distance * deltaTime); - SetOrientation(this->worldOrientation * - SwingTwist16::AngleAxis( - this->propulsion->GetAngularVelocity() * deltaTime, - Direction16::up)); + // Conversion from old units + Polar polarVelocity = this->propulsion->GetVelocity(); + this->linearVelocity = Spherical16( + polarVelocity.distance, + Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16()); + float oldAngular = this->propulsion->GetAngularVelocity(); + this->angularVelocity = + Spherical16(oldAngular, Angle16(), Angle16::Degrees(90)); + + SetPosition(this->position + this->orientation * Spherical16::forward * + this->linearVelocity.distance * deltaTime); + this->worldPosition = this->position; // assuming the roboid is the root + + SwingTwist16 rotation = SwingTwist16::AngleAxis( + this->angularVelocity.distance * deltaTime, Direction16::up); + if (perception != nullptr) + perception->UpdatePose(rotation); + + this->orientation = this->orientation * rotation; + + this->worldOrientation = + this->orientation; // assuming the roboid is the root } if (childCount > 0 && children != nullptr) { @@ -66,32 +78,9 @@ void Roboid::Update(unsigned long currentTimeMs) { lastUpdateTimeMs = currentTimeMs; } -Spherical16 Roboid::GetPosition() { - return this->worldPosition; -} -// Vector2 Roboid::GetPosition2D() { -// return Vector2(this->worldPosition.Right(), this->worldPosition.Forward()); -// } +Spherical16 Roboid::GetPosition() { return this->worldPosition; } -SwingTwist16 Roboid::GetOrientation() { - // Vector3 axis = this->worldAngleAxis.axis.ToVector3(); - // SwingTwist16 q = SwingTwist16::AngleAxis(this->worldAngleAxis.angle, axis); - return this->worldOrientation; -} - -// float Roboid::GetOrientation2D() { -// float maxAngle = 90 - Float::epsilon; // note: range vertical angle = -// -90..90 - -// // rotation axis is vertical, so we have a simple 2D orientation -// if (this->worldAngleAxis.axis.vertical.InDegrees() > maxAngle) -// return this->worldAngleAxis.angle; -// if (this->worldAngleAxis.axis.vertical.InDegrees() < -maxAngle) -// return -this->worldAngleAxis.angle; - -// SwingTwist16 q = GetOrientation(); -// return Quaternion::GetAngleAround(Vector3::up, q); -// } +SwingTwist16 Roboid::GetOrientation() { return this->worldOrientation; } void Roboid::SetPosition(Spherical16 newWorldPosition) { SwingTwist16 roboidOrientation = this->GetOrientation(); @@ -100,44 +89,33 @@ void Roboid::SetPosition(Spherical16 newWorldPosition) { Angle16 angle = Spherical16::SignedAngleBetween( roboidOrientation * Spherical16::forward, translation, Spherical16::up); Polar16 polarTranslation = Polar16( - distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance)); + distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance)); if (perception != nullptr) perception->UpdatePose(polarTranslation); - this->worldPosition = newWorldPosition; - if (networkSync != nullptr) - // networkSync->SendPosition(this->worldPosition); - networkSync->SendPose(this->worldPosition, roboidOrientation); + this->position = newWorldPosition; // roboid is the root? + // World position should be set in the update recursion + // this->worldPosition = newWorldPosition; + + // if (networkSync != nullptr) + // // networkSync->SendPosition(this->worldPosition); + // networkSync->SendPose(this->worldPosition, roboidOrientation); } #include -void Roboid::SetOrientation(SwingTwist16 worldOrientation) { - // float angle; - // Vector3 axis; - // worldOrientation.ToAngleAxis(&angle, &axis); - +void Roboid::SetOrientation(SwingTwist16 newOrientation) { SwingTwist16 delta = - SwingTwist16::Inverse(GetOrientation()) * worldOrientation; + SwingTwist16::Inverse(this->orientation) * newOrientation; if (perception != nullptr) perception->UpdatePose(delta); - // AngleAxisOf angleAxis = - // AngleAxisOf(angle, DirectionOf(axis)); - // this->worldAngleAxis = angleAxis; + this->orientation = newOrientation; } -// void Roboid::SetOrientation2D(float angle) { -// this->worldAngleAxis = AngleAxisOf(angle, DirectionOf::up); -// } - -Vector3 Passer::RoboidControl::Roboid::GetVelocity() { - return Vector3(); -} - -void Roboid::AddChild(Thing* child) { +void Roboid::AddChild(Thing *child) { Thing::AddChild(child); if (child->IsSensor()) { - Sensor* childSensor = (Sensor*)child; + Sensor *childSensor = (Sensor *)child; this->perception->AddSensor(childSensor); } } diff --git a/Roboid.h b/Roboid.h index 72ec04a..2a4c83d 100644 --- a/Roboid.h +++ b/Roboid.h @@ -12,16 +12,16 @@ class NetworkSync; /// @brief A Roboid is used to control autonomous robots class Roboid : public Thing { - public: +public: /// @brief Default constructor for a Roboid Roboid(); /// @brief Creates a Roboid with Perception and Propulsion abilities /// @param perception The Perception implementation to use for this Roboid /// @param propulsion The Propulsion implementation to use for this Roboid - Roboid(Propulsion* propulsion); + Roboid(Propulsion *propulsion); - char* name = nullptr; - void SetName(char* name); + char *name = nullptr; + void SetName(char *name); /// @brief Update the state of the Roboid /// @param currentTimeMs The time in milliseconds when calling this @@ -29,13 +29,13 @@ class Roboid : public Thing { void Update(unsigned long currentTimeMs); /// @brief The Perception module of this Roboid - Perception* perception = nullptr; + Perception *perception = nullptr; /// @brief The Propulsion module of this Roboid - Propulsion* propulsion = nullptr; + Propulsion *propulsion = nullptr; // ServoMotor *actuation = nullptr; /// @brief The reference to the module to synchronize states across a network - NetworkSync* networkSync = nullptr; + NetworkSync *networkSync = nullptr; /// @brief Retrieve the current position of the roboid /// @return The position in carthesian coordinates in world space @@ -50,7 +50,6 @@ class Roboid : public Thing { /// used. This value will be Quaternion::identity unless an orientation is /// received though network synchronization virtual SwingTwist16 GetOrientation(); - // float GetOrientation2D(); /// @brief Update the current position of the roboid /// @param worldPosition The position of the roboid in carthesian coordinates @@ -66,18 +65,15 @@ class Roboid : public Thing { /// perceived objects by the roboid (roboid->perception->perceivedObjets), /// as these are local to the roboid' orientation. virtual void SetOrientation(SwingTwist16 worldOrientation); - // void SetOrientation2D(float angle); - virtual Vector3 GetVelocity(); + virtual void AddChild(Thing *child) override; - virtual void AddChild(Thing* child) override; - - private: +private: /// @brief The position of the roboid in carthesian coordinates in world space /// @details This position may be set when NetworkSync is used to receive /// positions from an external tracking system. These values should not be set /// directly, but SetPosition should be used instead. - Spherical16 worldPosition = Spherical16::zero; + // Spherical16 worldPosition = Spherical16::zero; /// @brief The orientation of the roboid in world space /// @details The position may be set when NetworkSync is used to receive /// orientations from an external tracking system. This value should not be @@ -89,7 +85,7 @@ class Roboid : public Thing { unsigned long lastUpdateTimeMs = 0; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/Thing.h b/Thing.h index 6629acf..b5ce4d6 100644 --- a/Thing.h +++ b/Thing.h @@ -10,7 +10,7 @@ namespace RoboidControl { /// @brief A thing is a functional component on a robot class Thing { - public: +public: /// @brief Default constructor for a Thing Thing(unsigned char id); @@ -47,6 +47,7 @@ class Thing { /// @remark When this Thing has a parent, the position is relative to the /// parent's position and orientation Spherical16 position; + Spherical16 worldPosition; /// @brief The orientation of this Thing /// @remark When this Thing has a parent, the orientation is relative to the /// parent's orientation @@ -59,27 +60,27 @@ class Thing { /// @brief Sets the parent Thing /// @param parent The Thing which should become the parnet /// @remark This is equivalent to calling parent->AddChild(this); - virtual void SetParent(Thing* parent); + virtual void SetParent(Thing *parent); /// @brief Gets the parent Thing /// @return The parent Thing - Thing* GetParent(); + 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); + virtual void AddChild(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 *GetChild(unsigned char childIx); /// @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); + void SetModel(const char *url); /// @brief Updates the state of the thing /// @param currentTimeMs The current clock time in milliseconds @@ -87,9 +88,9 @@ class Thing { unsigned char childCount = 0; - const char* modelUrl = nullptr; + const char *modelUrl = nullptr; - protected: +protected: /// @brief Bitmask for Motor type static const unsigned int MotorType = 0x8000; /// @brief Bitmap for Sensor type @@ -112,14 +113,14 @@ class Thing { ExternalSensor, }; - Thing* parent = nullptr; - Thing** children = nullptr; + Thing *parent = nullptr; + Thing **children = nullptr; - public: +public: Spherical16 angularVelocity = Spherical16::zero; Spherical16 linearVelocity = Spherical16::zero; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file