Last active
February 16, 2021 05:29
-
-
Save Jendker/d0296650df4fbaa55745dd29de42e8fd to your computer and use it in GitHub Desktop.
Arduino Ultrasoninc Sensor HC-SR04 with ROS publisher
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| // ---------------------------------------------------------------- // | |
| // Arduino Ultrasoninc Sensor HC-SR04 with ROS publisher | |
| // Rewritten by Jedrzej Orbik | |
| // Using Arduino IDE 1.8.13 | |
| // ---------------------------------------------------------------- // | |
| #include <ros.h> | |
| #include <std_msgs/Float32MultiArray.h> | |
| const int SENSORS_COUNT = 2; | |
| int STARTING_PIN = 3; | |
| int DELAY = 1; | |
| struct UltrasonicSensor { | |
| int echoPin; | |
| int trigPin; | |
| }; | |
| void setupSensor(UltrasonicSensor* self) { | |
| pinMode(self->trigPin, OUTPUT); // Sets the trigPin as an OUTPUT | |
| pinMode(self->echoPin, INPUT); // Sets the echoPin as an INPUT | |
| } | |
| float getDistanceInM(UltrasonicSensor* self) { | |
| unsigned long duration; // variable for the duration of sound wave travel | |
| float distance; // variable for the distance measurement | |
| digitalWrite(self->trigPin, LOW); | |
| delayMicroseconds(2); | |
| // Sets the trigPin HIGH (ACTIVE) for 10 microseconds | |
| digitalWrite(self->trigPin, HIGH); | |
| delayMicroseconds(10); | |
| digitalWrite(self->trigPin, LOW); | |
| // Reads the echoPin, returns the sound wave travel time in microseconds | |
| unsigned long timeout = 23529; // distance would correspond to max range of 4 m | |
| duration = pulseIn(self->echoPin, HIGH, timeout); | |
| if (duration == 0) | |
| return 4.0f; | |
| // Calculating the distance | |
| distance = (float)duration * 0.00034f / 2.f; // Speed of sound wave divided by 2 (go and back) | |
| return distance; | |
| } | |
| ros::NodeHandle nh; | |
| std_msgs::Float32MultiArray array_msg; | |
| ros::Publisher publisher("ultrasonic_sensors", &array_msg); | |
| UltrasonicSensor us_array[SENSORS_COUNT]; | |
| float data_to_publish[SENSORS_COUNT]; | |
| int i; | |
| void setup() { | |
| nh.initNode(); | |
| nh.advertise(publisher); | |
| // Select pins | |
| for (int i=0; i<SENSORS_COUNT; i++) { | |
| us_array[i].echoPin = STARTING_PIN + i*2; | |
| us_array[i].trigPin = STARTING_PIN + i*2 + 1; | |
| setupSensor(&us_array[i]); | |
| } | |
| array_msg.data_length = SENSORS_COUNT; | |
| } | |
| void loop() { | |
| for (i = 0; i<SENSORS_COUNT; i++) { | |
| data_to_publish[i] = getDistanceInM(&us_array[i]); | |
| } | |
| array_msg.data = data_to_publish; | |
| publisher.publish(&array_msg); | |
| delay(DELAY); | |
| nh.spinOnce(); | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment