     t.transform.translation.y = 0.0;
     t.transform.translation.z = 1.0;
 
-    t.transform.rotation = createQuaternionFromRPY(0.0, 0.0, tempNumber);
+    t.transform.rotation = createQuaternionFromRPY(0.0, 0.0, DegreeToRadian(servoPos));
     /*
     t.transform.rotation.x = 0.0;
     t.transform.rotation.y = 0.0;

    return q;
}





void setup()
{
    nh.initNode();
    broadcaster.init(nh);

    nh.advertise(rangePublisher1);
    nh.advertise(rangePublisher2);
}

float tempNumber = 0;

void loop()
{
    tempNumber += 0.1;

    t.header.frame_id = odom;
    t.child_frame_id = base_link;


    broadcaster.sendTransform(t);


    t.header.frame_id = base_link;
    t.child_frame_id = servo_node_1;
    t.transform.translation.x = 0.0;
    t.transform.translation.y = 0.05;
    t.transform.translation.z = 0.0;

    /// radian = (degree * 71) / 4068.0
    /// degree = (radians * 4068) / 71.0

    t.transform.rotation = createQuaternionFromRPY(0.0, 0.0, ((90 * 71) / 4068.0));
    t.header.stamp = nh.now();

