IrRanger.cpp
Go to the documentation of this file.
1 /*
2  * rosserial IR Ranger Example
3  *
4  * This example is calibrated for the Sharp GP2D120XJ00F.
5  */
6 
7 #include <ros.h>
8 #include <ros/time.h>
9 #include <sensor_msgs/Range.h>
10 
12 
13 
14 sensor_msgs::Range range_msg;
15 ros::Publisher pub_range( "range_data", &range_msg);
16 
17 #if defined(TARGET_LPC1768)
18 PinName analog_pin = p20;
19 #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE)
20 PinName analog_pin = A0;
21 #else
22 #error "You need to specify a pin for the mic"
23 #endif
24 unsigned long range_timer;
25 
26 /*
27  * getRange() - samples the analog input from the ranger
28  * and converts it into meters.
29  */
30 float getRange(PinName pin_num) {
31  int sample;
32  // Get data
33  sample = AnalogIn(pin_num).read_u16()/4;
34  // if the ADC reading is too low,
35  // then we are really far away from anything
36  if(sample < 10)
37  return 254; // max range
38  // Magic numbers to get cm
39  sample= 1309/(sample-3);
40  return (sample - 1)/100; //convert to meters
41 }
42 
43 char frameid[] = "/ir_ranger";
44 
45 Timer t;
46 int main() {
47  t.start();
48  nh.initNode();
49  nh.advertise(pub_range);
50 
51  range_msg.radiation_type = sensor_msgs::Range::INFRARED;
52  range_msg.header.frame_id = frameid;
53  range_msg.field_of_view = 0.01;
54  range_msg.min_range = 0.03;
55  range_msg.max_range = 0.4;
56 
57  while (1) {
58  // publish the range value every 50 milliseconds
59  // since it takes that long for the sensor to stabilize
60  if ( (t.read_ms()-range_timer) > 50) {
61  range_msg.range = getRange(analog_pin);
62  range_msg.header.stamp = nh.now();
64  range_timer = t.read_ms() + 50;
65  }
66  nh.spinOnce();
67  }
68 }
69 
char frameid[]
Definition: IrRanger.cpp:43
void publish(const boost::shared_ptr< M > &message) const
sensor_msgs::Range range_msg
Definition: IrRanger.cpp:14
float getRange(PinName pin_num)
Definition: IrRanger.cpp:30
ros::Publisher pub_range("range_data",&range_msg)
int main()
Definition: IrRanger.cpp:46
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Timer t
Definition: IrRanger.cpp:45
unsigned long range_timer
Definition: IrRanger.cpp:24
ros::NodeHandle nh
Definition: IrRanger.cpp:11


rosserial_mbed
Author(s): Gary Servin
autogenerated on Mon Jun 10 2019 14:53:26