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