In this tutorial, you will learn how to use the rosserial communication to publish the ranges of three HC-SR04 sensors, including:
1.Three ultrasonic sensors and Arduino.
2.How to write the ROS node on Arduino and publish the ranges of the sensors.
3.How to identify the Arduino board on Raspberry Pi and run the ROS node via rosserial.
4.How to display the ranges using the Linux Terminal.
Data-flow diagram between sensors, Arduino and Raspberry Pi
Different projects may have different requirements. At the end of this tutorial, you will have a flexible structure that makes it possible to add more sensors, or use only one sensor, or use another type of sensor (for example, infrared sensor).
You don’t need to have strong knowledge about ROS to understand this tutorial. What you need is to have a computer like Raspberry Pi 4 running ROS, sensors, an Arduino and time to learn and build.
We go further and learn how to write the ROS node on Arduino and publish the ranges of the sensors using the sensor_msgs/Range message type from ROS.
At the end of the post, you will learn how to identify the Arduino board on Raspberry Pi, and how to display on Terminal the ranges advertised by the ROS node that is running on Arduino.
To learn how to use the sensor_msgs package from ROS to send the range readings from the ultrasonic sensors, just keep reading.
Table of Contents
ToggleBefore you get started with the sensor_msgs/Range message type
Before starting to connect the sensor and write the first line of code, let’s be sure that we have all the hardware parts:
- 1 X Arduino UNO
- 1 X USB cable
- 3 X HC-SR04
- 1 X Raspberry Pi 4 running ROS Melodic
- 1 X Breadboard
- female-to-female/male-to-male/female-to-male jumper wires
Hardware setup
To power the Raspberry Pi 4, I use a USB-C power supply with an output of 5.1V at 3.0A. The 5V USB port of Raspberry Pi 4 provides enough power to run the Arduino UNO board. The three ultrasonic sensors are powered using a 7.4V battery pack and a step-down converter to provide a 5V output for sensors.
The above setup is just for connecting and testing the ultrasonic detection system. If the sensors, Arduino and the Pi will be mounted on a mobile robot, the entire detection system will run on batteries, including the Pi board.
Of course, if you plan to build a simple mobile robot to detect obstacles, this tutorial is overwhelming. You don’t need such infrastructure for a simple robot. But if you plan to build advanced robots in a productive and professional manner, this is the point where you can start.
1. Three ultrasonic sensors and Arduino
In this part of the tutorial, I will show you how to connect the HC-SR04 sensors to Arduino.
First, let’s have a look on the HC-SR04 specifications:
- Operating Voltage: DC 5V
- Operating Current: 15mA
- Operating Frequency: 40KHz
- Range: from 2cm to 4m
- Ranging Accuracy: 3mm
- Trigger Input Signal: 10µS TTL pulse
For connections, I use female-to-male jumper wires, a breadboard, three HC-SR04, and an Arduino UNO board.
Connections
Sensor 1:
- Vcc -> breadboard -> 5V battery
- Trig -> pin 3 (digital pin)
- Echo -> pin 2 (digital pin)
- GND -> breadboard -> GND
Sensor 2:
- Vcc -> breadboard -> 5V battery
- Trig -> pin 5 (digital pin)
- Echo -> pin 4 (digital pin)
- GND -> breadboard -> GND
Sensor 3:
- Vcc -> breadboard -> 5V battery
- Trig -> pin 7 (digital pin)
- Echo -> pin 6 (digital pin)
- GND -> breadboard -> GND
Common ground: connect the ground of the Arduino to the ground of the breadboard.
Hardware setup for three ultrasonic sensors, Arduino, battery pack, and Raspberry Pi 4
Since we prepare the Pi to run ROS and the Arduino IDE, you can use Pi with a monitor, keyboard and mouse, or via the VNC Viewer to write and upload the sketch on the Arduino board.
Once the sensors are connected to the Arduino board, we can start writing the sketch to read the outputs and transform the reading from them. For writing the sketch, I use the Arduino IDE. I like to work with simple tools and don’t spend time on customizations and accessories. For the moment, the Arduino IDE satisfies my needs in terms of programming a microcontroller.
Before writing the first line of code, let’s recapitulate how an ultrasonic sensor works.
It pings a sound wave (for at least 10us according to specifications), which travels through the air, and if there is an object that reflects the sound wave, the sensor measures the time that ping took to return to the receiver. To calculate the distance between sensor and object detected, we consider the travel time and the speed of the sound.
2. How to write the ROS node on Arduino and publish the ranges of the sensors
At this point in the tutorial, we will create a ROS node on the Arduino UNO board and use the distributed computing environment to send the sensor outputs to the Raspberry Pi.
From many packages included in the ROS framework, the package sensor_msgs consists of a collection of messages for commonly used sensors. With ultrasonic sensors, we use the Range message type. This message type sends a single range reading from an ultrasonic sensor that is valid along an arc at the distance measured.
C++
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
|
/*
:Version 1.0
:Author: Dragos Calin
:Email: dragos@intorobotics.com
:License: BSD
:Date: 01/05/2020
:Last update: dd/mm/YYYY
*/
#include <NewPing.h>
#include <SimpleKalmanFilter.h>
#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>
#define SONAR_NUM 3 //The number of sensors.
#define MAX_DISTANCE 200 //Mad distance to detect obstacles.
#define PING_INTERVAL 33 //Looping the pings after 33 microseconds.
unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM]; // Where the ping distances are stored.
uint8_t currentSensor = 0; // Keeps track of which sensor is active.
unsigned long _timerStart = 0;
int LOOPING = 40; //Loop for every 40 milliseconds.
uint8_t oldSensorReading[3]; //Store last valid value of the sensors.
uint8_t leftSensor; //Store raw sensor’s value.
uint8_t centerSensor;
uint8_t rightSensor;
uint8_t leftSensorKalman; //Store filtered sensor’s value.
uint8_t centerSensorKalman;
uint8_t rightSensorKalman;
NewPing sonar[SONAR_NUM] = {
NewPing(3, 2, MAX_DISTANCE), // Trigger pin, echo pin, and max distance to ping.
NewPing(5, 4, MAX_DISTANCE),
NewPing(7, 6, MAX_DISTANCE)
};
/*
create Kalman filter objects for the sensors.
SimpleKalmanFilter(e_mea, e_est, q);
e_mea: Measurement Uncertainty
e_est: Estimation Uncertainty
q: Process Noise
*/
SimpleKalmanFilter KF_Left(2, 2, 0.01);
SimpleKalmanFilter KF_Center(2, 2, 0.01);
SimpleKalmanFilter KF_Right(2, 2, 0.01);
ros::NodeHandle nh; //create an object which represents the ROS node.
//looping the sensors
void sensorCycle() {
for (uint8_t i = 0; i < SONAR_NUM; i++) {
if (millis() >= pingTimer[i]) {
pingTimer[i] += PING_INTERVAL * SONAR_NUM;
if (i == 0 && currentSensor == SONAR_NUM – 1) oneSensorCycle();
sonar[currentSensor].timer_stop();
currentSensor = i;
cm[currentSensor] = 0;
sonar[currentSensor].ping_timer(echoCheck);
}
}
}
// If ping received, set the sensor distance to array.
void echoCheck() {
if (sonar[currentSensor].check_timer())
cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}
//Return the last valid value from the sensor.
void oneSensorCycle() {
leftSensor = returnLastValidRead(0, cm[0]);
centerSensor = returnLastValidRead(1, cm[1]);
rightSensor = returnLastValidRead(2, cm[2]);
}
//If sensor value is 0, then return the last stored value different than 0.
int returnLastValidRead(uint8_t sensorArray, uint8_t cm) {
if (cm != 0) {
return oldSensorReading[sensorArray] = cm;
} else {
return oldSensorReading[sensorArray];
}
}
//Apply Kalman Filter to sensor reading.
void applyKF() {
leftSensorKalman = KF_Left.updateEstimate(leftSensor);
centerSensorKalman = KF_Center.updateEstimate(centerSensor);
rightSensorKalman = KF_Right.updateEstimate(rightSensor);
}
void startTimer() {
_timerStart = millis();
}
bool isTimeForLoop(int _mSec) {
return (millis() – _timerStart) > _mSec;
}
void sensor_msg_init(sensor_msgs::Range &range_name, char *frame_id_name)
{
range_name.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_name.header.frame_id = frame_id_name;
range_name.field_of_view = 0.26;
range_name.min_range = 0.0;
range_name.max_range = 2.0;
}
//Create three instances for range messages.
sensor_msgs::Range range_left;
sensor_msgs::Range range_center;
sensor_msgs::Range range_right;
//Create publisher onjects for all sensors
ros::Publisher pub_range_left(“/ultrasound_left”, &range_left);
ros::Publisher pub_range_center(“/ultrasound_center”, &range_center);
ros::Publisher pub_range_right(“/ultrasound_right”, &range_right);
void setup() {
pingTimer[0] = millis() + 75;
for (uint8_t i = 1; i < SONAR_NUM; i++)
pingTimer[i] = pingTimer[i – 1] + PING_INTERVAL;
nh.initNode();
nh.advertise(pub_range_left);
nh.advertise(pub_range_center);
nh.advertise(pub_range_right);
sensor_msg_init(range_left, “/ultrasound_left”);
sensor_msg_init(range_center, “/ultrasound_center”);
sensor_msg_init(range_right, “/ultrasound_right”);
}
void loop() {
if (isTimeForLoop(LOOPING)) {
sensorCycle();
oneSensorCycle();
applyKF();
range_left.range = leftSensorKalman;
range_center.range = centerSensorKalman;
range_right.range = centerSensorKalman;
range_left.header.stamp = nh.now();
range_center.header.stamp = nh.now();
range_right.header.stamp = nh.now();
pub_range_left.publish(&range_left);
pub_range_center.publish(&range_center);
pub_range_right.publish(&range_right);
startTimer();
}
nh.spinOnce();//Handle ROS events
}
|
On Line 9, we import the NewPing library that works with the HC-SR04 ultrasonic sensors. You can install the library using the Manage Libraries from Arduino IDE. In the search form type newping, and in the manager screen you will have the option to install the latest version of the library.
On Line 10, we import the SimpleKalmanFilter library to filter the ultrasonic sensor’s output. You can install the library using the Manage Libraries from Arduino IDE. In the search form type simplekalman, and in the manager screen you will have the option to install the latest version of the library.
Lines 11-13: we import ROS packages. ros.h is the standard library and is part of every ROS node that is running on Arduino. We need to include the ros.h header for any message used in the ROS node.
The ros/time.h header represents the ROS clock time.
The sensor_msgs/Range.h is a message definition used to advertise a single range reading from the ultrasonic sensor valid along an arc at a distance measured.
Lines 38-42: create newping objects for all the sensors. The parameters of the object are the trigger and echo pins, and the maximum distance for the sensor.
Line 55: NodeHandle is an object which represents the ROS node. ros::NodeHandle will start the node on the Arduino board.
Lines 58-69: Loop through all the sensors, and when the sensor ping cycle is completed, add the results in oneSensorCycle().
Lines 72-75: When a new ping is received, add the sensor distance to an array.
Lines 85-91: We filter the readings of the sensor. Store the readings different than 0, and if the sensor returns 0, we return the last valid output. In this way, we filter the false readings of the ultrasonic sensor.
Lines 100-102: the function to start counting the time using millis(). I prefer to use millis() instead of delay() from two reasons: millis() is more accurate than delay(), and is a non-blocking alternative of delay().
Lines 104-106: Check if the time passed and return true.
Lines 108-115: write the settings of Range message object. The first parameter is the radiation_type of the sensor – ULTRASOUND.
The frame_id is used to specify the point of reference for data contained in that message.
The field_of_view represents the size of the arc that the distance reading is valid for in radians. HC-SR04 has a theoretical field of view of 30 degrees. In the previous post, I did tests to determine the most accurate area of the operating detection range. Since we aim to detect most of the objects in front of the robot accurately, we set a field of view of 15 degrees, which is about 0.26 radians.
Min and max range are the values of the measurements that are considered valid.
Lines 118-120: We define three objects of type Range, and then we give the name of the topics – range_left, range_center, and range_right.
Lines 123-125: We use the advertise() methods to create a Publisher, which is used to publish the ranges on topics.
Lines 143-160: We check the time and publish every 40 milliseconds. Read the value returned by the sensor. Then we use nh.now() to return the current time and then publish the range value.
Line 161: The ROS network monitors socket connections to push the messages from topics onto a queue. We send a new message and then use ros::spinOnce() to tell ROS that a new message arrives.
3. How to identify the Arduino board on Raspberry Pi and run the ROS node via rosserial
Once the Arduino is connected to Pi and the sketch is uploaded, we can identify the Arduino board.
Step 1: Open a Terminal to launch the roscore executable:
Shell
1
|
$roscore
|
Step 2: Open a new Terminal and run the below command:
Shell
1
|
$ls /dev/ttyACM*
|
If you have only one Arduino connected to the Pi, you should see only one abstract control modem (ACM):
ls dev ttyACM
Step 3: Run an executable via serial connection:
Shell
1
|
$rosrun rosserial_python serial_node.py /dev/ttyACM0
|
Run an executable via serial connection
4. How to display the ranges using the Linux Terminal
Step 1: Open a new terminal and run the command:
Shell
1
|
$rostopic list
|
The node that is running on Arduino is now accessible in the ROS network.
rostopic list nodes
Step 2: Run the outputs of one sensor in a Terminal. Below you can see the ranges of the “/ultrasound_center”:
Shell
1
|
$rostopic echo /ultrasound_center
|
The ranges of the “/ultrasound_center”
Summary
Obstacle detection is applicable to any robot that moves from an initial position to a goal position avoiding any obstacle in its path.
The first step was to connect the sensors to the Arduino board.
Then we go further and write a ROS node on Arduino to publish the ranges of the sensors. We use the sensor_msgs/Range message type to publish to advertise the ranges.
Once the ROS node is running on Arduino, we identify the board on Pi and check the results.