1 /*
   2  * rosserial SRF08 Ultrasonic Ranger Example
   3  *
   4  * This tutorial demonstrates the usage of SRF08 ultrasonic module
   5  *
   6  * Source Code Based on:
   7  * http://wiki.ros.org/rosserial_arduino/Tutorials/SRF08%20Ultrasonic%20Range%20Finder
   8  */
   9 
  10 #include "mbed.h"
  11 #include "SRF08.h"
  12 
  13 #include <ros.h>
  14 #include <ros/time.h>
  15 #include <sensor_msgs/Range.h>
  16 
  17 ros::NodeHandle  nh;
  18 
  19 sensor_msgs::Range range_msg;
  20 ros::Publisher pub_range("/ultrasound", &range_msg);
  21 
  22 char frameid[] = "/ultrasound";
  23 
  24 #ifdef TARGET_LPC1768
  25 SRF08 srf08(p9, p10, 0xE0);             // Define SDA, SCL pin and I2C address
  26 #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE)
  27 SRF08 srf08(I2C_SDA, I2C_SCL, 0xE0);    // Define SDA, SCL pin and I2C address
  28 #else
  29 #error "You need to specify a pin for the sensor"
  30 #endif
  31 
  32 Timer t;
  33 int main()
  34 {
  35   t.start();
  36   nh.initNode();
  37   nh.advertise(pub_range);
  38 
  39   range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
  40   range_msg.header.frame_id =  frameid;
  41   range_msg.field_of_view = 0.1;  // fake
  42   range_msg.min_range = 0.0;
  43   range_msg.max_range = 6.47;
  44 
  45   long range_time = 0;
  46 
  47   while (1)
  48   {
  49     //publish the adc value every 50 milliseconds
  50     //since it takes that long for the sensor to stablize
  51     if (t.read_ms() >= range_time)
  52     {
  53       range_msg.range = srf08.read() / 100.0;
  54       range_msg.header.stamp = nh.now();
  55       pub_range.publish(&range_msg);
  56       range_time =  t.read_ms() + 50;
  57     }
  58 
  59     nh.spinOnce();
  60   }
  61 }