 }
 
 
+void SendServoAngleToROS(int angle)
+{
+    float pos[2] = {float(angle), float(angle+180%360)};
+    jointMessage.position = pos;
+    jointMessage.header.stamp = nh.now();
+    odometryPublisher.publish(&jointMessage);
+}
+
+
 void SendDistanceToROS(int sensorID, float rangeInCentimeter)
 {
     rangeMessage.range = rangeInCentimeter/100;

    {
        rangePublisher2.publish(&rangeMessage);
    }

}



const uint8_t servoRotationAngel = 5;

enum direction {
    left,
    center,
    right
};

uint8_t servoPos = 90;
