publishing issue

edit

I want to publish a ultrasonic distance from arduino.The ultra sonic sensor is acting as a radar with the help of a servo motor.But i getting some out of sync messages when i echoed to the topic range data.The code is given below.can any one help to correct this issue.

code:

#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>
 #include <Servo.h>. 
 ros::NodeHandle nh;
    // Defines Tirg and Echo pins of the Ultrasonic Sensor
    const int trigPin = 2;
    const int echoPin = 4;
    int i=0;
    double r =0;
double d;
int j;

    // Variables for the duration and the distance
    double duration;
    double distance;
    sensor_msgs::Range range_msg;
    Servo myServo; 
    ros::Publisher pub_range( "range_data", &range_msg);
    char frameid[] = "range_data";// Creates a servo object for controlling the servo motor
    void setup() {
      Serial.begin(57600);
nh.initNode();
nh.advertise(pub_range);
range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg.header.frame_id = frameid;
range_msg.field_of_view = 0.8;
range_msg.min_range = 0.0;
range_msg.max_range = 400;
      pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
      pinMode(echoPin, INPUT); // Sets the echoPin as an Input
      //Serial.begin(9600);
      myServo.attach(8); // Defines on which pin is the servo motor attached
    }
    double getRange_Ultrasound(){
      // rotates the servo motor from 15 to 165 degrees
      digitalWrite(trigPin, LOW); 
      delayMicroseconds(2);
      // Sets the trigPin on HIGH state for 10 micro seconds
      digitalWrite(trigPin, HIGH); 
      delayMicroseconds(10);
      digitalWrite(trigPin, LOW);
      duration = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound wave travel time in microseconds
      distance= (duration*343)/20000;
      if (distance>500)
      {
        distance=400;
      }
      Serial.print(distance); // Sends the distance value into the Serial Port
        Serial.println();
      return distance;
     delay(50);
    }
    // Function for calculating the distance measured by the Ultrasonic sensor

    double range_time;
    void loop() {
/* The following trigPin/echoPin cycle is used to determine the
distance of the nearest object by bouncing soundwaves off of it. */
//if ( millis() >= range_time ){

for( i=0;i<=180;i++){  
 j=i-1;
      myServo.write(i);
      //delay(5);
range_msg.range = getRange_Ultrasound();
if (range_msg.range<90)
  {
      myServo.detach();
     // delay(10);
      i=j;
  }
  else
  {
     myServo.attach(8);
  }
range_msg.header.stamp = nh.now();
pub_range.publish(&range_msg);
      //distance = calculateDistancf e();// Calls a function for calculating the distance measured by the Ultrasonic sensor for each degree

      //Serial.print(i); // Sends the current degree into the Serial Port
      //Serial.print(","); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing
      //Serial.print(distance); // Sends the distance value into the Serial Port
        //Serial.println();
        //return (distance);
      //Serial.print("."); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing
      }

      // Repeats the previous lines from 165 to 15 degrees
      for(i=180;i>=0;i--){  
        j=i+1;
      myServo.write(i);
      range_msg.range = getRange_Ultrasound();
      if (range_msg.range<90)
  {
      myServo.detach();
    //  delay(10);
      i=j;
  }
  else
  {
     myServo.attach(8);
  }
  range_msg.header.stamp = nh.now();
pub_range.publish(&range_msg);
//range_msg.header.stamp = nh.now();
//pub_range.publish(&range_msg); 


     // delay(5);
      //distance = calculateDistance();

      //Serial.print(i);
      //Serial.print(",");
     // Serial.print(distance);
       // Serial.println();
       // return distance;
      //Serial.print ...