 double th = 0;
 char base_link[] = "/base_link";
 char odom[] = "/odom";
+char ultrafrid[] = "/ultrasound";
 
 void setup() {
   if(DEBUG)Serial.begin(115200);

    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;

#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Range.h>
#include <Servo.h>

#define DEBUG 1
