 
 double x = 1.0;
 double y = 0.0;
-double theta = 1.57;
+double th = 0;
 char base_link[] = "/base_link";
 char odom[] = "/odom";
 

int i;

// WiFi configuration. Replace *** by your data//
const char* ssid = "***";
const char* password = "**";
// Set the rosserial socket server IP address
IPAddress server(192,168,1,***);
// Set the rosserial socket server port
const uint16_t serverPort = 11411;


ros::Time last_time = current_time;
double DistancePerCount = (2 * 3.14159265 * 0.035) / 20;   // 2*PI*R/CPR
double lengthBetweenTwoWheels = 0.13;
int last_lmc = lmc;
int last_rmc = rmc;
int current_lmc = lmc;
int current_rmc = rmc;

void loop() {
  if (nh.connected()) {
    current_time = nh.now();
    current_lmc = lmc;
    current_rmc = rmc;
    double dt = current_time.toSec() - last_time.toSec();  
    double ld = (current_lmc-last_lmc)*DistancePerCount;   // left wheel linear distance
    double rd = (current_rmc-last_rmc)*DistancePerCount;  // right wheel linear distance
    double vlm = ld / dt;   // left wheel linear velocity
    double vrm = rd / dt;  // right wheel linear velocity
    double vx = (ld + rd) / 2.;
    double vy = 0;
    double th = (current_rmc - current_lmc) / 45. * 3.14159265;
    last_lmc = current_lmc;
    last_rmc = current_rmc;
    last_time = current_time;
    //compute odometry in a typical way given the velocities of the robot
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    x += delta_x;
    y += delta_y;
    t.header.frame_id = odom;
    t.child_frame_id = base_link;
    t.transform.translation.x = x; 
    t.transform.translation.y = y; 
    t.transform.rotation = tf::createQuaternionFromYaw(th);
    t.header.stamp = nh.now();
    broadcaster.sendTransform(t);
    int_msg.data = srange();
