Ultrasound.cpp

00001 //#define COMPILE_ULTRASOUND_CODE_ROSSERIAL
00002 #ifdef  COMPILE_ULTRASOUND_CODE_ROSSERIAL
00003 
00004 /*
00005  * rosserial Ultrasound Example
00006  *
00007  * This example is for the Maxbotix Ultrasound rangers.
00008  */
00009 
00010 #include "mbed.h"
00011 #include <ros.h>
00012 #include <ros/time.h>
00013 #include <sensor_msgs/Range.h>
00014 
00015 ros::NodeHandle  nh;
00016 
00017 sensor_msgs::Range range_msg;
00018 ros::Publisher pub_range( "/ultrasound", &range_msg);
00019 
00020 const PinName adc_pin = p20;
00021 
00022 char frameid[] = "/ultrasound";
00023 
00024 float getRange_Ultrasound(PinName pin_num) {
00025     int val = 0;
00026     for (int i=0; i<4; i++) val += AnalogIn(pin_num).read_u16();
00027     float range =  val;
00028     return range /322.519685;   // (0.0124023437 /4) ; //cvt to meters
00029 }
00030 
00031 Timer t;
00032 int main() {
00033     t.start();
00034     nh.initNode();
00035     nh.advertise(pub_range);
00036 
00037     range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
00038     range_msg.header.frame_id =  frameid;
00039     range_msg.field_of_view = 0.1;  // fake
00040     range_msg.min_range = 0.0;
00041     range_msg.max_range = 6.47;
00042 
00043     //pinMode(8,OUTPUT);
00044     //digitalWrite(8, LOW);
00045 
00046     long range_time=0;
00047 
00048     while (1) {
00049 
00050         //publish the adc value every 50 milliseconds
00051         //since it takes that long for the sensor to stablize
00052         if ( t.read_ms() >= range_time ) {
00053             int r =0;
00054 
00055             range_msg.range = getRange_Ultrasound(adc_pin);
00056             range_msg.header.stamp = nh.now();
00057             pub_range.publish(&range_msg);
00058             range_time =  t.read_ms() + 50;
00059         }
00060 
00061         nh.spinOnce();
00062     }
00063 }
00064 
00065 #endif