How to Use sensor_msgs/Range (ROS) For Multiple Sensors with rosserial

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

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.

Before 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.

Note: To go further in this article, I suppose you have a Raspberry Pi 4 that is running ROS Melodic. If not, please take a deep breath and read how to install ROS Melodic, rosserial, and more on Raspberry Pi 4 (Raspbian Buster)

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

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.

Info: The sound moves through the air at different speeds depending on the temperature. Since we are using the speed of sound to measure the time that ping took to return to the receiver, the environmental temperature can affect the accuracy of an ultrasonic sensor. Temperature compensation is not part of this tutorial, but you can add it later in your code.

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.

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:

Step 2: Open a new Terminal and run the below command:

If you have only one Arduino connected to the Pi, you should see only one abstract control modem (ACM):

ls dev ttyACM

ls dev ttyACM

Step 3: Run an executable via serial connection:

Run an executable via serial connection

Run an executable via serial connection

Note: If you have some strange errors like “Mismatched protocol version in packet (‘!’): lost sync or rosserial_python is from different ros release than the rosserial …“, you have to stop the node that is running on Arduino, close the Arduino IDE and then run the command again: rosrun rosserial_python serial_node.py /dev/ttyACM*

4. How to display the ranges using the Linux Terminal

Step 1: Open a new terminal and run the command:

The node that is running on Arduino is now accessible in the ROS network.

rostopic list nodes

rostopic list nodes

Step 2: Run the outputs of one sensor in a Terminal. Below you can see the ranges of the “/ultrasound_center”:

the ranges of the "/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.

2 comments » Write a comment

  1. Why following?

    SimpleKalmanFilter KF_Left(2, 2, 0.01);
    SimpleKalmanFilter KF_Center(2, 2, 0.01);
    SimpleKalmanFilter KF_Right(2, 2, 0.01);

    Can you explain, why you use the values 2, 2 and 0.01?

    In your loop function is somethin wrong…

    range_left.range = leftSensorKalman;
    range_center.range = centerSensorKalman;
    range_right.range = centerSensorKalman;

    I think it should be range_right.range = rightSensorKalman;

    • Hello,
      2, 2 and 0.01 are the e_mea, e_est, q of the Kalman filter library. [https://github.com/denyssene/SimpleKalmanFilter]
      Yes, there is a typo: it should be range_right.range = rightSensorKalman;

Leave a Reply to Michdo93 Cancel reply

Your email address will not be published. Required fields are marked *