Advertisement
kennyho-rass

range senzor

Oct 6th, 2021
894
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  #include <ros.h>
  2.  #include <ros/time.h>
  3.  #include <sensor_msgs/Range.h>
  4.  
  5.  ros::NodeHandle  nh;
  6.  sensor_msgs::Range sonar_msg;
  7.  ros::Publisher pub_sonar( "rangeSonar", &sonar_msg);
  8.  float sensoReading = 0;
  9.    
  10. // this constant won't change. It's the pin number of the sensor's output:
  11. const int trigPin = 7;
  12. const int echoPin= 5;
  13.  
  14. char frameid[] ="/sonar_ranger";
  15. void setup() {
  16.   // initialize serial communication:
  17. //  Serial.begin(9600);
  18.     nh.initNode();
  19.     nh.advertise(pub_sonar);
  20.     pinMode(trigPin,OUTPUT);
  21.     pinMode(echoPin,INPUT);
  22.     sonar_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
  23.     sonar_msg.header.frame_id =  frameid;
  24.     sonar_msg.field_of_view = (10.0/180.0) * 3.14;
  25.     sonar_msg.min_range = 0.0;
  26.     sonar_msg.max_range = 10.0;
  27.    
  28. }
  29. int duration;
  30. unsigned long publisher_timer;
  31. void loop() {
  32.     sensoReading = 0 ;
  33.    
  34.   // establish variables for duration of the ping, and the distance result
  35.   // in inches and centimeters:
  36.  
  37.  
  38. if ((millis() - publisher_timer)> 50 )
  39.    {
  40.     digitalWrite(trigPin,LOW);
  41.     delayMicroseconds(2);
  42.     digitalWrite(trigPin,HIGH);
  43.     delayMicroseconds(10);
  44.    
  45.    duration = pulseIn(echoPin, HIGH);
  46.    sensoReading = duration *0.342/2000;
  47.    //sensoReading = getDistance;
  48.    sonar_msg.range = sensoReading;
  49.    //Serial.println(sensoReading);
  50.    sonar_msg.header.stamp = nh.now();
  51.    pub_sonar.publish(&sonar_msg);
  52.    publisher_timer = millis(); //publish once a second
  53.    
  54.    }
  55.    
  56.   nh.spinOnce();
  57.    
  58. }
  59.  
Advertisement
RAW Paste Data Copied
Advertisement