Moved position/orientation to roboid

Propulsion.cpp

@@ -43,25 +43,4 @@ void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
 
 Polar Propulsion::GetVelocity() { return Polar(0, 0); }
 
-float Propulsion::GetAngularVelocity() { return 0; }
-
-Vector3 Propulsion::GetPosition() { return this->worldPosition; }
-
-Quaternion Propulsion::GetOrientation() { return this->worldOrientation; }
-
-void Propulsion::SetPosition(Vector3 newWorldPosition) {
-  Vector3 translation = newWorldPosition - this->worldPosition;
-  float distance = translation.magnitude();
-  float angle = Vector3::SignedAngle(this->worldOrientation * Vector3::forward,
-                                     translation, Vector3::up);
-  Polar polarTranslation = Polar(angle, distance);
-  roboid->perception->UpdatePose(polarTranslation);
-  this->worldPosition = newWorldPosition;
-}
-
-void Propulsion::SetOrientation(Quaternion worldOrientation) {
-  Quaternion delta =
-      Quaternion::Inverse(this->worldOrientation) * worldOrientation;
-  roboid->perception->UpdatePose(delta);
-  this->worldOrientation = worldOrientation;
-}
+float Propulsion::GetAngularVelocity() { return 0; }

Propulsion.h

@@ -63,15 +63,15 @@ public:
   virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
                              float pitch = 0.0F, float roll = 0.0F);
 
+  /// @brief Retrieve the current velocity of the roboid
+  /// @return The velocity in polar coordinates
+  /// The actual units of the velocity depend on the implementation
   virtual Polar GetVelocity();
+  /// @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 Vector3 GetPosition();
-  virtual Quaternion GetOrientation();
-
-  virtual void SetPosition(Vector3 worldPosition);
-  virtual void SetOrientation(Quaternion worldOrientation);
-
   Roboid *roboid = nullptr;
 
 protected:
@@ -79,9 +79,6 @@ protected:
   unsigned int motorCount = 0;
   /// @brief The Placement of the motors used for Propulsion
   Placement *placement = nullptr;
-
-  Vector3 worldPosition = Vector3::zero;
-  Quaternion worldOrientation = Quaternion::identity;
 };
 
 } // namespace RoboidControl

Roboid.cpp

@@ -16,4 +16,25 @@ void Roboid::Update(float currentTimeMs) {
     perception->Update(currentTimeMs);
   if (networkSync != nullptr)
     networkSync->NetworkUpdate(this);
+}
+
+Vector3 Roboid::GetPosition() { return this->worldPosition; }
+
+Quaternion Roboid::GetOrientation() { return this->worldOrientation; }
+
+void Roboid::SetPosition(Vector3 newWorldPosition) {
+  Vector3 translation = newWorldPosition - this->worldPosition;
+  float distance = translation.magnitude();
+  float angle = Vector3::SignedAngle(this->worldOrientation * Vector3::forward,
+                                     translation, Vector3::up);
+  Polar polarTranslation = Polar(angle, distance);
+  perception->UpdatePose(polarTranslation);
+  this->worldPosition = newWorldPosition;
+}
+
+void Roboid::SetOrientation(Quaternion worldOrientation) {
+  Quaternion delta =
+      Quaternion::Inverse(this->worldOrientation) * worldOrientation;
+  perception->UpdatePose(delta);
+  this->worldOrientation = worldOrientation;
 }

Roboid.h

@@ -24,11 +24,51 @@ public:
   void Update(float currentTimeMs);
 
   /// @brief The Perception module of this Roboid
-  Perception *perception;
+  Perception *perception = nullptr;
   /// @brief The Propulsion module of this Roboid
-  Propulsion *propulsion;
+  Propulsion *propulsion = nullptr;
 
+  /// @brief The reference to the module to synchronize states across a network
   NetworkSync *networkSync = nullptr;
+
+  /// @brief Retrieve the current position of the roboid
+  /// @return The position in carthesian coordinates in world space
+  /// The origin and units of the position depends on the position tracking
+  /// system used. This value will be Vector3::zero unless a position is
+  /// received through network synchronisation
+  virtual Vector3 GetPosition();
+  /// @brief Retrieve the current orientation of the roboid
+  /// @return The orientation quaternion in world space
+  /// The origin orientation depends on the position tracking system used. This
+  /// value will be Quaternion::identity unless an orientation is received
+  /// though network synchronization
+  virtual Quaternion GetOrientation();
+
+  /// @brief Update the current position of the roboid
+  /// @param worldPosition The position of the roboid in carthesian coordinates
+  /// in world space The use of this function will also update the positions and
+  /// orientations of the perceived objects by the roboid
+  /// (roboid->perception->perceivedObjects), as these are local to the
+  /// roboid's position.
+  virtual void SetPosition(Vector3 worldPosition);
+  /// @brief Update the current orientation of the roboid
+  /// @param worldOrientation The orientation of the roboid in world space
+  /// The use of this function will also update the orientations of the
+  /// perceived objects by the roboid (roboid->perception->perceivedObjets),
+  /// as these are local to the roboid' orientation.
+  virtual void SetOrientation(Quaternion worldOrientation);
+
+private:
+  /// @brief The position of the roboid in carthesian coordinates in world space
+  /// 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.
+  Vector3 worldPosition = Vector3::zero;
+  /// @brief The orientation of the roboid in world space
+  /// The position may be set when NetworkSync is used to receive orientations
+  /// from an external tracking system. This value should not be set directly,
+  /// but SetOrientation should be used instead.
+  Quaternion worldOrientation = Quaternion::identity;
 };
 
 } // namespace RoboidControl