 {
     nh.initNode();
     nh.advertise(rangePublisher);
+    nh.advertise(odometryPublisher);
 
     rangeMessage.radiation_type = sensor_msgs::Range::ULTRASOUND;
     rangeMessage.field_of_view = 0.1;
     rangeMessage.min_range = 0.0;
     rangeMessage.max_range = maxDistance/100; // cm to meter
 
+
+    jointMessage.header.frame_id = jointFrameid;
+    jointMessage.name_length = 2;
+    //jointMessage.velocity_length = 2;
+    jointMessage.position_length = 2;
+    //jointMessage.effort_length = 2;
+
+    jointMessage.name = jointNames;
 
 
     // attach servo pin and set to initial direction



sensor_msgs::JointState jointMessage;
ros::Publisher sinbot_odometry("sinbot_odometry", &jointMessage);

char jointFrameid[] = "/ultrasound/joint";


// Ultrasonic Sensor

    if(servoPos == actualServoPos)
    {
        ServoMotor.write(GetNextServoAngle());
        //delay(5);
    }

    SendDistanceToROS(1, rangeFront);
    SendDistanceToROS(2, rangeBack);

    // TODO - extra function to send
    float pos[2] = {servoPos, (servoPos+180%360)};
    jointMessage.position = pos;
    jointMessage.header.stamp = nh.now();
    odometryPublisher.publish(&jointMessage);

    nh.spinOnce();
}

