made pose sending generic

NetworkSync.cpp

@@ -4,13 +4,25 @@
 #include <Arduino.h>
 #endif
 
-void NetworkSync::SendVector3(unsigned char *data, int startIndex, Vector3 v) {
+UInt8 NetworkSync::CreatePoseMsg(Roboid* roboid, UInt8* buffer) {
+  Vector3 position = roboid->GetPosition();
+
+  UInt8 bufferSize = 3 + 12;
+  buffer[0] = PoseMsg;
+  buffer[1] = 0;  // ObjectId;
+  buffer[2] = Pose_Position;
+  SendVector3(buffer, 3, position);
+  return bufferSize;
+}
+
+void NetworkSync::SendVector3(unsigned char* data, int startIndex, Vector3 v) {
   SendSingle100(data, startIndex, v.x);
   SendSingle100(data, startIndex + 4, v.y);
   SendSingle100(data, startIndex + 8, v.z);
 }
 
-void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
+void NetworkSync::SendSingle100(unsigned char* data,
+                                int startIndex,
                                 float value) {
   // Sends a float with truncated 2 decimal precision
   Int32 intValue = value * 100;
@@ -20,16 +32,16 @@ void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
   // }
 }
 
-void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) {
+void NetworkSync::SendInt32(unsigned char* data, int startIndex, Int32 value) {
   for (unsigned char ix = 0; ix < 4; ix++) {
-    data[startIndex + ix] = ((unsigned char *)&value)[ix];
+    data[startIndex + ix] = ((unsigned char*)&value)[ix];
   }
 }
 
 void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
-                                        TrackedObject **objects) {
+                                        TrackedObject** objects) {
   for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
-    TrackedObject *obj = objects[objIx];
+    TrackedObject* obj = objects[objIx];
     if (obj == nullptr)
       continue;
     // if (obj->sensor->type == Thing::ExternalType)
@@ -42,7 +54,7 @@ void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
 }
 
 void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
-                                       TrackedObject *object) {
+                                       TrackedObject* object) {
   Vector2 worldPosition2 = Vector2::Rotate(
       Vector2::forward * object->position.distance, -object->position.angle);
   Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);
@@ -68,7 +80,7 @@ void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
 #endif
 }
 
-void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
+void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid) {
   Polar velocity = roboid->propulsion->GetVelocity();
   Vector2 worldVelocity2 =
       Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
@@ -94,7 +106,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
   const unsigned int bufferSize = 3 + 12 + 12;
   unsigned char buffer[bufferSize] = {
       PoseMsg,
-      0, // objectId;
+      0,  // objectId;
       Pose_LinearVelocity | Pose_AngularVelocity,
   };
   SendVector3(buffer, 3, worldVelocity3);
@@ -104,7 +116,7 @@ void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
 #endif
 }
 
-void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj) {
+void NetworkSync::SendDestroyObject(SendBuffer sendBuffer, TrackedObject* obj) {
 #ifdef RC_DEBUG
   Serial.print("Send Destroy ");
   Serial.println((int)obj->id);

NetworkSync.h

@@ -10,13 +10,13 @@ namespace RoboidControl {
 
 /// @brief Interface for synchronizaing state between clients across a network
 class NetworkSync {
-public:
+ public:
   /// @brief Retreive and send the roboid state
   /// @param roboid The roboid for which the state is updated
-  virtual void NetworkUpdate(Roboid *roboid) = 0;
+  virtual void NetworkUpdate(Roboid* roboid) = 0;
   /// @brief Inform that the given object is no longer being tracked
   /// @param obj
-  virtual void DestroyObject(TrackedObject *obj) = 0;
+  virtual void DestroyObject(TrackedObject* obj) = 0;
 
   /// @brief The id of a Pose message
   static const char PoseMsg = 0x10;
@@ -35,21 +35,22 @@ public:
 
   static const char DestroyMsg = 0x20;
 
-  typedef void (*SendBuffer)(UInt8 *buffer, UInt16 bufferSize);
+  typedef void (*SendBuffer)(UInt8* buffer, UInt16 bufferSize);
 
-  void SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid);
+  void SendPoseMsg(SendBuffer sendBuffer, Roboid* roboid);
-  void SendDestroyObject(SendBuffer sendBuffer, TrackedObject *obj);
+  void SendDestroyObject(SendBuffer sendBuffer, TrackedObject* obj);
 
-  void PublishTrackedObjects(SendBuffer sendBuffer, TrackedObject **objects);
+  void PublishTrackedObjects(SendBuffer sendBuffer, TrackedObject** objects);
 
-protected:
+ protected:
-  NetworkPerception *networkPerception;
+  NetworkPerception* networkPerception;
-  void PublishTrackedObject(SendBuffer sendBuffer, TrackedObject *object);
+  void PublishTrackedObject(SendBuffer sendBuffer, TrackedObject* object);
 
-  void SendVector3(unsigned char *data, int startIndex, Vector3 v);
+  UInt8 CreatePoseMsg(Roboid* roboid, UInt8* buffer);
-  void SendSingle100(unsigned char *data, int startIndex, float value);
+  void SendVector3(unsigned char* data, int startIndex, Vector3 v);
-  void SendInt32(unsigned char *data, int startIndex, Int32 value);
+  void SendSingle100(unsigned char* data, int startIndex, float value);
+  void SendInt32(unsigned char* data, int startIndex, Int32 value);
 };
 
-} // namespace RoboidControl
+}  // namespace RoboidControl
-} // namespace Passer
+}  // namespace Passer