Experimental Publish model support

NetworkPerception.cpp

@@ -56,12 +56,18 @@ void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
     // We only response to investigation requests for our own objects
     return;
 
-  InterestingThing *thing =
-      roboid->perception->FindTrackedObject(0x00, objectId);
-  if (thing == nullptr)
-    return;
+  if (objectId == 0x00) {
+    // They are investigating the roboid itself!
+    if (roboid->modelUrl != nullptr)
+      roboid->networkSync->PublishModel(roboid);
+  } else {
+    InterestingThing *thing =
+        roboid->perception->FindTrackedObject(0x00, objectId);
+    if (thing == nullptr)
+      return;
 
-  roboid->networkSync->NewObject(thing);
+    roboid->networkSync->NewObject(thing);
+  }
 }
 
 void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {

NetworkSync.cpp

@@ -10,6 +10,8 @@
 #include "LinearAlgebra/Spherical.h"
 #include "float16/float16.h"
 
+#include <string.h>
+
 NetworkSync::NetworkSync(Roboid *roboid) { this->roboid = roboid; }
 
 void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
@@ -47,6 +49,29 @@ void NetworkSync::NewObject(InterestingThing *thing) {
   // PublishTrackedObject(roboid, obj);
 }
 
+void NetworkSync::PublishModel(Roboid *roboid) {
+  int len = strlen(roboid->modelUrl);
+  if (len > 255)
+    return;
+
+  unsigned char ix = 0;
+  buffer[ix++] = 0x90; // modelMsg
+  buffer[ix++] = 0x00; // objectId
+  // SendVector3(buffer, &ix, position);
+  // Polar polar = Polar(Vector2(position));
+  // SendPolar(buffer, &ix, polar);
+  Spherical s = Spherical(roboid->modelPosition);
+  SendSpherical(buffer, &ix, s);
+  SendFloat16(buffer, &ix, roboid->modelScale);
+
+  buffer[ix++] = len;
+  // printf("send url %d, scale = %f \n", ix, sscale);
+  for (int urlIx = 0; urlIx < len; urlIx++) {
+    buffer[ix++] = roboid->modelUrl[urlIx];
+  }
+  SendBuffer(ix);
+}
+
 void NetworkSync::DestroyObject(InterestingThing *thing) {
   unsigned char ix = 0;
   buffer[ix++] = DestroyMsg;
@@ -58,6 +83,30 @@ void NetworkSync::DestroyObject(InterestingThing *thing) {
 #endif
 }
 
+void NetworkSync::SendPose(Roboid *roboid) {
+  if (roboid->propulsion == nullptr)
+    return;
+
+  Polar velocity = roboid->propulsion->GetVelocity();
+  Vector2 worldVelocity2 =
+      Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
+  Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y);
+
+  float angularVelocity = roboid->propulsion->GetAngularVelocity();
+  Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);
+
+  unsigned char buffer[3 + 12 + 12] = {
+      PoseMsg,
+      0, // objectId;
+      Pose_LinearVelocity | Pose_AngularVelocity,
+  };
+  unsigned char ix = 3;
+  SendVector3(buffer, &ix, worldVelocity3);
+  SendVector3(buffer, &ix, worldAngularVelocity);
+
+  SendBuffer(ix);
+}
+
 void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
                               const Vector3 v) {
   SendSingle100(data, *startIndex, v.Right());
@@ -191,66 +240,119 @@ void NetworkSync::PublishClient() {
 #endif
 }
 
-void NetworkSync::PublishTrackedObjects(Buffer sendBuffer,
+void NetworkSync::PublishTrackedObjects(Roboid *roboid,
                                         InterestingThing **objects) {
+  int n = 0;
   for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
     InterestingThing *obj = objects[objIx];
     if (obj == nullptr)
       continue;
-    if (obj->networkId != 0x00)
-      continue; // object is external
-    // if (obj->sensor->type == Thing::ExternalType)
-    //   continue;
+    if (obj->sensor->type == Thing::ExternalType)
+      continue;
 
-    // tmp
-    obj->id = objIx;
-    PublishTrackedObject(sendBuffer, obj);
+    if (obj->confidence > 0) {
+      PublishTrackedObject(roboid, obj);
+    }
+    n++;
   }
 }
 
-void NetworkSync::PublishTrackedObject(Buffer sendBuffer,
+void NetworkSync::PublishTrackedObject(Roboid *roboid,
                                        InterestingThing *object) {
-  Vector2 worldPosition2 =
-      Vector2::Rotate(Vector2::forward * object->position.distance,
-                      -object->position.horizontalAngle);
-  Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
+  if (object == nullptr || object->updated == false ||
+      object->networkId != 0x00) {
+    return;
+  }
 
-#ifdef RC_DEBUG
-  Serial.print("Send Pose ");
-  Serial.print((int)object->id);
-  Serial.print("  Position  ");
-  Serial.print(worldPosition3.Right());
-  Serial.print(", ");
-  Serial.print(worldPosition3.Up());
-  Serial.print(", ");
-  Serial.println(worldPosition3.Forward());
-#else
-  const UInt16 bufferSize = 3 + 12;
-  UInt8 buffer[bufferSize] = {
-      PoseMsg,
-      (UInt8)object->id,
-      Pose_Position,
-  };
-  unsigned char ix = 3;
-  SendVector3(buffer, &ix, worldPosition3);
-  sendBuffer(buffer, bufferSize);
+  // if (object->parentId != 0)
+  //   return PublishRelativeObject(object);
+
+  Vector3 roboidPosition = roboid->GetPosition();
+  Quaternion roboidOrientation = roboid->GetOrientation();
+
+  // Vector3 localPosition = object->position.ToVector3();
+  Vector3 localPosition = Vector3(object->position);
+  Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition;
+  Quaternion worldOrientation = roboidOrientation * object->orientation;
+
+  unsigned char ix = 0;
+  buffer[ix++] = PoseMsg;    // Position2DMsg;
+  buffer[ix++] = object->id; // objectId;
+  buffer[ix++] = Pose_Position | Pose_Orientation;
+  // SendPolar(buffer, &ix, polar); // 3 bytes
+  SendVector3(buffer, &ix, worldPosition);
+  // SendQuat32(buffer, &ix, worldOrientation);
+  SendBuffer(ix);
+
+  object->updated = false;
+
+#if DEBUG
+  printf("PublishTrackedObj [%d/%d] (%f %f)\n", object->networkId, buffer[1],
+         worldPosition.Right(), worldPosition.Forward());
 #endif
 }
 
-void NetworkSync::PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
-                                        InterestingThing *object) {
-  const UInt16 bufferSize = 4 + 12;
-  UInt8 buffer[bufferSize] = {
-      RelativePoseMsg,
-      (UInt8)parentId,
-      (UInt8)object->id,
-      Pose_Position,
-  };
-  unsigned char ix = 4;
-  SendSpherical(buffer, &ix, object->position);
-  // SendVector3(buffer, ix, worldPosition3);
-  sendBuffer(buffer, bufferSize);
-}
+// void NetworkSync::PublishTrackedObjects(Buffer sendBuffer,
+//                                         InterestingThing **objects) {
+//   for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++)
+//   {
+//     InterestingThing *obj = objects[objIx];
+//     if (obj == nullptr)
+//       continue;
+//     if (obj->networkId != 0x00)
+//       continue; // object is external
+//     // if (obj->sensor->type == Thing::ExternalType)
+//     //   continue;
+
+//     // tmp
+//     obj->id = objIx;
+//     PublishTrackedObject(sendBuffer, obj);
+//   }
+// }
+
+// void NetworkSync::PublishTrackedObject(Buffer sendBuffer,
+//                                        InterestingThing *object) {
+//   Vector2 worldPosition2 =
+//       Vector2::Rotate(Vector2::forward * object->position.distance,
+//                       -object->position.horizontalAngle);
+//   Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
+
+// #ifdef RC_DEBUG
+//   Serial.print("Send Pose ");
+//   Serial.print((int)object->id);
+//   Serial.print("  Position  ");
+//   Serial.print(worldPosition3.Right());
+//   Serial.print(", ");
+//   Serial.print(worldPosition3.Up());
+//   Serial.print(", ");
+//   Serial.println(worldPosition3.Forward());
+// #else
+//   const UInt16 bufferSize = 3 + 12;
+//   UInt8 buffer[bufferSize] = {
+//       PoseMsg,
+//       (UInt8)object->id,
+//       Pose_Position,
+//   };
+//   unsigned char ix = 3;
+//   SendVector3(buffer, &ix, worldPosition3);
+//   sendBuffer(buffer, bufferSize);
+// #endif
+// }
+
+// void NetworkSync::PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
+//                                         InterestingThing *object) {
+//   const UInt16 bufferSize = 4 + 12;
+//   UInt8 buffer[bufferSize] = {
+//       RelativePoseMsg,
+//       (UInt8)parentId,
+//       (UInt8)object->id,
+//       Pose_Position,
+//   };
+//   unsigned char ix = 4;
+//   SendSpherical(buffer, &ix, object->position);
+//   // SendVector3(buffer, ix, worldPosition3);
+//   sendBuffer(buffer, bufferSize);
+// }
 
 void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
   Polar velocity = roboid->propulsion->GetVelocity();
@@ -308,4 +410,22 @@ void NetworkSync::SendInvestigateThing(InterestingThing *thing) {
   buffer[ix++] = thing->networkId;
   buffer[ix++] = thing->id;
   SendBuffer(ix);
+}
+
+void NetworkSync::SendText(const char *s) {
+  unsigned char length;
+  for (length = 0; length < 253; length++) {
+    if (s[length] == '\0')
+      break;
+  }
+  if (length >= 253)
+    return;
+
+  unsigned char ix;
+  buffer[ix++] = 0xB0;
+  buffer[ix++] = length;
+  for (int urlIx = 0; urlIx < length; urlIx++)
+    buffer[ix++] = s[urlIx];
+
+  SendBuffer(ix);
 }

NetworkSync.h

@@ -22,6 +22,7 @@ public:
   /// @param obj
   virtual void DestroyObject(InterestingThing *obj);
   virtual void NewObject(InterestingThing *obj);
+  virtual void PublishModel(Roboid *obj);
 
   /// @brief The id of a Pose message
   static const unsigned char PoseMsg = 0x10;
@@ -62,16 +63,19 @@ public:
   void SendDestroyObject(Buffer sendBuffer, InterestingThing *obj);
   void PublishNewObject();
 
-  void PublishTrackedObjects(Buffer sendBuffer, InterestingThing **objects);
+  void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects);
 
   virtual void SendPosition(Vector3 worldPosition) {};
   virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
+  void SendPose(Roboid *roboid);
+
+  void SendText(const char *s);
 
 protected:
   Roboid *roboid;
   NetworkPerception *networkPerception;
 
-  void PublishTrackedObject(Buffer sendBuffer, InterestingThing *object);
+  void PublishTrackedObject(Roboid *roboid, InterestingThing *object);
   void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
                              InterestingThing *object);
 

Roboid.cpp

@@ -3,6 +3,8 @@
 #include "LinearAlgebra/FloatSingle.h"
 #include "NetworkSync.h"
 
+#include <string.h>
+
 // #define RC_DEBUG
 
 #ifdef RC_DEBUG
@@ -125,4 +127,12 @@ void Roboid::SetOrientation(Quaternion worldOrientation) {
 
 void Roboid::SetOrientation2D(float angle) {
   this->worldAngleAxis = AngleAxis(angle, Axis::up);
-}
+}
+
+void Roboid::SetModel(const char *url, Vector3 position, Quaternion orientation,
+                      float scale) {
+  this->modelUrl = url;
+  this->modelPosition = position;
+  this->modelOrientation = orientation;
+  this->modelScale = scale;
+}

Roboid.h

@@ -69,6 +69,14 @@ public:
   virtual void SetOrientation(Quaternion worldOrientation);
   void SetOrientation2D(float angle);
 
+  void SetModel(const char *url, Vector3 position = Vector3(0, 0, 0),
+                Quaternion orientation = Quaternion::identity, float scale = 1);
+
+  const char *modelUrl = nullptr;
+  Vector3 modelPosition = Vector3::zero;
+  Quaternion modelOrientation = Quaternion::identity;
+  float modelScale = 1;
+
 private:
   /// @brief The position of the roboid in carthesian coordinates in world space
   /// @details This position may be set when NetworkSync is used to receive