IrRanger.cpp

00001 //#define COMPILE_IRRANGER_CODE_ROSSERIAL
00002 #ifdef  COMPILE_IRRANGER_CODE_ROSSERIAL
00003 
00004 /*
00005  * rosserial IR Ranger Example
00006  *
00007  * This example is calibrated for the Sharp GP2D120XJ00F.
00008  */
00009 
00010 #include <ros.h>
00011 #include <ros/time.h>
00012 #include <sensor_msgs/Range.h>
00013 
00014 ros::NodeHandle  nh;
00015 
00016 
00017 sensor_msgs::Range range_msg;
00018 ros::Publisher pub_range( "range_data", &range_msg);
00019 
00020 PinName analog_pin = p20;
00021 unsigned long range_timer;
00022 
00023 /*
00024  * getRange() - samples the analog input from the ranger
00025  * and converts it into meters.
00026  */
00027 float getRange(PinName pin_num) {
00028     int sample;
00029     // Get data
00030     sample = AnalogIn(pin_num).read_u16()/4;
00031     // if the ADC reading is too low,
00032     //   then we are really far away from anything
00033     if (sample < 10)
00034         return 254;     // max range
00035     // Magic numbers to get cm
00036     sample= 1309/(sample-3);
00037     return (sample - 1)/100; //convert to meters
00038 }
00039 
00040 char frameid[] = "/ir_ranger";
00041 
00042 Timer t;
00043 int main() {
00044     t.start();
00045     nh.initNode();
00046     nh.advertise(pub_range);
00047 
00048     range_msg.radiation_type = sensor_msgs::Range::INFRARED;
00049     range_msg.header.frame_id =  frameid;
00050     range_msg.field_of_view = 0.01;
00051     range_msg.min_range = 0.03;
00052     range_msg.max_range = 0.4;
00053 
00054     while (1) {
00055         // publish the range value every 50 milliseconds
00056         //   since it takes that long for the sensor to stabilize
00057         if ( (t.read_ms()-range_timer) > 50) {
00058             range_msg.range = getRange(analog_pin);
00059             range_msg.header.stamp = nh.now();
00060             pub_range.publish(&range_msg);
00061             range_timer =  t.read_ms() + 50;
00062         }
00063         nh.spinOnce();
00064     }
00065 }
00066 
00067 #endif