Ultrasound.cpp
Go to the documentation of this file.
1 /*
2  * rosserial Ultrasound Example
3  *
4  * This example is for the Maxbotix Ultrasound rangers.
5  */
6 
7 #include "mbed.h"
8 #include <ros.h>
9 #include <ros/time.h>
10 #include <sensor_msgs/Range.h>
11 
13 
14 sensor_msgs::Range range_msg;
15 ros::Publisher pub_range( "/ultrasound", &range_msg);
16 
17 #if defined(TARGET_LPC1768)
18 const PinName adc_pin = p20;
19 #elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE)
20 const PinName adc_pin = A0;
21 #else
22 #error "You need to specify a pin for the sensor"
23 #endif
24 
25 char frameid[] = "/ultrasound";
26 
27 float getRange_Ultrasound(PinName pin_num) {
28  int val = 0;
29  for (int i=0; i<4; i++) val += AnalogIn(pin_num).read_u16();
30  float range = val;
31  return range /322.519685; // (0.0124023437 /4) ; //cvt to meters
32 }
33 
34 Timer t;
35 int main() {
36  t.start();
37  nh.initNode();
38  nh.advertise(pub_range);
39 
40  range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
41  range_msg.header.frame_id = frameid;
42  range_msg.field_of_view = 0.1; // fake
43  range_msg.min_range = 0.0;
44  range_msg.max_range = 6.47;
45 
46  //pinMode(8,OUTPUT);
47  //digitalWrite(8, LOW);
48 
49  long range_time=0;
50 
51  while (1) {
52 
53  //publish the adc value every 50 milliseconds
54  //since it takes that long for the sensor to stablize
55  if ( t.read_ms() >= range_time ) {
56  range_msg.range = getRange_Ultrasound(adc_pin);
57  range_msg.header.stamp = nh.now();
59  range_time = t.read_ms() + 50;
60  }
61 
62  nh.spinOnce();
63  }
64 }
65 
ros::NodeHandle nh
Definition: Ultrasound.cpp:12
float getRange_Ultrasound(PinName pin_num)
Definition: Ultrasound.cpp:27
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher pub_range("/ultrasound",&range_msg)
char frameid[]
Definition: Ultrasound.cpp:25
Timer t
Definition: Ultrasound.cpp:34
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int i
sensor_msgs::Range range_msg
Definition: Ultrasound.cpp:14
int main()
Definition: Ultrasound.cpp:35


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