Support Destroy and UpdateAll

ControlCore/CoreThing.cpp

@@ -1,5 +1,6 @@
 #include "CoreThing.h"
 
+#include "Participant.h"
 #include <iostream>
 
 CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) {
@@ -8,6 +9,7 @@ CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) {
   this->Init();
 
   int thingId = CoreThing::Add(this);
+
   if (thingId < 0) {
     std::cout << "ERROR: Thing store is full\n";
     this->id = 0; // what to do when we cannot store any more things?
@@ -15,15 +17,17 @@ CoreThing::CoreThing(unsigned char networkId, unsigned char thingType) {
     this->id = thingId;
 }
 
+void CoreThing::Terminate() { CoreThing::Remove(this); }
+
 void CoreThing::Init() {}
 
-void CoreThing::SetName(const char *name) { this->name = name; }
+CoreThing *CoreThing::GetParent() { return this->parent; }
 
 // All things
-CoreThing *CoreThing::allThings[256] = {nullptr};
+CoreThing *CoreThing::allThings[THING_STORE_SIZE] = {nullptr};
 
 CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
-  for (unsigned char ix = 0; ix < 256; ix++) {
+  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
     CoreThing *thing = allThings[ix];
     if (thing == nullptr)
       continue;
@@ -34,13 +38,27 @@ CoreThing *CoreThing::Get(unsigned char networkId, unsigned char thingId) {
 }
 
 int CoreThing::Add(CoreThing *newThing) {
-  for (unsigned char ix = 0; ix < 256; ix++) {
+  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
     CoreThing *thing = allThings[ix];
     if (thing == nullptr) {
       allThings[ix] = newThing;
 
+      // std::cout << " Add new thing " << (int)ix << "\n";
       return ix;
     }
   }
   return -1;
 }
+
+void CoreThing::Remove(CoreThing *thing) { allThings[thing->id] = nullptr; }
+
+void CoreThing::UpdateAll(unsigned long currentTimeMs) {
+  // Not very efficient, but it works for now.
+  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
+    CoreThing *thing = allThings[ix];
+    if (thing != nullptr &&
+        thing->parent == nullptr) { // update all root things
+      thing->Update(currentTimeMs);
+    }
+  }
+}

ControlCore/CoreThing.h

@@ -1,19 +1,25 @@
 #pragma once
+#include <iostream>
 
 namespace Passer {
 namespace Control {
 
+#define THING_STORE_SIZE 256
+// IMPORTANT: values higher than 256 will need to change the CoreThing::id type
+// to 16-bit or higher, breaking the networking protocol!
+
 class CoreThing {
 public:
   // Participant *client;
   unsigned char networkId;
   /// @char The id of the thing
   unsigned char id;
-  // CoreThing *parent;
+  CoreThing *parent;
   /// @brief The type of Thing
   unsigned char type;
   const char *name = nullptr;
   const char *modelUrl = nullptr;
+  float modelScale = 1;
   // protected Sensor sensor;
 
   /// @brief Basic Thing types
@@ -36,20 +42,32 @@ public:
 public:
   CoreThing(unsigned char networkId = 0,
             unsigned char thingType = (unsigned char)Type::Undetermined);
+  /// @brief Terminated thins are no longer updated
+  void Terminate();
 
-  void SetName(const char *name);
+  /// @brief Gets the parent Thing
+  /// @return The parent Thing
+  CoreThing *GetParent();
+
+  /// @brief Updates the state of the thing
+  /// @param currentTimeMs The current clock time in milliseconds
+  virtual void Update(unsigned long currentTimeMs) {};
 
   virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
-
+  virtual void ProcessBytes(unsigned char *bytes) {};
-  // All things
-private:
-  static CoreThing *allThings[];
-
-  static CoreThing *Get(unsigned char networkId, unsigned char thingId);
-  static int Add(CoreThing *thing);
 
 protected:
   virtual void Init();
+
+  //------------ All things
+public:
+  static CoreThing *Get(unsigned char networkId, unsigned char thingId);
+  static int Add(CoreThing *thing);
+  static void Remove(CoreThing *thing);
+  static void UpdateAll(unsigned long currentTimeMs);
+
+private:
+  static CoreThing *allThings[];
 };
 
 } // namespace Control

ControlCore/Messages.cpp

@@ -26,7 +26,7 @@ unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
 bool IMessage::Publish(Participant *participant) {
   return participant->PublishBuffer(Serialize(participant->buffer));
 }
-bool IMessage::Send(Participant *participant) {
+bool IMessage::SendTo(Participant *participant) {
   return participant->SendBuffer(Serialize(participant->buffer));
 }
 
@@ -251,7 +251,6 @@ CustomMsg::CustomMsg(unsigned char networkId, CoreThing *thing) {
   this->thing = thing;
 }
 
-#include <iostream>
 unsigned char CustomMsg::Serialize(unsigned char *buffer) {
   unsigned char ix = this->length;
   this->thing->SendBytes(buffer, &ix);
@@ -271,3 +270,21 @@ CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
 
 // CustomMsg
 #pragma endregion
+
+#pragma region DestroyMsg
+
+DestroyMsg::DestroyMsg(unsigned char networkId, CoreThing *thing) {
+  this->networkId = networkId;
+  this->thingId = thing->id;
+}
+
+unsigned char DestroyMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  return ix;
+}
+
+// DestroyMsg
+#pragma endregion

ControlCore/Messages.h

@@ -18,7 +18,7 @@ public:
   static unsigned char *ReceiveMsg(unsigned char packetSize);
 
   bool Publish(Participant *participant);
-  bool Send(Participant *participant);
+  bool SendTo(Participant *participant);
 };
 
 class ClientMsg : public IMessage {
@@ -147,6 +147,18 @@ public:
   static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize);
 };
 
+class DestroyMsg : public IMessage {
+public:
+  static const unsigned char id = 0x20;
+  static const unsigned length = 3;
+  unsigned char networkId;
+  unsigned char thingId;
+
+  DestroyMsg(unsigned char networkId, CoreThing *thing);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
 } // namespace Control
 } // namespace Passer
 

NetworkSync.cpp

@@ -58,32 +58,17 @@ void NetworkSync::ReceiveNetworkId() {
 }
 
 void NetworkSync::ProcessInvestigateMsg(InvestigateMsg msg) {
-  // #if RC_DEBUG
+#if RC_DEBUG
   printf("Received InvestigateMsg [%d/%d]\n", msg.networkId, msg.thingId);
-  // #endif
+#endif
 
-  if (networkId != roboid->networkSync->networkId)
+  if (msg.networkId != this->networkId)
-    // We only response to investigation requests for our own objects
+    // We only respond to investigation requests for our own objects
     return;
 
-  if (msg.thingId == 0x00) {
+  Thing *thing = (Thing *)Thing::Get(0, msg.thingId);
-    // They are investigating the roboid itself!
-    if (roboid->modelUrl != nullptr)
-      // roboid->networkSync->SendModel(roboid);
-      roboid->networkSync->SendThingInfo(roboid);
-  } else {
-    Thing *thing = roboid->FindChild(msg.thingId, true);
   if (thing != nullptr)
-      roboid->networkSync->SendThingInfo(thing);
+    this->SendThingInfo(thing);
-    else {
-      InterestingThing *interestingThing =
-          roboid->perception->FindTrackedObject(0x00, msg.thingId);
-      if (interestingThing == nullptr)
-        return;
-
-      roboid->networkSync->SendThingInfo(interestingThing, false);
-    }
-  }
 }
 
 void NetworkSync::ProcessThingMsg(ThingMsg msg) {
@@ -133,7 +118,7 @@ void NetworkSync::SendThing(Thing *thing) {
   Thing *parent = thing->GetParent();
   ThingMsg msg = ThingMsg(this->networkId, thing->id, thing->type,
                           parent == nullptr ? 0 : parent->id);
-  msg.Send(this);
+  msg.SendTo(this);
 
 #ifdef RC_DEBUG
   printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type);
@@ -149,7 +134,7 @@ void NetworkSync::SendName(Thing *thing) {
     return;
 
   NameMsg msg = NameMsg(this->networkId, thing->id, thing->name, len);
-  msg.Send(this);
+  msg.SendTo(this);
 
 #ifdef RC_DEBUG
   SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1],
@@ -167,12 +152,20 @@ void NetworkSync::SendModel(Thing *thing) {
 
   ModelUrlMsg msg = ModelUrlMsg(this->networkId, thing->id, len,
                                 thing->modelUrl, thing->modelScale);
-  msg.Send(this);
+  msg.SendTo(this);
 }
 
 void NetworkSync::SendCustom(Thing *thing) {
   CustomMsg msg = CustomMsg(this->networkId, thing);
-  msg.Send(this);
+  msg.SendTo(this);
+}
+
+void NetworkSync::SendDestroy(Thing *thing) {
+  DestroyMsg msg = DestroyMsg(this->networkId, thing);
+  msg.SendTo(this);
+#if RC_DEBUG
+  printf("Sent DestroyMsg [%d/%d]\n", thing->networkId, thing->id);
+#endif
 }
 
 #include <iostream>
@@ -183,21 +176,6 @@ void NetworkSync::ProcessCustomMsg(CustomMsg msg) {
     thing->ProcessBytes(msg.data);
 }
 
-void NetworkSync::SendDestroyThing(InterestingThing *thing) {
-  if (networkId == 0) // We're not connected to a site yet
-    return;
-
-  unsigned char ix = 0;
-  buffer[ix++] = DestroyMsg;
-  buffer[ix++] = this->networkId;
-  buffer[ix++] = thing->id;
-  SendBuffer(ix);
-
-#if RC_DEBUG
-  printf("Sent DestroyMsg [%d/%d]\n", thing->networkId, thing->id);
-#endif
-}
-
 void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
   if (this->networkId == 0) // We're not connected to a site yet
     return;
@@ -212,13 +190,13 @@ void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
       poseType |= PoseMsg::Pose_Orientation;
     PoseMsg msg = PoseMsg(this->networkId, thing->id, poseType, thing->position,
                           thing->orientation);
-    msg.Send(this);
+    msg.SendTo(this);
 
     thing->positionUpdated = false;
     thing->orientationUpdated = false;
 
 #if RC_DEBUG
-    if (thing->id == 0) {
+    if (thing->id != 0) {
       Vector3 v = thing->position.ToVector3();
       printf("Sent PoseMsg Thing [%d/%d] %f(%f)-(%f %f %f) %f\n",
              this->networkId, thing->id, thing->position.distance,

NetworkSync.h

@@ -19,17 +19,14 @@ public:
   /// @brief Retreive and send the roboid state
   /// @param roboid The roboid for which the state is updated
   virtual void NetworkUpdate(Roboid *roboid) = 0;
-  /// @brief Inform that the given object is no longer being tracked
-  /// @param obj
-  virtual void SendDestroyThing(InterestingThing *obj);
-
   void SendThingInfo(Thing *thing, bool recurse = false);
   void SendThing(Thing *thing);
   void SendName(Thing *roboid);
   void SendModel(Thing *thing);
   void SendCustom(Thing *thing);
-
+  /// @brief Inform that the given object is no longer being tracked
-  static const unsigned char DestroyMsg = 0x20;
+  /// @param obj
+  void SendDestroy(Thing *thing);
 
   typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
 

Perception.cpp

@@ -481,7 +481,7 @@ void Perception::Update(unsigned long currentTimeMs) {
     if (thing->DegradeConfidence(deltaTime) == false) {
       // delete obj
       if (roboid != nullptr && roboid->networkSync != nullptr) {
-        roboid->networkSync->SendDestroyThing(thing);
+        roboid->networkSync->SendDestroy(thing);
       }
       this->trackedObjects[objIx] = nullptr;
       delete thing;

Sensor.cpp

@@ -8,7 +8,8 @@ Sensor::Sensor() : Thing() { // for now, id should be set properly later
 
 void Sensor::SetParent(Thing *parent) {
   this->parent = parent;
-  if (this->parent->IsRoboid()) {
+  if (this->parent->parent == nullptr) { // Is the parent a root object?
+                                         // Then it is a roboid
     Roboid *roboidParent = (Roboid *)this->parent;
     roboidParent->perception->AddSensor(this);
   }

Thing.cpp

@@ -4,7 +4,7 @@
 
 using namespace Passer::RoboidControl;
 
-int Thing::lastThingId = 1;
+// int Thing::lastThingId = 1;
 
 Thing::Thing() : CoreThing() {
   // this->id = lastThingId++;
@@ -42,7 +42,7 @@ void Thing::SetParent(Thing *parent) {
   parent->AddChild(this);
 }
 
-Thing *Thing::GetParent() { return this->parent; }
+Thing *Thing::GetParent() { return (Thing *)this->parent; }
 
 void Thing::AddChild(Thing *child) {
   Thing **newChildren = new Thing *[this->childCount + 1];
@@ -147,3 +147,20 @@ void Passer::RoboidControl::Thing::SetOrientation(SwingTwist16 orientation) {
 Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
 
 Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }
+
+// Thing *Thing::Get(unsigned char networkId, unsigned char thingId) {
+//   CoreThing *coreThing = CoreThing::Get(networkId, thingId);
+//   // All CoreThings are Things in this context, so we can cast directly
+//   Thing *thing = (Thing *)coreThing;
+//   return thing;
+// }
+
+// int Thing::Add(Thing *thing) { return CoreThing::Add(thing); }
+
+// void Thing::UpdateAll(unsigned long currentTimeMs) {
+//   for (unsigned char ix = 0; ix < allThingsSize; ix++) {
+//     Thing *thing = Get(0, ix);
+//     if (thing != nullptr)
+//       thing->Update(currentTimeMs);
+//   }
+// }

Thing.h

@@ -93,20 +93,9 @@ public:
   /// the only official supported model format is .obj
   void SetModel(const char *url);
 
-  /// @brief Updates the state of the thing
-  /// @param currentTimeMs The current clock time in milliseconds
-  virtual void Update(unsigned long currentTimeMs) {}
-
   unsigned char childCount = 0;
 
-  float modelScale = 1;
-
-    virtual void ProcessBytes(unsigned char *bytes) {
-    std::cout << "no processing\n";
-  };
-
 protected:
-  static int lastThingId;
   /// @brief Bitmask for Motor type
   static const unsigned int MotorType = 0x8000;
   /// @brief Bitmap for Sensor type
@@ -115,7 +104,7 @@ protected:
   static const unsigned int RoboidType = 0x2000;
   static const unsigned char CustomType = 0x80;
 
-  Thing *parent = nullptr;
+  // Thing *parent = nullptr;
   Thing **children = nullptr;
 
 public:

TrackedObject.cpp

@@ -2,20 +2,21 @@
 
 #include <math.h>
 
-InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing() {
+// InterestingThing::InterestingThing(Sensor *sensor, Polar position) : Thing()
-  this->id = 0;
+// {
-  this->confidence = maxConfidence;
+//   this->id = 0;
-  this->sensor = sensor;
+//   this->confidence = maxConfidence;
-  this->position = Spherical16(position.distance,
+//   this->sensor = sensor;
-                               Angle16::Degrees(position.angle.InDegrees()),
+//   this->position = Spherical16(position.distance,
-                               Angle16::Degrees(0));
+//                                Angle16::Degrees(position.angle.InDegrees()),
-  this->updated = true;
+//                                Angle16::Degrees(0));
-}
+//   this->updated = true;
+// }
 
 InterestingThing::InterestingThing(Sensor *sensor, Spherical16 position,
                                    SwingTwist16 orientation)
     : Thing() {
-  this->id = 0;
+  // this->id = 0;
   this->confidence = maxConfidence;
   this->sensor = sensor;
   this->position = position;
@@ -73,13 +74,6 @@ bool InterestingThing::DegradeConfidence(float deltaTime) {
   }
 }
 
-// void InterestingThing::Refresh(Polar position) {
-//   this->position =
-//       Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
-//   this->confidence = maxConfidence;
-//   this->updated = true;
-// }
-
 void InterestingThing::Refresh(Spherical16 position, SwingTwist16 orientation) {
   this->position = position;
   this->orientation = orientation;

TrackedObject.h

@@ -17,7 +17,7 @@ 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, Spherical16 position,
                    SwingTwist16 orientation = SwingTwist16::identity);
 
@@ -61,11 +61,11 @@ public:
   /// coordinated position->angle = 30 and position->angle = 27 can be the same.
   static constexpr float equalityAngle = 5.0F;
 
-  unsigned char networkId;
+  // unsigned char networkId;
   /// @brief The id of the tracked object
   // unsigned char id;
 
-  char parentId = 0;
+  // char parentId = 0;
   /// @brief The current position of the object
   // Spherical16 position = Spherical16::zero;
   /// @brief The current orientation of the object