From 525ba3ea18cecedd2cd27708574f267422d521a3 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 14 Jun 2024 19:48:07 +0200 Subject: [PATCH] Perception initialization is now though adding sensors as children --- ControlledMotor.h | 2 +- Motor.cpp | 2 +- Motor.h | 2 +- NetworkSync.h | 2 +- Perception.cpp | 6 ++-- Perception.h | 4 +-- Propulsion.cpp | 2 +- Propulsion.h | 2 +- Roboid.cpp | 53 ++++++++++++---------------- Roboid.h | 20 +++-------- Sensor.cpp | 10 ++++++ Sensor.h | 4 ++- Thing.cpp | 32 ++++++++++------- Thing.h | 52 ++++++++++++++++++++------- test/BB2B_Test.cc | 90 ++++++++++++++++++++++------------------------- 15 files changed, 154 insertions(+), 129 deletions(-) diff --git a/ControlledMotor.h b/ControlledMotor.h index a4a0cf6..d85ae83 100644 --- a/ControlledMotor.h +++ b/ControlledMotor.h @@ -23,7 +23,7 @@ public: float pidD = 0.0F; float pidI = 0.0F; - void Update(float currentTimeMs) override; + void Update(unsigned long currentTimeMs) override; /// @brief Set the target speed for the motor controller /// @param speed the target in revolutions per second. diff --git a/Motor.cpp b/Motor.cpp index c005d72..35ee9cd 100644 --- a/Motor.cpp +++ b/Motor.cpp @@ -14,4 +14,4 @@ void Motor::SetTargetSpeed(float targetSpeed) { float Motor::GetTargetSpeed() { return (this->currentTargetSpeed); } -void Motor::Update(float currentTimeMs) {} \ No newline at end of file +void Motor::Update(unsigned long currentTimeMs) {} \ No newline at end of file diff --git a/Motor.h b/Motor.h index 037962f..8563c1b 100644 --- a/Motor.h +++ b/Motor.h @@ -33,7 +33,7 @@ public: /// forward) virtual float GetActualSpeed(); - virtual void Update(float currentTimeMs); + virtual void Update(unsigned long currentTimeMs); float currentTargetSpeed = 0; diff --git a/NetworkSync.h b/NetworkSync.h index bba05f2..4b7128c 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -72,7 +72,7 @@ public: void SendPose(Roboid *roboid, bool recurse = true); void SendPose(Thing *thing, bool recurse = true); - void SendText(const char *s); + virtual void SendText(const char *s) {}; protected: Roboid *roboid; diff --git a/Perception.cpp b/Perception.cpp index 66783da..5bce0c0 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -454,8 +454,8 @@ InterestingThing *Perception::GetMostInterestingThing() { return closestObject; } -void Perception::Update(float currentTimeMs) { - float deltaTime = (currentTimeMs - lastUpdateTimeMs) / 1000; +void Perception::Update(unsigned long currentTimeMs) { + float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000.0f; if (deltaTime <= 0) return; @@ -487,7 +487,7 @@ void Perception::Update(float currentTimeMs) { // AddTrackedObject(switchSensor, position); } } else { - sensor->Update(); + sensor->Update(currentTimeMs); } } diff --git a/Perception.h b/Perception.h index 26483f8..721c61b 100644 --- a/Perception.h +++ b/Perception.h @@ -133,7 +133,7 @@ public: /// @details This will update the perceptoin of object. It will retrieve the /// latest state for each sensor and update the confidence of the tracked /// objects. - void Update(float currentTimeMs); + void Update(unsigned long currentTimeMs); /// @brief Update the position/orientation of the preceived objects from the /// given roboid translation @@ -160,7 +160,7 @@ public: /// @brief The number of Sensors used for Perception unsigned int sensorCount = 0; - float lastUpdateTimeMs = 0; + unsigned long lastUpdateTimeMs = 0; unsigned char lastObjectId = 1; static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum diff --git a/Propulsion.cpp b/Propulsion.cpp index 9510a4c..035ce82 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -18,7 +18,7 @@ Motor *Propulsion::GetMotor(unsigned int motorId) { return motor; } -void Propulsion::Update(float currentTimeMs) { +void Propulsion::Update(unsigned long currentTimeMs) { for (unsigned char motorIx = 0; motorIx < this->motorCount; motorIx++) { Motor *motor = this->motors[motorIx]; if (motor == nullptr) diff --git a/Propulsion.h b/Propulsion.h index bac840a..6800d41 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -22,7 +22,7 @@ public: /// @brief Update the propulsion state of the Roboid /// @param currentTimeMs The time in milliseconds when calling this - void Update(float currentTimeMs); + void Update(unsigned long currentTimeMs); /// @brief Get the number of motors in this roboid /// @return The number of motors. Zero when no motors are present diff --git a/Roboid.cpp b/Roboid.cpp index 31d103e..8c8936a 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -11,50 +11,35 @@ #include #endif -Roboid::Roboid() { +Roboid::Roboid() : Thing(0) { #ifdef RC_DEBUG Serial.begin(115200); #endif + this->type = RoboidType; this->perception = new Perception(); this->perception->roboid = this; this->propulsion = nullptr; this->networkSync = nullptr; - this->actuation = nullptr; + // this->actuation = nullptr; this->worldPosition = Vector3::zero; // this->worldOrientation = Quaternion::identity; this->worldAngleAxis = AngleAxis(); } -Roboid::Roboid(Perception *perception, Propulsion *propulsion) : Roboid() { - if (this->perception != nullptr) - delete this->perception; - this->perception = perception; - if (perception != nullptr) - perception->roboid = this; - +Roboid::Roboid(Propulsion *propulsion) : Roboid() { this->propulsion = propulsion; if (propulsion != nullptr) propulsion->roboid = this; } -Roboid::Roboid(Perception *perception, ServoMotor *actuation) : Roboid() { - this->perception = perception; - perception->roboid = this; - this->propulsion = nullptr; - - this->actuation = actuation; -} - -Roboid::Roboid(ServoMotor *actuation) : actuation(actuation) {} - -void Roboid::Update(float currentTimeMs) { +void Roboid::Update(unsigned long currentTimeMs) { if (perception != nullptr) perception->Update(currentTimeMs); if (propulsion != nullptr) { propulsion->Update(currentTimeMs); - float deltaTime = (currentTimeMs - lastUpdateTimeMs) / 1000; + float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000; Quaternion roboidOrientation = this->GetOrientation(); SetPosition(this->worldPosition + roboidOrientation * Vector3::forward * @@ -64,8 +49,12 @@ void Roboid::Update(float currentTimeMs) { Vector3::up)); } - if (actuation != nullptr) - actuation->Update(currentTimeMs); + // if (actuation != nullptr) + // actuation->Update(currentTimeMs); + if (childCount > 0 && children != nullptr) { + for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + children[childIx]->Update(currentTimeMs); + } if (networkSync != nullptr) networkSync->NetworkUpdate(this); @@ -104,7 +93,8 @@ void Roboid::SetPosition(Vector3 newWorldPosition) { float angle = Vector3::SignedAngle(roboidOrientation * Vector3::forward, translation, Vector3::up); Polar polarTranslation = Polar(angle, distance); - perception->UpdatePose(polarTranslation); + if (perception != nullptr) + perception->UpdatePose(polarTranslation); this->worldPosition = newWorldPosition; if (networkSync != nullptr) @@ -119,7 +109,8 @@ void Roboid::SetOrientation(Quaternion worldOrientation) { worldOrientation.ToAngleAxis(&angle, &axis); Quaternion delta = Quaternion::Inverse(GetOrientation()) * worldOrientation; - perception->UpdatePose(delta); + if (perception != nullptr) + perception->UpdatePose(delta); AngleAxis angleAxis = AngleAxis(angle, Axis(axis)); this->worldAngleAxis = angleAxis; @@ -129,10 +120,10 @@ void Roboid::SetOrientation2D(float angle) { this->worldAngleAxis = AngleAxis(angle, Axis::up); } -void Roboid::SetModel(const char *url, Vector3 position, Quaternion orientation, - float scale) { - this->modelUrl = url; - this->modelPosition = position; - this->modelOrientation = orientation; - this->modelScale = scale; +void Roboid::AddChild(Thing *child) { + Thing::AddChild(child); + if (child->IsSensor()) { + Sensor *childSensor = (Sensor *)child; + this->perception->AddSensor(childSensor); + } } diff --git a/Roboid.h b/Roboid.h index 7b93d98..7578bf9 100644 --- a/Roboid.h +++ b/Roboid.h @@ -11,29 +11,25 @@ namespace RoboidControl { class NetworkSync; /// @brief A Roboid is used to control autonomous robots -class Roboid { +class Roboid : public Thing { 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(Perception *perception, Propulsion *propulsion = nullptr); - - Roboid(Perception *perception, ServoMotor *actuation); - - Roboid(ServoMotor *actuationRoot); + Roboid(Propulsion *propulsion); /// @brief Update the state of the Roboid /// @param currentTimeMs The time in milliseconds when calling this /// function - void Update(float currentTimeMs); + void Update(unsigned long currentTimeMs); /// @brief The Perception module of this Roboid Perception *perception = nullptr; /// @brief The Propulsion module of this Roboid Propulsion *propulsion = nullptr; - ServoMotor *actuation = nullptr; + // ServoMotor *actuation = nullptr; /// @brief The reference to the module to synchronize states across a network NetworkSync *networkSync = nullptr; @@ -69,13 +65,7 @@ public: virtual void SetOrientation(Quaternion worldOrientation); void SetOrientation2D(float angle); - void SetModel(const char *url, Vector3 position = Vector3(0, 0, 0), - Quaternion orientation = Quaternion::identity, float scale = 1); - - const char *modelUrl = nullptr; - Vector3 modelPosition = Vector3::zero; - Quaternion modelOrientation = Quaternion::identity; - float modelScale = 1; + virtual void AddChild(Thing *child) override; private: /// @brief The position of the roboid in carthesian coordinates in world space diff --git a/Sensor.cpp b/Sensor.cpp index 3808d15..73a53e9 100644 --- a/Sensor.cpp +++ b/Sensor.cpp @@ -1,5 +1,15 @@ #include "Sensor.h" +#include "Roboid.h" + Sensor::Sensor() : Thing(0) { // for now, id should be set properly later this->type = Thing::SensorType; +} + +void Sensor::SetParent(Thing *parent) { + this->parent = parent; + if (this->parent->IsRoboid()) { + Roboid *roboidParent = (Roboid *)this->parent; + roboidParent->perception->AddSensor(this); + } } \ No newline at end of file diff --git a/Sensor.h b/Sensor.h index b7cfd82..8457e7e 100644 --- a/Sensor.h +++ b/Sensor.h @@ -11,7 +11,9 @@ public: /// @brief Default Constructor for a Sensor Sensor(); - virtual void Update(){}; + /// @brief Sets the parent Thing + /// @param parent The Thing which should become the parent + virtual void SetParent(Thing *parent) override; }; } // namespace RoboidControl diff --git a/Thing.cpp b/Thing.cpp index 5fddcf6..49c2891 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -1,5 +1,7 @@ #include "Thing.h" +// #include "Roboid.h" + using namespace Passer::RoboidControl; Thing::Thing(unsigned char id) : position(Polar::zero), id(id) { @@ -17,31 +19,37 @@ const unsigned int Thing::ControlledMotorType = const unsigned int Thing::UncontrolledMotorType = MotorType | (unsigned int)Type::UncontrolledMotor; const unsigned int Thing::ServoType = (unsigned int)Type::Servo; -const unsigned int Thing::ExternalType = (unsigned int)Type::ExternalSensor; bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; } bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; } -void Thing::SetModel(const char *url, Vector3 position, Quaternion orientation, - float scale) { - this->modelUrl = url; - this->modelPosition = position; - this->modelOrientation = orientation; - this->modelScale = scale; -} +bool Thing::IsRoboid() { return (type & Thing::RoboidType) != 0; } -void Thing::SetParent(Thing *parent) { this->parent = parent; } +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 this->parent; } void Thing::AddChild(Thing *child) { Thing **newChildren = new Thing *[this->childCount + 1]; - for (unsigned char childIx = 0; childIx < this->childCount; childIx++) + 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->SetParent(this); + child->parent = this; if (this->children != nullptr) delete[] this->children; @@ -51,7 +59,7 @@ void Thing::AddChild(Thing *child) { } Thing *Thing::GetChild(unsigned char childIx) { - if (childIx < this->childCount) { + if (childIx >= 0 && childIx < this->childCount) { return this->children[childIx]; } else return nullptr; diff --git a/Thing.h b/Thing.h index bd1ec14..436ae0a 100644 --- a/Thing.h +++ b/Thing.h @@ -27,36 +27,62 @@ public: static const unsigned int UncontrolledMotorType; /// @brief The type of an object received from the network static const unsigned int ServoType; - static const unsigned int ExternalType; /// @brief Check if the Thing is a Motor - /// @returns True when the Thing is a Motor and False otherwise + /// @return True when the Thing is a Motor and False otherwise bool IsMotor(); /// @brief Check if the Thing is a Sensor - /// @returns True when the Thing is a Sensor and False otherwise + /// @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 of this Thing + /// @remark When this Thing has a parent, the position is relative to the + /// parent's position and orientation Spherical position; + /// @brief The orientation of this Thing + /// @remark When this Thing has a parent, the orientation is relative to the + /// parent's orientation Quaternion orientation; - void SetModel(const char *url, Vector3 position = Vector3(0, 0, 0), - Quaternion orientation = Quaternion::identity, float scale = 1); - const char *modelUrl = nullptr; - Vector3 modelPosition = Vector3::zero; - Quaternion modelOrientation = Quaternion::identity; - float modelScale = 1; - - void SetParent(Thing *parent); + /// @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(); - void AddChild(Thing *child); + /// @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); + /// @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); + /// @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) {} + 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; /// @brief Basic Thing types enum class Type { @@ -75,6 +101,8 @@ protected: Thing *parent = nullptr; unsigned char childCount = 0; Thing **children = nullptr; + + const char *modelUrl = nullptr; }; } // namespace RoboidControl diff --git a/test/BB2B_Test.cc b/test/BB2B_Test.cc index 2e79882..66643a9 100644 --- a/test/BB2B_Test.cc +++ b/test/BB2B_Test.cc @@ -25,21 +25,21 @@ TEST(BB2B, NoObstacle) { Motor *motorLeft = new Motor(); Motor *motorRight = new Motor(); - MockDistanceSensor *sensorLeft = new MockDistanceSensor(10.0F); - sensorLeft->position.angle = -30; - MockDistanceSensor *sensorRight = new MockDistanceSensor(10.0F); - sensorRight->position.angle = 30; - - Sensor *sensors[] = {sensorLeft, sensorRight}; - - Perception *perception = new Perception(sensors, 2); - DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight); propulsion->SetDimensions( 1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s 1); - Roboid *roboid = new Roboid(perception, propulsion); + Roboid *roboid = new Roboid(propulsion); + + MockDistanceSensor *sensorLeft = new MockDistanceSensor(10.0F); + sensorLeft->position.horizontalAngle = -30; + roboid->AddChild(sensorLeft); + MockDistanceSensor *sensorRight = new MockDistanceSensor(10.0F); + sensorRight->SetParent(roboid); + sensorRight->position.horizontalAngle = 30; + + roboid->perception->nearbyDistance = 0.2f; #pragma endregion @@ -60,7 +60,7 @@ TEST(BB2B, NoObstacle) { sensorLeft->SimulateMeasurement(100.0F); sensorRight->SimulateMeasurement(100.0F); - roboid->Update(0.0F); + roboid->Update(0); #pragma endregion @@ -120,23 +120,22 @@ TEST(BB2B, ObstacleLeft) { Motor *motorLeft = new Motor(); Motor *motorRight = new Motor(); - MockDistanceSensor *sensorLeft = new MockDistanceSensor(); - sensorLeft->position.angle = -30; - MockDistanceSensor *sensorRight = new MockDistanceSensor(); - sensorRight->position.angle = 30; - - Sensor *sensors[] = {sensorLeft, sensorRight}; - - Perception *perception = new Perception(sensors, 2); - perception->nearbyDistance = 0.2f; - DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight); propulsion->SetDimensions( 1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s 8 / Angle::pi); // we use this, such that angular velocity will be 90 // degrees per second - Roboid *roboid = new Roboid(perception, propulsion); + Roboid *roboid = new Roboid(propulsion); + + MockDistanceSensor *sensorLeft = new MockDistanceSensor(); + sensorLeft->position.horizontalAngle = -30; + roboid->AddChild(sensorLeft); + MockDistanceSensor *sensorRight = new MockDistanceSensor(); + sensorRight->SetParent(roboid); + sensorRight->position.horizontalAngle = 30; + + roboid->perception->nearbyDistance = 0.2f; #pragma endregion @@ -145,7 +144,7 @@ TEST(BB2B, ObstacleLeft) { // place obstacle on the left sensorLeft->SimulateMeasurement(0.1F); - roboid->Update(0.1F); + roboid->Update(100); #pragma endregion @@ -222,23 +221,22 @@ TEST(BB2B, ObstacleRight) { Motor *motorLeft = new Motor(); Motor *motorRight = new Motor(); - MockDistanceSensor *sensorLeft = new MockDistanceSensor(); - sensorLeft->position.angle = -30; - MockDistanceSensor *sensorRight = new MockDistanceSensor(); - sensorRight->position.angle = 30; - - Sensor *sensors[] = {sensorLeft, sensorRight}; - - Perception *perception = new Perception(sensors, 2); - perception->nearbyDistance = 0.2f; - DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight); propulsion->SetDimensions( 1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s 8 / Angle::pi); // we use this, such that angular velocity will be 90 // degrees per second - Roboid *roboid = new Roboid(perception, propulsion); + Roboid *roboid = new Roboid(propulsion); + + MockDistanceSensor *sensorLeft = new MockDistanceSensor(); + sensorLeft->position.horizontalAngle = -30; + roboid->AddChild(sensorLeft); + MockDistanceSensor *sensorRight = new MockDistanceSensor(); + sensorRight->SetParent(roboid); + sensorRight->position.horizontalAngle = 30; + + roboid->perception->nearbyDistance = 0.2f; #pragma endregion @@ -247,7 +245,7 @@ TEST(BB2B, ObstacleRight) { // place obstacle on the left sensorRight->SimulateMeasurement(0.1F); - roboid->Update(0.1F); + roboid->Update(100); #pragma endregion @@ -322,23 +320,21 @@ TEST(BB2B, ObstacleBoth) { Motor *motorLeft = new Motor(); Motor *motorRight = new Motor(); - MockDistanceSensor *sensorLeft = new MockDistanceSensor(); - sensorLeft->position.angle = -30; - MockDistanceSensor *sensorRight = new MockDistanceSensor(); - sensorRight->position.angle = 30; - - Sensor *sensors[] = {sensorLeft, sensorRight}; - - Perception *perception = new Perception(sensors, 2); - perception->nearbyDistance = 0.2f; - DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight); propulsion->SetDimensions( 1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s 8 / Angle::pi); // we use this, such that angular velocity will be 90 // degrees per second + Roboid *roboid = new Roboid(propulsion); - Roboid *roboid = new Roboid(perception, propulsion); + MockDistanceSensor *sensorLeft = new MockDistanceSensor(); + sensorLeft->position.horizontalAngle = -30; + roboid->AddChild(sensorLeft); + MockDistanceSensor *sensorRight = new MockDistanceSensor(); + sensorRight->SetParent(roboid); + sensorRight->position.horizontalAngle = 30; + + roboid->perception->nearbyDistance = 0.2f; #pragma endregion @@ -347,7 +343,7 @@ TEST(BB2B, ObstacleBoth) { sensorLeft->SimulateMeasurement(0.1F); sensorRight->SimulateMeasurement(0.1F); - roboid->Update(0.1F); + roboid->Update(100); #pragma endregion