 
 // ROS nodes //
 ros::NodeHandle nh;
-
 geometry_msgs::TransformStamped t;
+geometry_msgs::TransformStamped t2;
+tf::TransformBroadcaster broadcaster;
 nav_msgs::Odometry odom_pub;
-tf::TransformBroadcaster broadcaster;
+sensor_msgs::Range range_msg;
 
 // Functions definitions //
 

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

#define DEBUG 1

double th = 0;
char base_link[] = "/base_link";
char odom[] = "/odom";
char ultrafrid[] = "/ultrasound";

void setup() {
  if(DEBUG)Serial.begin(115200);
