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/IrRanger.cpp@1:225298843679, 2015-02-15 (annotated)

Committer:
akashvibhute
Date:
Sun Feb 15 10:56:37 2015 +0000
Revision:
1:225298843679
Parent:
0:cb1dffdc7d05 
rosserial_mbed for hydro testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 0:cb1dffdc7d05 1 //#define COMPILE_IRRANGER_CODE_ROSSERIAL
akashvibhute 0:cb1dffdc7d05 2 #ifdef COMPILE_IRRANGER_CODE_ROSSERIAL
akashvibhute 0:cb1dffdc7d05 3
akashvibhute 0:cb1dffdc7d05 4 /*
akashvibhute 0:cb1dffdc7d05 5 * rosserial IR Ranger Example
akashvibhute 0:cb1dffdc7d05 6 *
akashvibhute 0:cb1dffdc7d05 7 * This example is calibrated for the Sharp GP2D120XJ00F.
akashvibhute 0:cb1dffdc7d05 8 */
akashvibhute 0:cb1dffdc7d05 9
akashvibhute 0:cb1dffdc7d05 10 #include <ros.h>
akashvibhute 0:cb1dffdc7d05 11 #include <ros/time.h>
akashvibhute 0:cb1dffdc7d05 12 #include <sensor_msgs/Range.h>
akashvibhute 0:cb1dffdc7d05 13
akashvibhute 0:cb1dffdc7d05 14 ros::NodeHandle nh;
akashvibhute 0:cb1dffdc7d05 15
akashvibhute 0:cb1dffdc7d05 16
akashvibhute 0:cb1dffdc7d05 17 sensor_msgs::Range range_msg;
akashvibhute 0:cb1dffdc7d05 18 ros::Publisher pub_range( "range_data", &range_msg);
akashvibhute 0:cb1dffdc7d05 19
akashvibhute 0:cb1dffdc7d05 20 PinName analog_pin = p20;
akashvibhute 0:cb1dffdc7d05 21 unsigned long range_timer;
akashvibhute 0:cb1dffdc7d05 22
akashvibhute 0:cb1dffdc7d05 23 /*
akashvibhute 0:cb1dffdc7d05 24 * getRange() - samples the analog input from the ranger
akashvibhute 0:cb1dffdc7d05 25 * and converts it into meters.
akashvibhute 0:cb1dffdc7d05 26 */
akashvibhute 0:cb1dffdc7d05 27 float getRange(PinName pin_num) {
akashvibhute 0:cb1dffdc7d05 28 int sample;
akashvibhute 0:cb1dffdc7d05 29 // Get data
akashvibhute 0:cb1dffdc7d05 30 sample = AnalogIn(pin_num).read_u16()/4;
akashvibhute 0:cb1dffdc7d05 31 // if the ADC reading is too low,
akashvibhute 0:cb1dffdc7d05 32 // then we are really far away from anything
akashvibhute 0:cb1dffdc7d05 33 if (sample < 10)
akashvibhute 0:cb1dffdc7d05 34 return 254; // max range
akashvibhute 0:cb1dffdc7d05 35 // Magic numbers to get cm
akashvibhute 0:cb1dffdc7d05 36 sample= 1309/(sample-3);
akashvibhute 0:cb1dffdc7d05 37 return (sample - 1)/100; //convert to meters
akashvibhute 0:cb1dffdc7d05 38 }
akashvibhute 0:cb1dffdc7d05 39
akashvibhute 0:cb1dffdc7d05 40 char frameid[] = "/ir_ranger";
akashvibhute 0:cb1dffdc7d05 41
akashvibhute 0:cb1dffdc7d05 42 Timer t;
akashvibhute 0:cb1dffdc7d05 43 int main() {
akashvibhute 0:cb1dffdc7d05 44 t.start();
akashvibhute 0:cb1dffdc7d05 45 nh.initNode();
akashvibhute 0:cb1dffdc7d05 46 nh.advertise(pub_range);
akashvibhute 0:cb1dffdc7d05 47
akashvibhute 0:cb1dffdc7d05 48 range_msg.radiation_type = sensor_msgs::Range::INFRARED;
akashvibhute 0:cb1dffdc7d05 49 range_msg.header.frame_id = frameid;
akashvibhute 0:cb1dffdc7d05 50 range_msg.field_of_view = 0.01;
akashvibhute 0:cb1dffdc7d05 51 range_msg.min_range = 0.03;
akashvibhute 0:cb1dffdc7d05 52 range_msg.max_range = 0.4;
akashvibhute 0:cb1dffdc7d05 53
akashvibhute 0:cb1dffdc7d05 54 while (1) {
akashvibhute 0:cb1dffdc7d05 55 // publish the range value every 50 milliseconds
akashvibhute 0:cb1dffdc7d05 56 // since it takes that long for the sensor to stabilize
akashvibhute 0:cb1dffdc7d05 57 if ( (t.read_ms()-range_timer) > 50) {
akashvibhute 0:cb1dffdc7d05 58 range_msg.range = getRange(analog_pin);
akashvibhute 0:cb1dffdc7d05 59 range_msg.header.stamp = nh.now();
akashvibhute 0:cb1dffdc7d05 60 pub_range.publish(&range_msg);
akashvibhute 0:cb1dffdc7d05 61 range_timer = t.read_ms() + 50;
akashvibhute 0:cb1dffdc7d05 62 }
akashvibhute 0:cb1dffdc7d05 63 nh.spinOnce();
akashvibhute 0:cb1dffdc7d05 64 }
akashvibhute 0:cb1dffdc7d05 65 }
akashvibhute 0:cb1dffdc7d05 66
akashvibhute 0:cb1dffdc7d05 67 #endif