Roboid name support

NetworkPerception.cpp

@@ -62,7 +62,7 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data,
   if (objectId == 0x00) {
     // They are investigating the roboid itself!
     if (roboid->modelUrl != nullptr)
-      roboid->networkSync->PublishModel(roboid);
+      roboid->networkSync->SendModel(roboid);
   } else {
     InterestingThing* thing =
         roboid->perception->FindTrackedObject(0x00, objectId);
@@ -76,7 +76,7 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char* data,
 void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) {
   unsigned char networkId = data[1];
   unsigned char poseType = data[3];
-  if ((poseType & NetworkSync::Pose_Position) == 0 |
+  if ((poseType & NetworkSync::Pose_Position) == 0 ||
       (poseType & NetworkSync::Pose_Orientation) == 0)
 
     // both position and orientation are required

NetworkSync.cpp

@@ -1,6 +1,6 @@
 #include "NetworkSync.h"
 
-// #define RC_DEBUG 1
+#define RC_DEBUG 1
 
 #ifdef RC_DEBUG
 #include <Arduino.h>
@@ -30,12 +30,14 @@ void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) {
 
 #include <Arduino.h>
 
+// Actually, this is a kind of investigate...
 void NetworkSync::ReceiveNetworkId() {
   this->networkId = buffer[1];
 #ifdef RC_DEBUG
-  printf("_Received network Id %d\n", this->networkId);
+  Serial.printf("_Received network Id %d\n", this->networkId);
 #endif
-  PublishModel(roboid);
+  SendName(roboid);
+  SendModel(roboid);
   for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) {
     Thing* child = roboid->GetChild(childIx);
     if (child != nullptr)
@@ -43,13 +45,12 @@ void NetworkSync::ReceiveNetworkId() {
   }
 }
 
-void NetworkSync::BroadcastState(Roboid *roboid)
+void NetworkSync::PublishState(Roboid* roboid) {
-{
   // if (roboid->updated == false)
   //   return;
 
-  SendPose(roboid);
+  // SendPose(roboid);
-  BroadcastPerception(roboid);
+  // PublishPerception(roboid);
 }
 
 void NetworkSync::NewObject(InterestingThing* thing) {
@@ -83,7 +84,7 @@ void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) {
   SendSpherical16(buffer, &ix, thing->position);
   SendBuffer(ix);
 
-  PublishModel(thing);
+  SendModel(thing);
 
   if (recurse) {
     for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
@@ -94,7 +95,30 @@ void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) {
   }
 }
 
-void NetworkSync::PublishModel(Roboid* roboid) {
+void NetworkSync::SendName(Roboid* roboid) {
+  if (roboid->name == nullptr)
+    return;
+
+  unsigned char len = strlen(roboid->name);
+  if (len > 255)
+    return;
+
+  unsigned char ix = 0;
+  buffer[ix++] = NameMsg;
+  buffer[ix++] = 0x00;  // objectId
+
+  buffer[ix++] = len;
+  for (unsigned char nameIx = 0; nameIx < len; nameIx++)
+    buffer[ix++] = roboid->name[nameIx];
+
+  SendBuffer(ix);
+
+#ifdef RC_DEBUG
+  printf("Sent Name [%d/%d] %s\n", networkId, buffer[1], roboid->name);
+#endif
+}
+
+void NetworkSync::SendModel(Roboid* roboid) {
   if (roboid->modelUrl == nullptr)
     return;
 
@@ -103,7 +127,7 @@ void NetworkSync::PublishModel(Roboid* roboid) {
     return;
 
   unsigned char ix = 0;
-  buffer[ix++] = 0x90;            // modelMsg
+  buffer[ix++] = ModelMsg;
   buffer[ix++] = 0x00;            // objectId
   Spherical s = Spherical::zero;  //(roboid->modelPosition);
   SendSpherical(buffer, &ix, s);
@@ -116,7 +140,7 @@ void NetworkSync::PublishModel(Roboid* roboid) {
   SendBuffer(ix);
 }
 
-void NetworkSync::PublishModel(Thing* thing) {
+void NetworkSync::SendModel(Thing* thing) {
   if (thing->modelUrl == nullptr)
     return;
 
@@ -125,7 +149,7 @@ void NetworkSync::PublishModel(Thing* thing) {
     return;
 
   unsigned char ix = 0;
-  buffer[ix++] = 0x90;            // modelMsg
+  buffer[ix++] = ModelMsg;
   buffer[ix++] = thing->id;       // objectId
   Spherical s = Spherical::zero;  // Spherical(thing->modelPosition);
   SendSpherical(buffer, &ix, s);
@@ -210,7 +234,7 @@ void NetworkSync::PublishClient() {
 #endif
 }
 
-void NetworkSync::BroadcastState(Sensor* sensor) {
+void NetworkSync::PublishState(Sensor* sensor) {
   unsigned char ix = 0;
   buffer[ix++] = StateMsg;
   buffer[ix++] = sensor->type;
@@ -219,17 +243,18 @@ void NetworkSync::BroadcastState(Sensor* sensor) {
   SendBuffer(ix);
 }
 
-void NetworkSync::BroadcastPerception(Roboid *roboid) {
+void NetworkSync::PublishPerception(Roboid* roboid) {
   Perception* perception = roboid->perception;
   if (perception == nullptr)
     return;
 
-  for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount; sensorIx++) {
+  for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
+       sensorIx++) {
     Sensor* sensor = perception->sensors[sensorIx];
     if (sensor == nullptr)
       continue;
-    // sensor->BroadcastState();
+    // sensor->PublishState();
-    BroadcastState(sensor);
+    PublishState(sensor);
   }
   PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects());
 }

NetworkSync.h

@@ -23,8 +23,9 @@ class NetworkSync {
   /// @param obj
   virtual void SendDestroyThing(InterestingThing* obj);
   virtual void NewObject(InterestingThing* obj);
-  virtual void PublishModel(Roboid* obj);
+  void SendName(Roboid* roboid);
-  void PublishModel(Thing* thing);
+  virtual void SendModel(Roboid* obj);
+  void SendModel(Thing* thing);
 
   /// @brief The id of a Pose message
   static const unsigned char PoseMsg = 0x10;
@@ -51,6 +52,8 @@ class NetworkSync {
   static const unsigned char Velocity2DMsg = 0x62;
   static const unsigned char CreateMsg = 0x80;
   static const unsigned char InvestigateMsg = 0x81;
+  static const unsigned char ModelMsg = 0x90;
+  static const unsigned char NameMsg = 0x91;
   static const unsigned char ClientMsg = 0xA0;
   static const unsigned char NetworkIdMsg = 0xA1;
 
@@ -60,7 +63,7 @@ class NetworkSync {
 
   void ReceiveNetworkId();
 
-  void BroadcastState(Roboid* roboid);
+  void PublishState(Roboid* roboid);
 
   void SendInvestigate(InterestingThing* thing);
 
@@ -69,7 +72,7 @@ class NetworkSync {
   // void PublishNewObject();
   void PublishRelativeThing(Thing* thing, bool recurse = false);
 
-  void BroadcastPerception(Roboid *roboid);
+  void PublishPerception(Roboid* roboid);
   void PublishTrackedObjects(Roboid* roboid, InterestingThing** objects);
 
   virtual void SendPosition(Vector3 worldPosition) {};
@@ -84,7 +87,7 @@ class NetworkSync {
   Roboid* roboid;
   NetworkPerception* networkPerception;
 
-  void BroadcastState(Sensor* sensor);
+  void PublishState(Sensor* sensor);
 
   void PublishTrackedObject(Roboid* roboid, InterestingThing* object);
   void PublishRelativeObject(Buffer sendBuffer,

Perception.cpp

@@ -194,93 +194,6 @@ bool Perception::ObjectNearby(float direction, float range) {
   return false;
 }
 
-// #include <WifiSync.h>
-// This function is deprecated
-/*
-void Perception::AddTrackedObject(Sensor* sensor,
-                                  Spherical16 position,
-                                  unsigned char thingType,
-                                  unsigned char networkId) {
-  // Spherical16 sPos =
-  //     Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
-  Quaternion orientation = Quaternion::identity;
-  AddTrackedObject(sensor, sPos, orientation, 0x00, thingType, networkId);
-  /*
-    InterestingThing *obj = new InterestingThing(sensor, position);
-    obj->type = thingType;
-
-    unsigned char farthestObjIx = 0;
-    int availableSlotIx = -1;
-    for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
-      if (this->trackedObjects[objIx] == nullptr) {
-        availableSlotIx = objIx;
-      }
-      // Do we see the same object?
-      else {
-        if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
-  #ifdef RC_DEBUG2
-          // Serial.print((int)this->trackedObjects[objIx]->id);
-          // Serial.println(": update tracked object");
-          printf("%d: update tracked object [/%d]\n", objIx, obj->id);
-  #endif
-
-          this->trackedObjects[objIx]->Refresh(obj->position);
-          this->trackedObjects[objIx]->type = thingType;
-          delete obj;
-          return;
-        }
-        // Is this the fartest object we see?
-        else if (this->trackedObjects[farthestObjIx] == nullptr ||
-                 (this->trackedObjects[objIx]->position.distance >
-                  this->trackedObjects[farthestObjIx]->position.distance)) {
-          farthestObjIx = objIx;
-        }
-      }
-    }
-
-    // Check if an perception slot is available (we currently see less than the
-    // max number of objects)
-    if (availableSlotIx >= 0) { //< maxObjectCount) {
-      // a slot is available
-      this->trackedObjects[availableSlotIx] = obj;
-      obj->networkId = networkId;
-      obj->id = lastObjectId++; // availableSlotIx + 1;
-  #ifdef RC_DEBUG2
-      printf("%d: new tracked object [%d/%d]\n", availableSlotIx,
-  obj->networkId, obj->id); #endif if (roboid->networkSync != nullptr) {
-        roboid->networkSync->NewObject(obj);
-        // ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
-      }
-    }
-    // If this object is closer than the farthest object, then replace it
-    else if (obj->position.distance <
-             this->trackedObjects[farthestObjIx]->position.distance) {
-      delete this->trackedObjects[farthestObjIx];
-      this->trackedObjects[farthestObjIx] = obj;
-      obj->networkId = networkId;
-      obj->id = lastObjectId++; // availableSlotIx + 1;
-  #ifdef RC_DEBUG2
-      // Serial.print((int)obj->id);
-      // Serial.println(": replaced tracked object");
-      printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id);
-  #endif
-      if (roboid->networkSync != nullptr) {
-        roboid->networkSync->NewObject(obj);
-        // ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
-      }
-    } else {
-  #ifdef RC_DEBUG2
-      // Serial.print((int)obj->id);
-      // Serial.println(": delete tracked object");
-      printf("%d: delete tracked object [/%d]\n", -1, obj->id);
-
-  #endif
-      // No available slot, delete trackedobject
-      delete obj;
-    }
-    }
-  */
-
 InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
                                                Spherical16 position,
                                                Quaternion orientation,

Roboid.cpp

@@ -32,6 +32,10 @@ Roboid::Roboid(Propulsion* propulsion) : Roboid() {
     propulsion->roboid = this;
 }
 
+void Passer::RoboidControl::Roboid::SetName(char* name) {
+  this->name = name;
+}
+
 void Roboid::Update(unsigned long currentTimeMs) {
   if (perception != nullptr)
     perception->Update(currentTimeMs);

Roboid.h

@@ -20,6 +20,9 @@ class Roboid : public Thing {
   /// @param propulsion The Propulsion implementation to use for this Roboid
   Roboid(Propulsion* propulsion);
 
+  char* name = nullptr;
+  void SetName(char* name);
+
   /// @brief Update the state of the Roboid
   /// @param currentTimeMs The time in milliseconds when calling this
   /// function

Sensor.h

@@ -15,7 +15,7 @@ public:
   /// @param parent The Thing which should become the parent
   virtual void SetParent(Thing *parent) override;
 
-  virtual void BroadcastState() {};
+  virtual void PublishState() {};
 
   virtual void* GetValue() { return nullptr; };
 };