Merge branch 'main' of http://gitlab.passervr.com/passer/cpp/roboidcontrol

NetworkSync.cpp

@@ -49,8 +49,9 @@ void NetworkSync::PublishState(Roboid* roboid) {
   // if (roboid->updated == false)
   //   return;
 
-  SendPose(roboid);
-  // PublishPerception(roboid);
+  if (roboid->GetVelocity().magnitude() > 0)
+    SendPose(roboid);
+  PublishPerception(roboid);
 }
 
 void NetworkSync::NewObject(InterestingThing* thing) {
@@ -235,12 +236,15 @@ void NetworkSync::PublishClient() {
 }
 
 void NetworkSync::PublishState(Sensor* sensor) {
+  float* value = (float*)sensor->GetValue();
+  if (value == nullptr)
+    return;
+
   unsigned char ix = 0;
   buffer[ix++] = StateMsg;
   buffer[ix++] = sensor->type;
-  float* value = (float*)sensor->GetValue();
   SendFloat16(buffer, &ix, *value);
-  SendBuffer(ix);
+  PublishBuffer(ix);
 }
 
 void NetworkSync::PublishPerception(Roboid* roboid) {
@@ -253,7 +257,6 @@ void NetworkSync::PublishPerception(Roboid* roboid) {
     Sensor* sensor = perception->sensors[sensorIx];
     if (sensor == nullptr)
       continue;
-    // sensor->PublishState();
     PublishState(sensor);
   }
   PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects());
@@ -532,3 +535,5 @@ void NetworkSync::SendInt32(unsigned char* data,
 }
 
 void NetworkSync::SendBuffer(unsigned char bufferSize) {}
+
+void NetworkSync::PublishBuffer(unsigned char bufferSize) {}

NetworkSync.h

@@ -126,6 +126,7 @@ class NetworkSync {
 
   unsigned char buffer[256];
   virtual void SendBuffer(unsigned char bufferSize);
+  virtual void PublishBuffer(unsigned char bufferSize);
 
   void PublishClient();
 };

Perception.cpp

@@ -16,6 +16,9 @@ unsigned char Perception::maxObjectCount = 7;  // 7 is typically the maximum
                                                // be tracked by a human
 
 Perception::Perception() {
+  this->sensorCount = 0;
+  this->sensors = new Sensor*[0];
+
   this->trackedObjects = new InterestingThing*[maxObjectCount];
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
     this->trackedObjects[objIx] = nullptr;

Roboid.cpp

@@ -126,6 +126,10 @@ void Roboid::SetOrientation2D(float angle) {
   this->worldAngleAxis = AngleAxis<float>(angle, Direction<float>::up);
 }
 
+Vector3 Passer::RoboidControl::Roboid::GetVelocity() {
+  return Vector3();
+}
+
 void Roboid::AddChild(Thing* child) {
   Thing::AddChild(child);
   if (child->IsSensor()) {

Roboid.h

@@ -68,6 +68,8 @@ class Roboid : public Thing {
   virtual void SetOrientation(Quaternion worldOrientation);
   void SetOrientation2D(float angle);
 
+  virtual Vector3 GetVelocity();
+
   virtual void AddChild(Thing* child) override;
 
  private: