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.