Perception initialization is now though adding sensors as children

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.

Motor.cpp

@@ -14,4 +14,4 @@ void Motor::SetTargetSpeed(float targetSpeed) {
 
 float Motor::GetTargetSpeed() { return (this->currentTargetSpeed); }
 
-void Motor::Update(float currentTimeMs) {}
+void Motor::Update(unsigned long currentTimeMs) {}

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;
 

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;

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

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

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)

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

Roboid.cpp

@@ -11,50 +11,35 @@
 #include <Arduino.h>
 #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);
+  }
 }

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

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

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

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;

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

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