Personal fork

Dependencies:   Servo mbed rosserial_mbed_lib

Fork of rosserial_mbed by nucho

examples/IrRanger.cpp

Committer:
garyservin
Date:
2014-05-01
Revision:
5:e52b44294b2b
Parent:
0:06fc856e99ca

File content as of revision 5:e52b44294b2b:

//#define COMPILE_IRRANGER_CODE_ROSSERIAL
#ifdef  COMPILE_IRRANGER_CODE_ROSSERIAL

/*
 * rosserial IR Ranger Example
 *
 * This example is calibrated for the Sharp GP2D120XJ00F.
 */

#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>

ros::NodeHandle  nh;


sensor_msgs::Range range_msg;
ros::Publisher pub_range( "range_data", &range_msg);

PinName analog_pin = p20;
unsigned long range_timer;

/*
 * getRange() - samples the analog input from the ranger
 * and converts it into meters.
 */
float getRange(PinName pin_num) {
    int sample;
    // Get data
    sample = AnalogIn(pin_num).read_u16()/4;
    // if the ADC reading is too low,
    //   then we are really far away from anything
    if (sample < 10)
        return 254;     // max range
    // Magic numbers to get cm
    sample= 1309/(sample-3);
    return (sample - 1)/100; //convert to meters
}

char frameid[] = "/ir_ranger";

Timer t;
int main() {
    t.start();
    nh.initNode();
    nh.advertise(pub_range);

    range_msg.radiation_type = sensor_msgs::Range::INFRARED;
    range_msg.header.frame_id =  frameid;
    range_msg.field_of_view = 0.01;
    range_msg.min_range = 0.03;
    range_msg.max_range = 0.4;

    while (1) {
        // publish the range value every 50 milliseconds
        //   since it takes that long for the sensor to stabilize
        if ( (t.read_ms()-range_timer) > 50) {
            range_msg.range = getRange(analog_pin);
            range_msg.header.stamp = nh.now();
            pub_range.publish(&range_msg);
            range_timer =  t.read_ms() + 50;
        }
        nh.spinOnce();
    }
}

#endif