 const uint8_t servoRotationAngel = 5;
 
 enum direction {
-  left,
-  center,
-  right
+    left,
+    center,
+    right
 };
 
 uint8_t servoPos = 90;

    {
        rangePublisher2.publish(&rangeMessage);
    }

}



}


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;
