Akash Vibhute / mbed_roshydro_test

ros_serial for mbed updated to work with ROS Hydro. Library still needs to be debugged

Dependencies:   rosserial_hydro

Library and program still under development!

examples/Ultrasound.cpp@0:cb1dffdc7d05, 2015-02-15 (annotated)

Committer:
akashvibhute
Date:
Sun Feb 15 10:54:04 2015 +0000
Revision:
0:cb1dffdc7d05
Error: ; "mbed::Stream::Stream(const mbed::Stream &)" (declared at <a href="#" onmousedown="mbed_doc_goto('/mbed_roshydro_test//extras/mbed_e188a91d3eaa/Stream.h', '59'); return false;">/extras/mbed_e188a91d3eaa/Stream.h:59</a>) is inaccessible in "ext

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 0:cb1dffdc7d05 1 //#define COMPILE_ULTRASOUND_CODE_ROSSERIAL
akashvibhute 0:cb1dffdc7d05 2 #ifdef COMPILE_ULTRASOUND_CODE_ROSSERIAL
akashvibhute 0:cb1dffdc7d05 3
akashvibhute 0:cb1dffdc7d05 4 /*
akashvibhute 0:cb1dffdc7d05 5 * rosserial Ultrasound Example
akashvibhute 0:cb1dffdc7d05 6 *
akashvibhute 0:cb1dffdc7d05 7 * This example is for the Maxbotix Ultrasound rangers.
akashvibhute 0:cb1dffdc7d05 8 */
akashvibhute 0:cb1dffdc7d05 9
akashvibhute 0:cb1dffdc7d05 10 #include "mbed.h"
akashvibhute 0:cb1dffdc7d05 11 #include <ros.h>
akashvibhute 0:cb1dffdc7d05 12 #include <ros/time.h>
akashvibhute 0:cb1dffdc7d05 13 #include <sensor_msgs/Range.h>
akashvibhute 0:cb1dffdc7d05 14
akashvibhute 0:cb1dffdc7d05 15 ros::NodeHandle nh;
akashvibhute 0:cb1dffdc7d05 16
akashvibhute 0:cb1dffdc7d05 17 sensor_msgs::Range range_msg;
akashvibhute 0:cb1dffdc7d05 18 ros::Publisher pub_range( "/ultrasound", &range_msg);
akashvibhute 0:cb1dffdc7d05 19
akashvibhute 0:cb1dffdc7d05 20 const PinName adc_pin = p20;
akashvibhute 0:cb1dffdc7d05 21
akashvibhute 0:cb1dffdc7d05 22 char frameid[] = "/ultrasound";
akashvibhute 0:cb1dffdc7d05 23
akashvibhute 0:cb1dffdc7d05 24 float getRange_Ultrasound(PinName pin_num) {
akashvibhute 0:cb1dffdc7d05 25 int val = 0;
akashvibhute 0:cb1dffdc7d05 26 for (int i=0; i<4; i++) val += AnalogIn(pin_num).read_u16();
akashvibhute 0:cb1dffdc7d05 27 float range = val;
akashvibhute 0:cb1dffdc7d05 28 return range /322.519685; // (0.0124023437 /4) ; //cvt to meters
akashvibhute 0:cb1dffdc7d05 29 }
akashvibhute 0:cb1dffdc7d05 30
akashvibhute 0:cb1dffdc7d05 31 Timer t;
akashvibhute 0:cb1dffdc7d05 32 int main() {
akashvibhute 0:cb1dffdc7d05 33 t.start();
akashvibhute 0:cb1dffdc7d05 34 nh.initNode();
akashvibhute 0:cb1dffdc7d05 35 nh.advertise(pub_range);
akashvibhute 0:cb1dffdc7d05 36
akashvibhute 0:cb1dffdc7d05 37 range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
akashvibhute 0:cb1dffdc7d05 38 range_msg.header.frame_id = frameid;
akashvibhute 0:cb1dffdc7d05 39 range_msg.field_of_view = 0.1; // fake
akashvibhute 0:cb1dffdc7d05 40 range_msg.min_range = 0.0;
akashvibhute 0:cb1dffdc7d05 41 range_msg.max_range = 6.47;
akashvibhute 0:cb1dffdc7d05 42
akashvibhute 0:cb1dffdc7d05 43 //pinMode(8,OUTPUT);
akashvibhute 0:cb1dffdc7d05 44 //digitalWrite(8, LOW);
akashvibhute 0:cb1dffdc7d05 45
akashvibhute 0:cb1dffdc7d05 46 long range_time=0;
akashvibhute 0:cb1dffdc7d05 47
akashvibhute 0:cb1dffdc7d05 48 while (1) {
akashvibhute 0:cb1dffdc7d05 49
akashvibhute 0:cb1dffdc7d05 50 //publish the adc value every 50 milliseconds
akashvibhute 0:cb1dffdc7d05 51 //since it takes that long for the sensor to stablize
akashvibhute 0:cb1dffdc7d05 52 if ( t.read_ms() >= range_time ) {
akashvibhute 0:cb1dffdc7d05 53 int r =0;
akashvibhute 0:cb1dffdc7d05 54
akashvibhute 0:cb1dffdc7d05 55 range_msg.range = getRange_Ultrasound(adc_pin);
akashvibhute 0:cb1dffdc7d05 56 range_msg.header.stamp = nh.now();
akashvibhute 0:cb1dffdc7d05 57 pub_range.publish(&range_msg);
akashvibhute 0:cb1dffdc7d05 58 range_time = t.read_ms() + 50;
akashvibhute 0:cb1dffdc7d05 59 }
akashvibhute 0:cb1dffdc7d05 60
akashvibhute 0:cb1dffdc7d05 61 nh.spinOnce();
akashvibhute 0:cb1dffdc7d05 62 }
akashvibhute 0:cb1dffdc7d05 63 }
akashvibhute 0:cb1dffdc7d05 64
akashvibhute 0:cb1dffdc7d05 65 #endif