HCSR04 with ROS

Dependencies:   Pulse mbed ros_lib_kinetic

main.cpp

Committer:
wupinxian
Date:
2017-07-12
Revision:
0:e70ca69c6311

File content as of revision 0:e70ca69c6311:

/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */

#include"mbed.h"
#include"Pulse.h"
#include"stdio.h"
#include <ros.h>
#include <std_msgs/String.h>


ros::NodeHandle  nh;

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

char hello[20] = "How a fucking day!";
//HCSR04 sonar(PB_0,PB_1) ;
//PA_6 ->trig PA_7->echo
PulseInOut trig(PB_1);
PulseInOut echo(PB_0);
float hc_distance(void)
 {
   long count=0;
   float distance=0;
     trig.write_us(1,10);
      count=echo.read_high_us();
      distance= float(count*0.017);
      return distance;
  }


float dist=0.0;
int main() {
    nh.initNode();
    nh.advertise(chatter);
    while (1) {
       
        dist =  hc_distance();
        sprintf(hello,"%f \n",dist);
         str_msg.data = hello;
        chatter.publish( &str_msg );
         nh.spinOnce();
        wait_ms(1000);
        
    }
}