Replace roboid worldpos/orient with worldOriginPos/orient

NetworkPerception.cpp

@@ -37,12 +37,13 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
   if (networkId == roboid->networkSync->networkId)
     return;
 
-  // printf("Received create message [%d/%d]\n", networkId, objectId);
+  printf("Received create message [%d/%d]\n", networkId, objectId);
   InterestingThing *thing =
       roboid->perception->FindTrackedObject(networkId, objectId);
   if (thing != nullptr) {
     thing->type = objectType;
   }
+  printf("complete\n");
 }
 
 void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
@@ -83,6 +84,7 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
     return;
 
   Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4));
+  /*
   Spherical16 roboidPosition = roboid->GetPosition();
   Spherical16 deltaPosition = worldPosition - roboidPosition;
 
@@ -101,7 +103,17 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
   // Polar position = Polar(angle, distance);
   Spherical16 position =
       localPosition; // Spherical16::FromVector3(localPosition);
-
+  */
+  SwingTwist16 originOrientation;
+  Spherical16 originPosition;
+  if (roboid->worldOrigin == nullptr) {
+    originOrientation = SwingTwist16::identity;
+    originPosition = Spherical16::zero;
+  } else {
+    originOrientation = roboid->worldOrigin->orientation;
+    originPosition = roboid->worldOrigin->position;
+  }
+  Spherical16 position = originPosition + originOrientation * worldPosition;
   // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x,
   //        worldPosition.y, worldPosition.z, roboidPosition.x,
   //        roboidPosition.y, roboidPosition.z, position.distance,
@@ -118,6 +130,7 @@ void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
   float radius = ReceiveFloat100(data, 3);
 
   Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7));
+  /*
   Spherical16 roboidPosition = roboid->GetPosition();
   Spherical16 deltaPosition = worldPosition - roboidPosition;
 
@@ -127,6 +140,17 @@ void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
 
   Spherical16 position =
       localPosition; // Spherical16::FromVector3(localPosition);
+  */
+  SwingTwist16 originOrientation;
+  Spherical16 originPosition;
+  if (roboid->worldOrigin == nullptr) {
+    originOrientation = SwingTwist16::identity;
+    originPosition = Spherical16::zero;
+  } else {
+    originOrientation = roboid->worldOrigin->orientation;
+    originPosition = roboid->worldOrigin->position;
+  }
+  Spherical16 position = originPosition + originOrientation * worldPosition;
 
   roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
                                        0x81, 0x81, networkId);
@@ -154,17 +178,84 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
 
   InterestingThing *thing =
       roboid->perception->FindTrackedObject(networkId, objectId);
+  if (thing == nullptr) {
+    thing = roboid->perception->AddTrackedObject(this, position, orientation,
+                                                 0xFF, objectId, networkId);
+    if (thing->networkId != 0x00) {
+      // Unknown thing
+      roboid->networkSync->SendInvestigate(thing);
+    }
+  }
 
-  SwingTwist16 roboidOrientation = roboid->GetOrientation();
+  // SwingTwist16 roboidOrientation = roboid->GetOrientation();
   Spherical16 position = Spherical16::zero;
   SwingTwist16 orientation = SwingTwist16::identity;
 
-  if ((poseType & NetworkSync::Pose_Position) != 0) {
+  Vector3 worldAngles = ReceiveVector3(data, 16);
-    Spherical16 worldPosition =
+  SwingTwist16 worldOrientation = SwingTwist16(
-        Spherical16::FromVector3(ReceiveVector3(data, 4));
+      Angle16::Degrees(worldAngles.Up()), Angle16::Degrees(worldAngles.Right()),
+      Angle16::Degrees(
+          worldAngles.Forward())); // Quaternion::Euler(worldAngles);
+
+  if ((poseType & NetworkSync::Pose_Orientation) != 0) {
     if (objectId == 0) {
-      roboid->SetPosition(worldPosition);
+      // roboid->SetOrientation(worldOrientation);
+      if (roboid->worldOrigin == nullptr) {
+        printf("creating new origin\n");
+        roboid->worldOrigin = new Thing(0);
+        printf("created\n");
+      }
+
+      roboid->worldOrigin->orientation =
+          SwingTwist16::Inverse(worldOrientation);
     } else {
+      // orientation = SwingTwist16::Inverse(roboidOrientation) *
+      // worldOrientation; if (thing != nullptr) {
+      //   thing->orientation = orientation;
+      // }
+      SwingTwist16 originOrientation;
+      Spherical16 originPosition;
+      if (roboid->worldOrigin == nullptr) {
+        originOrientation = SwingTwist16::identity;
+        originPosition = Spherical16::zero;
+      } else {
+        originOrientation = roboid->worldOrigin->orientation;
+        originPosition = roboid->worldOrigin->position;
+      }
+
+      thing->orientation = originOrientation * worldOrientation;
+    }
+  }
+
+  if ((poseType & NetworkSync::Pose_Position) != 0) {
+    Vector3 worldVector3 = ReceiveVector3(data, 4);
+    Spherical16 worldPosition = Spherical16::FromVector3(worldVector3);
+    if (objectId == 0) {
+      // roboid->SetPosition(worldPosition);
+      if (roboid->worldOrigin == nullptr) {
+        printf("creating new origin again\n");
+        roboid->worldOrigin = new Thing(0);
+      }
+
+      roboid->worldOrigin->position =
+          roboid->worldOrigin->orientation * -worldPosition;
+    } else {
+      SwingTwist16 originOrientation;
+      Spherical16 originPosition;
+      if (roboid->worldOrigin == nullptr) {
+        originOrientation = SwingTwist16::identity;
+        originPosition = Spherical16::zero;
+      } else {
+        originOrientation = roboid->worldOrigin->orientation;
+        originPosition = roboid->worldOrigin->position;
+      }
+
+      // SwingTwist16 roboidLocalOrientation =
+      //     originOrientation * worldOrientation;
+
+      thing->position = originPosition + originOrientation * worldPosition;
+
+      /*
       Spherical16 roboidPosition = roboid->GetPosition();
 
       // float distance = Vector3::Distance(roboidPosition, worldPosition);
@@ -175,41 +266,17 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
 
       Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
                                   (worldPosition - roboidPosition);
-      position = localPosition; // Spherical16::FromVector3(localPosition);
+      Vector3 roboidVector3 = roboidPosition.ToVector3();
-      // printf("  worldPosition (%f %f %f) localPosition (%f %f %f)\n",
+      printf("  roboid position (%f %f %f)\n", roboidVector3.Right(),
-      //        worldPosition.Right(), worldPosition.Up(),
+             roboidVector3.Up(), roboidVector3.Forward());
-      //        worldPosition.Forward(), localPosition.Right(),
+      printf("  [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n ",
-      //        localPosition.Up(), localPosition.Forward());
+             networkId, objectId, worldVector3.Right(), worldVector3.Up(),
-      // printf("  position: %f (%f %f)\n", position.distance,
+             worldVector3.Forward(), localPosition.distance,
-      //        position.horizontal.InDegrees(), position.vertical.InDegrees());
+             localPosition.direction.horizontal.InDegrees(),
-      if (thing != nullptr)
+             localPosition.direction.vertical.InDegrees());
-        thing->position = position;
-    }
-  }
 
-  if ((poseType & NetworkSync::Pose_Orientation) != 0) {
+      thing->position = localPosition;
-    Vector3 worldAngles = ReceiveVector3(data, 16);
+      */
-    SwingTwist16 worldOrientation = SwingTwist16(
-        Angle16::Degrees(worldAngles.Up()),
-        Angle16::Degrees(worldAngles.Right()),
-        Angle16::Degrees(
-            worldAngles.Forward())); // Quaternion::Euler(worldAngles);
-    if (objectId == 0) {
-      roboid->SetOrientation(worldOrientation);
-    } else {
-      orientation = SwingTwist16::Inverse(roboidOrientation) * worldOrientation;
-      if (thing != nullptr) {
-        thing->orientation = orientation;
-      }
-    }
-  }
-
-  if (thing == nullptr) {
-    thing = roboid->perception->AddTrackedObject(this, position, orientation,
-                                                 0xFF, objectId, networkId);
-    if (thing->networkId != 0x00) {
-      // Unknown thing
-      roboid->networkSync->SendInvestigate(thing);
     }
   }
 }

NetworkSync.cpp

@@ -1,6 +1,6 @@
 #include "NetworkSync.h"
 
-// #define RC_DEBUG 1
+#define RC_DEBUG 1
 
 #ifdef RC_DEBUG
 #include <Arduino.h>
@@ -204,8 +204,8 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
 
 #if RC_DEBUG
     if (thing->id == 0)
-      SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", this->networkId,
+      SERIALPORT.printf("Sent PoseMsg Thing [%d/%d] %f\n", this->networkId,
-                        buffer[1]);
+                        buffer[1], thing->position.distance);
 #endif
   }
 
@@ -270,12 +270,33 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
     return;
   }
 
+  /*
     Spherical16 roboidPosition = roboid->GetPosition();
     SwingTwist16 roboidOrientation = roboid->GetOrientation();
 
     Spherical16 worldPosition =
         roboidPosition + roboidOrientation * object->position;
     SwingTwist16 worldOrientation = roboidOrientation * object->orientation;
+  */
+
+  SwingTwist16 inv_originOrientation;
+  Spherical16 originPosition;
+  if (roboid->worldOrigin == nullptr) {
+    inv_originOrientation = SwingTwist16::identity;
+    originPosition = Spherical16::zero;
+  } else {
+    inv_originOrientation =
+        SwingTwist16::Inverse(roboid->worldOrigin->orientation);
+    originPosition = roboid->worldOrigin->position;
+  }
+
+  // SwingTwist16 inv_originOrientation =
+  //     SwingTwist16::Inverse(roboid->worldOrigin->orientation);
+  // Spherical16 originPosition = roboid->worldOrigin->position;
+
+  SwingTwist16 worldOrientation = inv_originOrientation * object->orientation;
+  Spherical16 worldPosition =
+      inv_originOrientation * (object->position - originPosition);
 
   unsigned char ix = 0;
   buffer[ix++] = PoseMsg;    // Position2DMsg;
@@ -330,17 +351,6 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
 #endif
 }
 
-// void NetworkSync::SendDestroyThing(Buffer sendBuffer, InterestingThing*
-// thing) {
-//   unsigned char ix = 0;
-//   buffer[ix++] = DestroyMsg;
-//   buffer[ix++] = thing->id;
-//   SendBuffer(ix);
-// #ifdef RC_DEBUG
-//   printf("Sent DestroyThing [%d/%d]", thing->networkId, thing->id);
-// #endif
-// }
-
 void NetworkSync::SendInvestigate(InterestingThing *thing) {
   unsigned char ix = 0;
   buffer[ix++] = InvestigateMsg;

Perception.cpp

@@ -36,9 +36,7 @@ Perception::Perception(Sensor** sensors, unsigned int sensorCount)
     this->trackedObjects[objIx] = nullptr;
 }
 
-unsigned int Perception::GetSensorCount() {
+unsigned int Perception::GetSensorCount() { return this->sensorCount; }
-  return this->sensorCount;
-}
 
 Sensor *Perception::GetSensor(unsigned int sensorId) {
   if (sensorId >= this->sensorCount)
@@ -74,8 +72,7 @@ Sensor* Perception::FindSensorOfType(unsigned int sensorType) {
   return nullptr;
 }
 
-float GetPlaneDistance(InterestingThing* plane,
+float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
-                       float horizontalAngle,
                        float range) {
   float distance = plane->position.distance;
   float deltaAngle =
@@ -137,8 +134,7 @@ float Perception::GetDistance(float horizontalDirection, float range) {
 }
 
 float Perception::GetDistanceOfType(unsigned char thingType,
-                                    float horizontalAngle,
+                                    float horizontalAngle, float range) {
-                                    float range) {
   float minDistance = INFINITY;
   if (range < 0)
     range = -range;
@@ -164,8 +160,7 @@ float Perception::GetDistanceOfType(unsigned char thingType,
 }
 
 float Perception::GetDistance(float horizontalDirection,
-                              float verticalDirection,
+                              float verticalDirection, float range) {
-                              float range) {
   float minDistance = INFINITY;
   if (range < 0)
     range = -range;
@@ -202,12 +197,10 @@ bool Perception::ObjectNearby(float direction, float range) {
   return false;
 }
 
-InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
+InterestingThing *
-                                               Spherical16 position,
+Perception::AddTrackedObject(Sensor *sensor, Spherical16 position,
-                                               SwingTwist16 orientation,
+                             SwingTwist16 orientation, unsigned char thingType,
-                                               unsigned char thingType,
+                             unsigned char thingId, unsigned char networkId) {
-                                               unsigned char thingId,
-                                               unsigned char networkId) {
   InterestingThing *thing = new InterestingThing(sensor, position, orientation);
   if (thingId != 0x00)
     thing->id = thingId;
@@ -439,6 +432,7 @@ void Perception::Update(unsigned long currentTimeMs) {
   }
 }
 
+#include <Arduino.h>
 void Perception::UpdatePose(Polar16 translation) {
   for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
     InterestingThing *thing = trackedObjects[thingIx];
@@ -469,10 +463,18 @@ void Perception::UpdatePose(Polar16 translation) {
       // // obj->position.ProjectOnHorizontalPlane();
       // Spherical16 newPosition = Spherical16(horizontalPosition -
       // translation);
+
       Spherical16 translationS = Spherical16(
           translation.distance, Angle16::Degrees(translation.angle.InDegrees()),
           Angle16::Degrees(0));
       Spherical16 newPosition = thing->position + translationS;
+
+      Vector3 oldPos = thing->position.ToVector3();
+      Vector3 newPos = newPosition.ToVector3();
+      printf(" update percepted position (%f 0 %f) -> (%f 0 %f)\n",
+             oldPos.Right(), oldPos.Forward(), newPos.Right(),
+             newPos.Forward());
+
       thing->position = newPosition;
     }
   }

Roboid.cpp

@@ -21,8 +21,8 @@ Roboid::Roboid() : Thing(0) {
   this->propulsion = nullptr;
   this->networkSync = nullptr;
 
-  this->worldPosition = Spherical16::zero;
+  this->roboidPosition = Spherical16::zero;
-  this->worldOrientation = SwingTwist16::identity;
+  this->roboidOrientation = SwingTwist16::identity;
 }
 
 Roboid::Roboid(Propulsion *propulsion) : Roboid() {
@@ -33,7 +33,6 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() {
 
 void Roboid::SetName(const char *name) { this->name = name; }
 
-#include <Arduino.h>
 void Roboid::Update(unsigned long currentTimeMs) {
   if (perception != nullptr)
     perception->Update(currentTimeMs);
@@ -52,9 +51,12 @@ void Roboid::Update(unsigned long currentTimeMs) {
     this->angularVelocity =
         Spherical16(oldAngular, Angle16(), Angle16::Degrees(90));
 
-    SetPosition(this->position + this->orientation * Spherical16::forward *
+    // SetPosition(this->position + this->orientation * Spherical16::forward *
-                                     this->linearVelocity.distance * deltaTime);
+    //                                  this->linearVelocity.distance *
-    this->worldPosition = this->position; // assuming the roboid is the root
+    //                                  deltaTime);
+    // this->roboidPosition = this->position; // assuming the roboid is the root
+    this->position += this->orientation * Spherical16::forward *
+                      this->linearVelocity.distance * deltaTime;
 
     SwingTwist16 rotation = SwingTwist16::AngleAxis(
         this->angularVelocity.distance * deltaTime, Direction16::up);
@@ -63,7 +65,7 @@ void Roboid::Update(unsigned long currentTimeMs) {
 
     this->orientation = this->orientation * rotation;
 
-    this->worldOrientation =
+    this->roboidOrientation =
         this->orientation; // assuming the roboid is the root
   }
 
@@ -78,20 +80,24 @@ void Roboid::Update(unsigned long currentTimeMs) {
   lastUpdateTimeMs = currentTimeMs;
 }
 
-Spherical16 Roboid::GetPosition() { return this->worldPosition; }
+// Spherical16 Roboid::GetPosition() { return this->roboidPosition; }
 
-SwingTwist16 Roboid::GetOrientation() { return this->worldOrientation; }
+// SwingTwist16 Roboid::GetOrientation() { return this->roboidOrientation; }
 
+/*
 void Roboid::SetPosition(Spherical16 newWorldPosition) {
   SwingTwist16 roboidOrientation = this->GetOrientation();
-  Spherical16 translation = newWorldPosition - this->worldPosition;
+  Spherical16 translation = newWorldPosition - this->roboidPosition;
   float distance = translation.distance;
   Angle16 angle = Spherical16::SignedAngleBetween(
       roboidOrientation * Spherical16::forward, translation, Spherical16::up);
   Polar16 polarTranslation = Polar16(
       distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance));
-  if (perception != nullptr)
+  if (perception != nullptr) {
+    printf("roboid translation %f, %f\n", polarTranslation.distance,
+           polarTranslation.angle.InDegrees());
     perception->UpdatePose(polarTranslation);
+  }
 
   this->position = newWorldPosition; // roboid is the root?
   // World position should be set in the update recursion
@@ -111,7 +117,7 @@ void Roboid::SetOrientation(SwingTwist16 newOrientation) {
 
   this->orientation = newOrientation;
 }
-
+*/
 void Roboid::AddChild(Thing *child) {
   Thing::AddChild(child);
   if (child->IsSensor()) {

Roboid.h

@@ -42,14 +42,14 @@ public:
   /// @details 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 Spherical16 GetPosition();
+  // virtual Spherical16 GetPosition();
   // Vector2 GetPosition2D();
   /// @brief Retrieve the current orientation of the roboid
   /// @return The orientation quaternion in world space
   /// @details 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 SwingTwist16 GetOrientation();
+  // virtual SwingTwist16 GetOrientation();
 
   /// @brief Update the current position of the roboid
   /// @param worldPosition The position of the roboid in carthesian coordinates
@@ -58,31 +58,22 @@ public:
   /// orientations of the perceived objects by the roboid
   /// (roboid->perception->perceivedObjects), as these are local to the
   /// roboid's position.
-  virtual void SetPosition(Spherical16 worldPosition);
+  // virtual void SetPosition(Spherical16 worldPosition);
   /// @brief Update the current orientation of the roboid
   /// @param worldOrientation The orientation of the roboid in world space
   /// @details 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(SwingTwist16 worldOrientation);
+  // virtual void SetOrientation(SwingTwist16 worldOrientation);
 
   virtual void AddChild(Thing *child) override;
   void Release(Thing *child);
 
+  Thing *worldOrigin =
+      nullptr; // thing to track the world origin to be able to transform
+               // world coordinates into roboid or local coordinates
+               // Perhaps this will move to perception at some point
 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;
-  /// @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
-  /// set directly, but SetOrientation should be used instead.
-  // Quaternion worldOrientation = Quaternion::identity;
-
-  // AngleAxis worldAngleAxis = AngleAxis();
-
   unsigned long lastUpdateTimeMs = 0;
 };
 

Thing.h

@@ -43,16 +43,22 @@ public:
   /// @return True when the Thing is a Roboid and False otherwise
   bool IsRoboid();
 
-  /// @brief The position of this Thing
+  /// @brief The position in local space
   /// @remark When this Thing has a parent, the position is relative to the
   /// parent's position and orientation
   Spherical16 position;
-  Spherical16 worldPosition;
+  /// @brief The position in roboid space
-  /// @brief The orientation of this Thing
+  /// @remark This is the position relative to the root of the roboid,
+  /// or the Roboid itself.
+  Spherical16 roboidPosition;
+  /// @brief The orientation in local space
   /// @remark When this Thing has a parent, the orientation is relative to the
   /// parent's orientation
   SwingTwist16 orientation;
-  SwingTwist16 worldOrientation;
+  /// @brief The orientation in roboid space
+  /// @remark This is the orientation relative to the root of the roboid,
+  /// or the Roboid itself.
+  SwingTwist16 roboidOrientation;
 
   virtual Spherical16 GetLinearVelocity();
   virtual Spherical16 GetAngularVelocity();