         //delay(5);
     }
 
-    SendDistanceToROS(1, rangeFront);
-    SendDistanceToROS(2, rangeBack);
+    if(rangeFront > 0)
+    {
+        SendDistanceToROS(1, rangeFront);
+    }
 
-    // TODO - extra function to send
-    float pos[2] = {float(servoPos), float(servoPos+180%360)};
-    jointMessage.position = pos;
-    jointMessage.header.stamp = nh.now();
-    odometryPublisher.publish(&jointMessage);
+    if(rangeBack > 0)
+    {
+        SendDistanceToROS(2, rangeBack);
+    }
+
+    SendServoAngleToROS(servoPos);
 
     nh.spinOnce();
 }
-
-

}


void SendDistanceToROS(int sensorID, float rangeInCentimeter)
{
    rangeMessage.range = rangeInCentimeter/100;

const uint8_t servoRotationAngel = 5;

enum direction {
    left,
    center,
    right
};

uint8_t servoPos = 90;
