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],
@@ -70,6 +69,7 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data,
       return;
 
     roboid->networkSync->NewObject(thing);
+    roboid->networkSync->SendModel(thing);
   }
 }
 

Thing.cpp

@@ -22,21 +22,13 @@ 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) {
   if (parent == nullptr)
@@ -45,9 +37,7 @@ void Thing::SetParent(Thing* parent) {
   parent->AddChild(this);
 }
 
-Thing* Thing::GetParent() {
+Thing *Thing::GetParent() { return this->parent; }
-  return this->parent;
-}
 
 void Thing::AddChild(Thing *child) {
   Thing **newChildren = new Thing *[this->childCount + 1];
@@ -77,10 +67,29 @@ Thing* Thing::GetChild(unsigned char childIx) {
     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];
+    }
   }
 
-Spherical16 Thing::GetAngularVelocity() {
+  child->parent = nullptr;
-  return this->angularVelocity;
+
+  delete[] this->children;
+  this->children = newChildren;
+  this->childCount--;
+
+  return child;
 }
+
+Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
+
+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;

TrackedObject.h

@@ -12,14 +12,13 @@ namespace Passer {
 namespace RoboidControl {
 
 /// @brief An object tracked by the roboid
-class InterestingThing {
+class InterestingThing : public Thing {
 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,
+  InterestingThing(Sensor *sensor, Spherical16 position,
-                   Spherical16 position,
                    SwingTwist16 orientation = SwingTwist16::identity);
 
   /// @brief Update the position of the object
@@ -64,18 +63,18 @@ 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;
   /// @brief The sensor which provided that lastet pose this object
   Sensor *sensor = nullptr;
 
-  unsigned char type = 0x00;
+  // unsigned char type = 0x00;
   unsigned char confidence;
   bool updated = false;