Improved interestingThings

NetworkPerception.cpp

@@ -71,8 +71,9 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
       return;
 
     // roboid->networkSync->NewObject(thing);
-    roboid->networkSync->SendThing(thing);
+    // roboid->networkSync->SendThing(thing);
-    roboid->networkSync->SendModel(thing);
+    // roboid->networkSync->SendModel(thing);
+    roboid->networkSync->SendThingInfo(thing, false);
   }
 }
 

NetworkSync.cpp

@@ -174,7 +174,6 @@ void NetworkSync::SendDestroyThing(InterestingThing *thing) {
 #endif
 }
 
-#include <Arduino.h>
 void NetworkSync::SendPose(Thing *thing, bool recurse) {
   if (this->networkId == 0) // We're not connected to a site yet
     return;
@@ -198,7 +197,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
     thing->positionUpdated = false;
     thing->orientationUpdated = false;
 
-    // #if RC_DEBUG
+#if RC_DEBUG
     if (thing->id == 0) {
       Vector3 v = thing->position.ToVector3();
       printf("Sent PoseMsg Thing [%d/%d] %f(%f)-(%f %f %f) %f\n",
@@ -207,7 +206,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
              v.Up(), v.Forward(),
              thing->orientation.swing.horizontal.InDegrees());
     }
-    // #endif
+#endif
   }
 
   if (recurse) {
@@ -265,12 +264,11 @@ void NetworkSync::PublishTrackedObjects(Roboid *roboid,
 }
 
 void NetworkSync::PublishTrackedObject(Roboid *roboid,
-                                       InterestingThing *object) {
+                                       InterestingThing *thing) {
-  if (object == nullptr || object->updated == false ||
+  if (thing == nullptr || thing->updated == false || thing->networkId != 0x00) {
-      object->networkId != 0x00) {
     return;
   }
-
+  printf("publish tracked\n");
   /*
     Spherical16 roboidPosition = roboid->GetPosition();
     SwingTwist16 roboidOrientation = roboid->GetOrientation();
@@ -295,23 +293,23 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
   //     SwingTwist16::Inverse(roboid->worldOrigin->orientation);
   // Spherical16 originPosition = roboid->worldOrigin->position;
 
-  SwingTwist16 worldOrientation = inv_originOrientation * object->orientation;
+  SwingTwist16 worldOrientation = inv_originOrientation * thing->orientation;
   Spherical16 worldPosition =
-      inv_originOrientation * (object->position - originPosition);
+      inv_originOrientation * (thing->position - originPosition);
 
   unsigned char ix = 0;
   buffer[ix++] = PoseMsg;   // Position2DMsg;
-  buffer[ix++] = object->id; // objectId;
+  buffer[ix++] = thing->id; // objectId;
   buffer[ix++] = Pose_Position | Pose_Orientation;
   SendSpherical16(buffer, &ix, worldPosition);
   SendSwingTwist(buffer, &ix, worldOrientation);
   SendBuffer(ix);
 
-#if RC_DEBUG
+  // #if RC_DEBUG
-  printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], object->type);
+  printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], thing->type);
-#endif
+  // #endif
 
-  object->updated = false;
+  thing->updated = false;
 }
 
 void NetworkSync::SendInvestigate(InterestingThing *thing) {

Perception.cpp

@@ -273,6 +273,63 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
   return thing;
 }
 
+InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
+                                               Thing *orgThing) {
+  InterestingThing *thing =
+      new InterestingThing(sensor, orgThing->position, orgThing->orientation);
+  thing->id = orgThing->id;
+  thing->networkId = 0;
+  thing->type = orgThing->type;
+  thing->name = orgThing->name;
+  thing->modelUrl = orgThing->modelUrl;
+  thing->modelScale = orgThing->modelScale;
+
+  unsigned char farthestObjIx = 0;
+  unsigned char availableSlotIx = 0;
+  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
+    if (this->trackedObjects[thingIx] == nullptr) {
+      availableSlotIx = thingIx;
+    }
+    // Do we see the same object?
+    else {
+      if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
+        this->trackedObjects[thingIx]->Refresh(
+            thing->position, thing->orientation); //.ToQuaternion());
+        delete thing;
+
+        return this->trackedObjects[thingIx];
+      }
+      // Is this the fartest object we see?
+      else if (this->trackedObjects[farthestObjIx] == nullptr ||
+               (this->trackedObjects[thingIx]->position.distance >
+                this->trackedObjects[farthestObjIx]->position.distance)) {
+        farthestObjIx = thingIx;
+      }
+    }
+  }
+
+  // Check if an perception slot is available (we currently see less than the
+  // max number of objects)
+  if (availableSlotIx < maxObjectCount) {
+    // a slot is available
+    this->trackedObjects[availableSlotIx] = thing;
+    if (thing->id == 0x00)
+      thing->id = lastObjectId++; // availableSlotIx + 1;
+    return thing;
+  }
+  // If this object is closer than the farthest object, then replace it
+  else if (thing->position.distance <
+           this->trackedObjects[farthestObjIx]->position.distance) {
+    delete this->trackedObjects[farthestObjIx];
+    this->trackedObjects[farthestObjIx] = thing;
+    if (thing->id == 0x00)
+      thing->id = lastObjectId++; // availableSlotIx + 1;
+    return thing;
+  } else {
+    return nullptr;
+  }
+}
+
 bool Perception::IsInteresting(float distance) {
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
     InterestingThing *thing = this->trackedObjects[objIx];

Perception.h

@@ -15,38 +15,37 @@ class Roboid;
 
 /// @brief Module to which keeps track of objects around the roboid
 class Perception {
- public:
+public:
   /// @brief Default Constructor
   Perception();
 
   /// @brief Create a perception setup with the given Sensors
   /// @param sensors The Placement of Sensors on the Roboid
   /// @param sensorCount The number of sensors in the placement array
-  Perception(Sensor** sensors, unsigned int sensorCount);
+  Perception(Sensor **sensors, unsigned int sensorCount);
 
   /// @brief The roboid of this perception system
-  Roboid* roboid = nullptr;
+  Roboid *roboid = nullptr;
 
-  unsigned int AddSensor(Sensor* sensor);
+  unsigned int AddSensor(Sensor *sensor);
 
   /// @brief Get the number of Sensors
   /// @return The number of sensors, zero when no sensors are present
   unsigned int GetSensorCount();
-  Sensor* GetSensor(unsigned int sensorId);
+  Sensor *GetSensor(unsigned int sensorId);
   /// @brief Find the first sensor of the given type
   /// @param sensorType The type of sensor as is defined in the Thing class (for
   /// example Thing::SensorType)
   /// @return The first sensor found or a nullptr which no sensor has been found
   /// of the given type
-  Sensor* FindSensorOfType(unsigned int sensorType);
+  Sensor *FindSensorOfType(unsigned int sensorType);
 
   /// @brief Gets the distance to the closest object
   /// @param direction The direction to look for objects
   /// @param range The range in which objects should be looked for
   /// @return The distance to the closest object in meters
   float GetDistance(float direction, float range = 10.0F);
-  float GetDistanceOfType(unsigned char thingType,
+  float GetDistanceOfType(unsigned char thingType, float horizontalAngle,
-                          float horizontalAngle,
                           float range = 10.0F);
   /// @brief Gets the distance to the closest object
   /// @param horizontalDirection The direction in the horizontal plane to look
@@ -57,8 +56,7 @@ class Perception {
   /// @return The distance to the closest object in meters
   /// @details The directions can be thought of as the polar angle (vertical)
   /// and azimuthal angle (horizontal) in the spherical coordinate system.
-  float GetDistance(float horizontalDirection,
+  float GetDistance(float horizontalDirection, float verticalDirection,
-                    float verticalDirection,
                     float range = 10.0F);
 
   /// @brief Checks if an object is nearby
@@ -93,25 +91,23 @@ class Perception {
   //                         Spherical16 position,
   //                         unsigned char objectType = 0x00,
   //                         unsigned char networkId = 0x00);
-  InterestingThing* AddTrackedObject(
+  InterestingThing *
-      Sensor* sensor,
+  AddTrackedObject(Sensor *sensor, Spherical16 position,
-      Spherical16 position,
                    SwingTwist16 orientation = SwingTwist16::identity,
                    unsigned char objectType = 0xFF,
                    unsigned char objectId = 0x00,
                    unsigned char networkId = 0x00);
 
-  InterestingThing* AddTrackedObject(
+  InterestingThing *
-      Sensor* sensor,
+  AddTrackedObject(Sensor *sensor, unsigned char networkId,
-      unsigned char networkId,
+                   unsigned char objectId, Spherical16 position,
-      unsigned char objectId,
-      Spherical16 position,
                    SwingTwist16 orientation = SwingTwist16::identity);
+  InterestingThing *AddTrackedObject(Sensor *sensor, Thing *thing);
 
   bool IsInteresting(float distance);
 
-  InterestingThing* FindTrackedObject(char objectId);
+  InterestingThing *FindTrackedObject(char objectId);
-  InterestingThing* FindTrackedObject(unsigned char networkId,
+  InterestingThing *FindTrackedObject(unsigned char networkId,
                                       unsigned char objectId);
 
   /// @brief Retrieve the number of objects currently being tracked by the
@@ -124,14 +120,14 @@ class Perception {
   /// @details The returned array this should never be a nullptr, but
   /// each array entry may be a nullptr when less than maxObjectCount objects is
   /// currently being tracked.
-  InterestingThing** GetTrackedObjects();
+  InterestingThing **GetTrackedObjects();
 
   unsigned char ThingsOfType(unsigned char objectType,
-                             InterestingThing* buffer[],
+                             InterestingThing *buffer[],
                              unsigned char bufferSize);
-  InterestingThing* ThingOfType(unsigned char objectType);
+  InterestingThing *ThingOfType(unsigned char objectType);
 
-  InterestingThing* GetMostInterestingThing();
+  InterestingThing *GetMostInterestingThing();
 
   // mainly used for confidence update
 
@@ -161,9 +157,9 @@ class Perception {
   /// objects
   float nearbyDistance = 0.02F;
 
- public:
+public:
   /// @brief The Sensors used for Perception
-  Sensor** sensors = nullptr;
+  Sensor **sensors = nullptr;
   /// @brief The number of Sensors used for Perception
   unsigned int sensorCount = 0;
 
@@ -173,7 +169,7 @@ class Perception {
   static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
                                        // number of object which can
                                        // be tracked by a human
-  InterestingThing** trackedObjects;
+  InterestingThing **trackedObjects;
 };
 
 } // namespace RoboidControl

Roboid.cpp

@@ -108,10 +108,14 @@ void Roboid::AddChild(Thing *child) {
   }
 }
 
+#include <Arduino.h>
 void Passer::RoboidControl::Roboid::Release(Thing *child) {
-  if (RemoveChild(child) != nullptr)
+  if (RemoveChild(child) != nullptr) {
-    this->perception->AddTrackedObject(nullptr, Spherical16::zero,
+    child->position = this->position;
-                                       SwingTwist16::identity, child->type);
+    child->orientation = this->orientation;
+    printf("obj distance %f\n", child->position.distance);
+    this->perception->AddTrackedObject(nullptr, child);
+  }
 }
 
 void Roboid::LoadModel(const char *url) {

Thing.cpp

@@ -66,8 +66,9 @@ void Thing::AddChild(Thing *child) {
 }
 
 Thing *Thing::RemoveChild(Thing *child) {
-  int newChildCount = this->childCount - 1;
+  unsigned char newChildCount = this->childCount - 1;
   Thing **newChildren = new Thing *[newChildCount];
+
   unsigned char newChildIx = 0;
   for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
     if (this->children[childIx] != child) {