   digitalWrite(TRIGGER, LOW);
   duration = pulseIn(ECHO, HIGH);
   distance = (duration/2) / 29.1;
+  sstep();
   return (int) distance;
 }
 

  nh.advertise(leftenc);
  nh.advertise(rightenc);
  nh.advertise(range);
  nh.subscribe(sub_r);
  nh.subscribe(sub_l);
  nh.subscribe(sub_f);
  nh.subscribe(sub_b);
  nh.subscribe(sub_a);

  pinMode(D0, OUTPUT); // Ultrasonic Trigger
  pinMode(D1, OUTPUT); // 1,2EN aka D1 pwm left

    broadcaster.sendTransform(t);
    int_msg.data = srange();
    range.publish( &int_msg );
    int_msg.data = sa;
    angle.publish( &int_msg );
    int_msg.data = lmc;
    leftenc.publish( &int_msg );
    int_msg.data = rmc;
