Ask Your Question
0

Ultrasonic Sensor on Arduino

asked 2012-03-31 15:09:51 -0500

allenh1 gravatar image

updated 2014-01-28 17:11:49 -0500

ngrennan gravatar image

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.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2012-03-31 20:56:01 -0500

fergs gravatar image

First, I would suggest never putting nh.spinOnce() calls in your setup() function -- spinning when the program is not entirely configured could lead to problems.

However, the bigger problem is that it would also appear that you are using "Serial.x" function calls when interfacing with your sensor, however, ROSSerial would also be using the first serial port to send data to the PC (unless you have a different ros.h, configuring a different serial port for the ros node handle to use, if so, please post those details as they are likely the culprit).

edit flag offensive delete link more

Comments

I do not have a different ros.h file. How do I fix my code? Or, should I use a different ros.h file?

allenh1 gravatar imageallenh1 ( 2012-04-01 07:38:01 -0500 )edit
1

answered 2012-03-31 16:13:15 -0500

Kevin gravatar image

This is a common problem with rosserial ... search to see what others have done.

Have you tried reducing the data rate down to 9600? It might help keep sync.

edit flag offensive delete link more

Comments

Reduction how? I don't know how to get it to be that rate. Would you please provide me some example?

allenh1 gravatar imageallenh1 ( 2012-03-31 16:17:21 -0500 )edit

In your script where the line reads " Serial.begin(19200);" change to (9600).

Atom gravatar imageAtom ( 2012-04-01 17:30:03 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2012-03-31 15:09:51 -0500

Seen: 1,814 times

Last updated: Mar 31 '12