InterestingThings are now Things

NetworkPerception.cpp

@@ -7,8 +7,7 @@
 #include <Arduino.h>
 #endif
 
-void NetworkPerception::ProcessPacket(Roboid* roboid,
+void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
-                                      unsigned char* buffer,
                                       int packetsize) {
 #if RC_DEBUG
   // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
@@ -16,22 +15,22 @@ void NetworkPerception::ProcessPacket(Roboid* roboid,
 #endif
 
   switch (buffer[0]) {
-    case NetworkSync::CreateMsg:
+  case NetworkSync::CreateMsg:
-      ReceiveCreateMsg(buffer, roboid);
+    ReceiveCreateMsg(buffer, roboid);
-      break;
+    break;
-    case NetworkSync::InvestigateMsg:
+  case NetworkSync::InvestigateMsg:
-      ReceiveInvestigateMsg(buffer, roboid);
+    ReceiveInvestigateMsg(buffer, roboid);
-      break;
+    break;
-    case NetworkSync::PoseMsg:
+  case NetworkSync::PoseMsg:
-      ReceivePoseMsg(buffer, roboid);
+    ReceivePoseMsg(buffer, roboid);
-      break;
+    break;
-    case NetworkSync::PoseTypeMsg:
+  case NetworkSync::PoseTypeMsg:
-      ReceiveTypedObject(buffer, roboid);
+    ReceiveTypedObject(buffer, roboid);
-      break;
+    break;
   }
 }
 
-void NetworkPerception::ReceiveCreateMsg(unsigned char* data, Roboid* roboid) {
+void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char objectId = data[2];
   unsigned char objectType = data[3];
@@ -39,15 +38,15 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char* data, Roboid* roboid) {
     return;
 
   // printf("Received create message [%d/%d]\n", networkId, objectId);
-  InterestingThing* thing =
+  InterestingThing *thing =
       roboid->perception->FindTrackedObject(networkId, objectId);
   if (thing != nullptr) {
     thing->type = objectType;
   }
 }
 
-void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data,
+void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
-                                              Roboid* roboid) {
+                                              Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char objectId = data[2];
 
@@ -64,16 +63,17 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data,
     if (roboid->modelUrl != nullptr)
       roboid->networkSync->SendModel(roboid);
   } else {
-    InterestingThing* thing =
+    InterestingThing *thing =
         roboid->perception->FindTrackedObject(0x00, objectId);
     if (thing == nullptr)
       return;
 
     roboid->networkSync->NewObject(thing);
+    roboid->networkSync->SendModel(thing);
   }
 }
 
-void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) {
+void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char poseType = data[3];
   if ((poseType & NetworkSync::Pose_Position) == 0 ||
@@ -86,7 +86,7 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) {
   Spherical16 roboidPosition = roboid->GetPosition();
   Spherical16 deltaPosition = worldPosition - roboidPosition;
 
-  float distance = deltaPosition.distance;  // magnitude();
+  float distance = deltaPosition.distance; // magnitude();
   if (roboid->perception->IsInteresting(distance) == false) {
     // printf("  plane not interesting\n");
     return;
@@ -100,7 +100,7 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) {
 
   // Polar position = Polar(angle, distance);
   Spherical16 position =
-      localPosition;  // Spherical16::FromVector3(localPosition);
+      localPosition; // Spherical16::FromVector3(localPosition);
 
   // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x,
   //        worldPosition.y, worldPosition.z, roboidPosition.x,
@@ -112,7 +112,7 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) {
                                        0x80, 0x80, networkId);
 }
 
-void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) {
+void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
 
   float radius = ReceiveFloat100(data, 3);
@@ -126,14 +126,14 @@ void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) {
       SwingTwist16::Inverse(roboidOrientation) * deltaPosition;
 
   Spherical16 position =
-      localPosition;  // Spherical16::FromVector3(localPosition);
+      localPosition; // Spherical16::FromVector3(localPosition);
 
   roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
                                        0x81, 0x81, networkId);
 }
 
-void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
+void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
-  if (roboid->networkSync->networkId == 0)  // We're not connected to a site yet
+  if (roboid->networkSync->networkId == 0) // We're not connected to a site yet
     return;
 
   unsigned char networkId = data[1];
@@ -152,7 +152,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
     // printf("Received PoseMsg [%d/%d]\n", networkId, objectId);
 #endif
 
-  InterestingThing* thing =
+  InterestingThing *thing =
       roboid->perception->FindTrackedObject(networkId, objectId);
 
   SwingTwist16 roboidOrientation = roboid->GetOrientation();
@@ -175,7 +175,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
 
       Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
                                   (worldPosition - roboidPosition);
-      position = localPosition;  // Spherical16::FromVector3(localPosition);
+      position = localPosition; // Spherical16::FromVector3(localPosition);
       // printf("  worldPosition (%f %f %f) localPosition (%f %f %f)\n",
       //        worldPosition.Right(), worldPosition.Up(),
       //        worldPosition.Forward(), localPosition.Right(),
@@ -193,7 +193,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
         Angle16::Degrees(worldAngles.Up()),
         Angle16::Degrees(worldAngles.Right()),
         Angle16::Degrees(
-            worldAngles.Forward()));  // Quaternion::Euler(worldAngles);
+            worldAngles.Forward())); // Quaternion::Euler(worldAngles);
     if (objectId == 0) {
       roboid->SetOrientation(worldOrientation);
     } else {
@@ -214,11 +214,11 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
   }
 }
 
-void NetworkPerception::ReceiveTypedObject(unsigned char* data,
+void NetworkPerception::ReceiveTypedObject(unsigned char *data,
-                                           Roboid* roboid) {
+                                           Roboid *roboid) {
   unsigned char objectType = data[1];
   unsigned char objectId = data[2];
-  if (objectType == 0x02) {  // lighthouse
+  if (objectType == 0x02) { // lighthouse
     // We require position and orientation for lighthouses
     if (data[3] !=
         (NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) {
@@ -239,7 +239,7 @@ void NetworkPerception::ReceiveTypedObject(unsigned char* data,
   }
 }
 
-Int32 NetworkPerception::ReceiveInt32(unsigned char* data, int startIndex) {
+Int32 NetworkPerception::ReceiveInt32(unsigned char *data, int startIndex) {
   Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 |
                   (UInt32)(data[startIndex + 2]) << 16 |
                   (UInt32)(data[startIndex + 1]) << 8 |
@@ -247,13 +247,13 @@ Int32 NetworkPerception::ReceiveInt32(unsigned char* data, int startIndex) {
   return a;
 }
 
-float NetworkPerception::ReceiveFloat100(unsigned char* data, int startIndex) {
+float NetworkPerception::ReceiveFloat100(unsigned char *data, int startIndex) {
   Int32 intValue = ReceiveInt32(data, startIndex);
   float f = (float)intValue / 100.0F;
   return f;
 }
 
-Vector3 NetworkPerception::ReceiveVector3(unsigned char* data, int startIndex) {
+Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) {
   float x = ReceiveFloat100(data, startIndex);
   float y = ReceiveFloat100(data, startIndex + 4);
   float z = ReceiveFloat100(data, startIndex + 8);

Thing.cpp

@@ -22,35 +22,25 @@ const unsigned int Thing::UncontrolledMotorType =
     MotorType | (unsigned int)Type::UncontrolledMotor;
 const unsigned int Thing::ServoType = (unsigned int)Type::Servo;
 
-bool Thing::IsMotor() {
+bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; }
-  return (type & Thing::MotorType) != 0;
-}
 
-bool Thing::IsSensor() {
+bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; }
-  return (type & Thing::SensorType) != 0;
-}
 
-bool Thing::IsRoboid() {
+bool Thing::IsRoboid() { return (type & Thing::RoboidType) != 0; }
-  return (type & Thing::RoboidType) != 0;
-}
 
-void Thing::SetModel(const char* url) {
+void Thing::SetModel(const char *url) { this->modelUrl = url; }
-  this->modelUrl = url;
-}
 
-void Thing::SetParent(Thing* parent) {
+void Thing::SetParent(Thing *parent) {
   if (parent == nullptr)
     return;
 
   parent->AddChild(this);
 }
 
-Thing* Thing::GetParent() {
+Thing *Thing::GetParent() { return this->parent; }
-  return this->parent;
-}
 
-void Thing::AddChild(Thing* child) {
+void Thing::AddChild(Thing *child) {
-  Thing** newChildren = new Thing*[this->childCount + 1];
+  Thing **newChildren = new Thing *[this->childCount + 1];
   for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
     newChildren[childIx] = this->children[childIx];
     if (this->children[childIx] == child) {
@@ -70,17 +60,36 @@ void Thing::AddChild(Thing* child) {
   this->childCount++;
 }
 
-Thing* Thing::GetChild(unsigned char childIx) {
+Thing *Thing::GetChild(unsigned char childIx) {
   if (childIx >= 0 && childIx < this->childCount) {
     return this->children[childIx];
   } else
     return nullptr;
 }
 
-Spherical16 Thing::GetLinearVelocity() {
+Thing *Passer::RoboidControl::Thing::RemoveChild(Thing *child) {
-  return this->linearVelocity;
+  Thing **newChildren = new Thing *[this->childCount - 1];
+  unsigned char newChildIx = 0;
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    if (this->children[childIx] != child) {
+      if (newChildIx == this->childCount - 1) { // We did not find the child
+        // stop copying and return nothing
+        delete[] newChildren;
+        return nullptr;
+      } else
+        newChildren[newChildIx++] = this->children[childIx];
+    }
+  }
+
+  child->parent = nullptr;
+
+  delete[] this->children;
+  this->children = newChildren;
+  this->childCount--;
+
+  return child;
 }
 
-Spherical16 Thing::GetAngularVelocity() {
+Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
-  return this->angularVelocity;
+
-}
+Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }

Thing.h

@@ -74,6 +74,7 @@ public:
   /// @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);
 
   /// @brief Sets the location from where the 3D model of this Thing can be
   /// loaded from

TrackedObject.cpp

@@ -2,7 +2,7 @@
 
 #include <math.h>
 
-InterestingThing::InterestingThing(Sensor* sensor, Polar position) {
+InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing(0) {
   this->id = 0;
   this->confidence = maxConfidence;
   this->sensor = sensor;
@@ -12,9 +12,9 @@ InterestingThing::InterestingThing(Sensor* sensor, Polar position) {
   this->updated = true;
 }
 
-InterestingThing::InterestingThing(Sensor* sensor,
+InterestingThing::InterestingThing(Sensor *sensor, Spherical16 position,
-                                   Spherical16 position,
+                                   SwingTwist16 orientation)
-                                   SwingTwist16 orientation) {
+    : Thing(0) {
   this->id = 0;
   this->confidence = maxConfidence;
   this->sensor = sensor;
@@ -22,12 +22,12 @@ InterestingThing::InterestingThing(Sensor* sensor,
   // float angle;
   // Vector3 axis;
   // orientation.ToAngleAxis(&angle, &axis);
-  this->orientation = orientation;  // AngleAxisOf<float>(angle, axis);
+  this->orientation = orientation; // AngleAxisOf<float>(angle, axis);
   this->updated = true;
 }
 
 // #include <Arduino.h>
-bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) {
+bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) {
   if (id != 0 && id == otherObj->id)
     return true;
   if (type != otherObj->type)

TrackedObject.h

@@ -12,14 +12,13 @@ namespace Passer {
 namespace RoboidControl {
 
 /// @brief An object tracked by the roboid
-class InterestingThing {
+class InterestingThing : public Thing {
- public:
+public:
   /// @brief An object tracked by the roboid
   /// @param sensor The Sensor which detected this object
   /// @param position The position in polar coordinates local to the roboid
-  InterestingThing(Sensor* sensor, Polar position);
+  InterestingThing(Sensor *sensor, Polar position);
-  InterestingThing(Sensor* sensor,
+  InterestingThing(Sensor *sensor, Spherical16 position,
-                   Spherical16 position,
                    SwingTwist16 orientation = SwingTwist16::identity);
 
   /// @brief Update the position of the object
@@ -43,7 +42,7 @@ class InterestingThing {
   /// @return Returns true when both objects are considered the same
   /// The result of this check depends on the equalityDistance and equalityAngle
   /// value.
-  bool IsTheSameAs(InterestingThing* otherObj);
+  bool IsTheSameAs(InterestingThing *otherObj);
   /// @brief The maximum difference in distance from the roboid in which two
   /// objects may be considered the same
   /// @details When the difference in distance is exactly this
@@ -64,25 +63,25 @@ class InterestingThing {
 
   unsigned char networkId;
   /// @brief The id of the tracked object
-  unsigned char id;
+  // unsigned char id;
 
   char parentId = 0;
   /// @brief The current position of the object
-  Spherical16 position = Spherical16::zero;
+  // Spherical16 position = Spherical16::zero;
   /// @brief The current orientation of the object
-  SwingTwist16 orientation = SwingTwist16();
+  // SwingTwist16 orientation = SwingTwist16();
-  // Quaternion orientation = Quaternion::identity;
+  //  Quaternion orientation = Quaternion::identity;
   /// @brief The sensor which provided that lastet pose this object
-  Sensor* sensor = nullptr;
+  Sensor *sensor = nullptr;
 
-  unsigned char type = 0x00;
+  // unsigned char type = 0x00;
   unsigned char confidence;
   bool updated = false;
 
- protected:
+protected:
   static constexpr unsigned char maxConfidence = 255;
-  static constexpr unsigned char confidenceDropSpeed = 10;  // 150; // 2;
+  static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2;
 };
 
-}  // namespace RoboidControl
+} // namespace RoboidControl
-}  // namespace Passer
+} // namespace Passer