Ask Your Question

Revision history [back]

Ultrasonic Sensor on Arduino

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 have the code publishing a message, but it returns a -1 consistently... Below is the code for the sonar:

// 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.advertise(pub_range);


  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;

  pinMode(EN, OUTPUT);  
  Serial.begin(19200);  
  delay(200);  
  digitalWrite(EN,HIGH);  
  delay(2000);    
}  


long range_time;

void loop()
{

  //publish the adc value every 50 milliseconds
  //since it takes that long for the sensor to stablize
  if ( millis() >= range_time ){
    int r =0;

    measureDistance(0x11);
    if (readDistance() != -1)
      range_msg.range = readDistance() / 100.0;
    else 
      range_msg.range = readDistance();
    range_msg.header.stamp = nh.now();
    pub_range.publish(&range_msg);
    range_time =  millis() + 50;
  }

  nh.spinOnce();
}

What am I doing wrong? It is plugged in to the "B" jack on the sensor, if you need that info. It blinks the green LED four times when plugged in (means that it is nominally functioning). I don't understand...

Thanks,

-Hunter A.

click to hide/show revision 2
the problem is different now.

Ultrasonic Sensor on Arduino

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 code publishing a message, but sensor is flashing, so it returns a -1 consistently... Below is the code for the sonar: 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);
    if (readDistance() != -1)
  nh.spinOnce();
    range_msg.range = readDistance() / 100.0;
    else 
      range_msg.range = readDistance();
    range_msg.header.stamp = nh.now();
    pub_range.publish(&range_msg);
    nh.spinOnce();
    range_time =  millis() + 50;
  }

  nh.spinOnce();
}

What am I doing wrong? It For some reason, when run, this is plugged in to the "B" jack 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 the sensor, if you need that info. It blinks the green LED four times when plugged in (means that it is nominally functioning). I don't understand... 

/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.

Ultrasonic Sensor on Arduino

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.

Ultrasonic Sensor on Arduino

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.