 
 
 sensor_msgs::JointState jointMessage;
-ros::Publisher sinbot_odometry("sinbot_odometry", &jointMessage);
+ros::Publisher odometryPublisher("/odometry", &jointMessage);
 
 char jointFrameid[] = "/ultrasound/joint";
+char *jointNames[] = {"FRONT", "BACK"};
 
 
 // Ultrasonic Sensor

{
    nh.initNode();
    nh.advertise(rangePublisher);

    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



    // attach servo pin and set to initial direction

    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();
}

