From ba009f79f7d28af31ebd512e613a4468c6b795ec Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 27 Nov 2024 13:40:58 +0100 Subject: [PATCH] Support simulated sensing --- DifferentialDrive.cpp | 68 +++++++------- DifferentialDrive.h | 6 +- DistanceSensor.cpp | 6 ++ DistanceSensor.h | 2 + NetworkPerception.cpp | 15 +++- NetworkPerception.h | 3 + NetworkSync.cpp | 201 +++++++++++++++--------------------------- NetworkSync.h | 17 ++-- Propulsion.cpp | 6 +- Propulsion.h | 4 +- Roboid.cpp | 34 ++----- Sensor.cpp | 20 ++++- Sensor.h | 7 +- Thing.cpp | 54 +++++++++--- Thing.h | 19 +++- 15 files changed, 242 insertions(+), 220 deletions(-) diff --git a/DifferentialDrive.cpp b/DifferentialDrive.cpp index 9413c72..316d23f 100644 --- a/DifferentialDrive.cpp +++ b/DifferentialDrive.cpp @@ -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; -} \ No newline at end of file +// return angularVelocity; +// } \ No newline at end of file diff --git a/DifferentialDrive.h b/DifferentialDrive.h index 781d6f9..01fb7e6 100644 --- a/DifferentialDrive.h +++ b/DifferentialDrive.h @@ -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 diff --git a/DistanceSensor.cpp b/DistanceSensor.cpp index 4d4f6ee..5006e82 100644 --- a/DistanceSensor.cpp +++ b/DistanceSensor.cpp @@ -1,5 +1,6 @@ #include "DistanceSensor.h" +#include "NetworkPerception.h" #include 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); +} diff --git a/DistanceSensor.h b/DistanceSensor.h index b24d4fd..fa39598 100644 --- a/DistanceSensor.h +++ b/DistanceSensor.h @@ -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; diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 6772abe..a453aeb 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -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(); +} \ No newline at end of file diff --git a/NetworkPerception.h b/NetworkPerception.h index 769c3c2..b84b254 100644 --- a/NetworkPerception.h +++ b/NetworkPerception.h @@ -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 diff --git a/NetworkSync.cpp b/NetworkSync.cpp index ad4697c..c01d7af 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -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 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; diff --git a/NetworkSync.h b/NetworkSync.h index 786934e..5a961c3 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -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 diff --git a/Propulsion.cpp b/Propulsion.cpp index 932d139..344806d 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -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; } \ No newline at end of file +Spherical16 Propulsion::GetAngularVelocity() { return this->angularVelocity; } \ No newline at end of file diff --git a/Propulsion.h b/Propulsion.h index fcc4b49..ff003b0 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -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 diff --git a/Roboid.cpp b/Roboid.cpp index 02a6f3c..29a05ed 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -30,6 +30,7 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() { propulsion->roboid = this; } +#include 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(); diff --git a/Sensor.cpp b/Sensor.cpp index 73a53e9..daa996d 100644 --- a/Sensor.cpp +++ b/Sensor.cpp @@ -12,4 +12,22 @@ void Sensor::SetParent(Thing *parent) { Roboid *roboidParent = (Roboid *)this->parent; roboidParent->perception->AddSensor(this); } -} \ No newline at end of file +} + +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); +} diff --git a/Sensor.h b/Sensor.h index a6615f4..c669038 100644 --- a/Sensor.h +++ b/Sensor.h @@ -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 diff --git a/Thing.cpp b/Thing.cpp index 96d7399..8e2bd64 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -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; } \ No newline at end of file diff --git a/Thing.h b/Thing.h index 62fcb9e..967ab72 100644 --- a/Thing.h +++ b/Thing.h @@ -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; \ No newline at end of file