 {
     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

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

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

    nh.spinOnce();
}




sensor_msgs::JointState jointMessage;
ros::Publisher odometryPublisher("/odometry", &jointMessage);

char jointFrameid[] = "/ultrasound/joint";
char *jointNames[] = {"FRONT", "BACK"};


// Ultrasonic Sensor
