Ultrasonic Sensor on Arduino

edit

I am trying to use the URM04 V2.0 ultrasonic sensor with ROS. I have it attached to a DFRduino I/O Expansion v5.0 shield. I cannot get the arduino to publish it's message. I have no clue why... The LED on the sensor is flashing, so it is taking readings.

    // Usage code sample:  
int EN = 2; // Pin number to enable XBee expansion board V5  
// Measure distance using the URM04V2 ultrasonic sensor.  
//
//
//#include "URM4.h"
#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>

ros::NodeHandle  nh;

sensor_msgs::Range range_msg;
ros::Publisher pub_range( "/ultrasound", &range_msg);

const int adc_pin = 0;

char frameid[] = "/ultrasound";


void measureDistance(byte device) {  
  digitalWrite(EN, HIGH);  
  // Trigger distance measurement.  
  uint8_t DScmd[6]={0x55,0xaa,device,0x00,0x01,0x00};    
  for(int i=0; i<6; i++) {  
    Serial.write(DScmd[i]);  
    DScmd[5] += DScmd[i];  
  }  
  delay(30);  
  // Send command to read measured distance.  
  uint8_t STcmd[6]={0x55,0xaa,device,0x00,0x02,0x00};    
  for(int i=0; i<6; i++) {  
    Serial.write(STcmd[i]);  
    STcmd[5] += STcmd[i];  
  }    
  delay(3);  
}  

// Return last measured distance by the URM04V2 ultrasonic sensor.  
// -1 means the last measurement is out of range or unsuccessful.  
int readDistance() {  
  uint8_t data[8];  
  digitalWrite(EN,LOW);  
  boolean done = false;  
  int counter = 0;  
  int result = -1;  

  while(!done){  
    int bytes = Serial.available();  
    if(bytes==8) {    
      for(int i=0; i<8; i++) {  
        data[i] = Serial.read();  
      }  
      result = (int)data[5] * 256 + data[6];  
      done = true;  
    } else {  
      delay(10);  
      counter++;  
      if(counter==5) { // If failed to read measured data for 5 times, give up and return -1.  
        done = true;  
      }  
    }  
  }  
  return result;  
}  


void setup()
{
  nh.initNode();
  nh.spinOnce();
  nh.advertise(pub_range);

  nh.spinOnce();

  range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
  range_msg.header.frame_id =  frameid;
  range_msg.field_of_view = 0.1;  // fake
  range_msg.min_range = 0.0;
  range_msg.max_range = 6.47;
  nh.spinOnce();
  pinMode(EN, OUTPUT);  
  Serial.begin(19200);  
  delay(200);  
  nh.spinOnce();
  digitalWrite(EN,HIGH);  
  delay(2000); 
  nh.spinOnce();  
}  


long range_time;

void loop()
{
  nh.spinOnce();
  //publish the adc value every 50 milliseconds
  //since it takes that long for the sensor to stablize
  if ( millis() >= range_time ){
    int r =0;
    nh.spinOnce();
    measureDistance(0x11);
    nh.spinOnce();
    range_msg.range = readDistance();
    range_msg.header.stamp = nh.now();
    pub_range.publish(&range_msg);
    nh.spinOnce();
    range_time =  millis() + 50;
  }

  nh.spinOnce();
}

For some reason, when run, this is the terminal output:

turtlebot@Turtlebot_0356:~/arduino-1.0$ rosrun rosserial_python serial_node.py /dev/ttyUSB0 _baud:=19200
[INFO] [WallTime: 1333243936.767010] ROS Serial Python Node
[INFO] [WallTime: 1333243936.777283] Connected on /dev/ttyUSB0 at 19200 baud
[ERROR] [WallTime: 1333243951.785576] Lost sync with device, restarting...
[ERROR] [WallTime: 1333243966.790381] Lost sync with device, restarting...
[ERROR] [WallTime: 1333243981.800134] Lost sync with device, restarting...
[ERROR] [WallTime: 1333243996.811665] Lost sync with device, restarting...
[ERROR] [WallTime: 1333244011.816783] Lost sync with device, restarting...
[ERROR] [WallTime: 1333244026.818693] Lost sync with device, restarting...
[ERROR] [WallTime: 1333244041.822535] Lost sync with device, restarting...
[ERROR] [WallTime: 1333244056.828588] Lost sync with device, restarting...

Thanks,

-Hunter A.