Writing your HC-SR04 driver (C++)

This tutorial will guide you through combining distance measurement and relative velocity measurement of the HC-SR04 sensor into a driver, which will be included in a ROS wrapper.

It is assumed that you have already read Distance measurement with ultrasonic sensor HC-SR04 (C++) and Speed measurement with ultrasonic sensor HC-SR04 (C++).

Besides the basics in ROS from the Tutorials, it is also already assumed that you have read the chapters What is a ROS Wrapper?, Write a ROS Wrapper (C++) and Package and Test Your Driver.

Programming our Driver code

Why do we need a driver? Of course, we can simplistically say that we need a driver so that we can operate the sensor. That is also correct. We could simplified yes access the sensor also within our ROS code and save and the concept of a driver. One advantage of programming our driver is that we create a reusability through it. This driver is a HC-SR04 C++ driver for the Raspberry Pi. That means we can use it on every Raspberry Pi, also independent of ROS. Another important point of reusability is that I can connect multiple HC-SR04 sensors to one Raspberry Pi. To achieve this, we need to rewrite our previous code in an better object-oriented way:

#include <iostream>
#include <wiringPi.h>
#include "libHCSR04.h"

HCSR04::HCSR04(){}

void HCSR04::init(int trigger, int echo)
{
    this->trigger=trigger;
    this->echo=echo;
    pinMode(trigger, OUTPUT);
    pinMode(echo, INPUT);
    digitalWrite(trigger, LOW);
    delay(500);
}

double HCSR04::distance(int timeout)
{
    delay(10);

    digitalWrite(trigger, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigger, LOW);

    now=micros();

    while (digitalRead(echo) == LOW && micros()-now<timeout);
        recordPulseLength();

    travelTimeUsec = endTimeUsec - startTimeUsec;
    distanceMeters = 100*((travelTimeUsec/1000000.0)*340.29)/2;

    return distanceMeters;
}

double HCSR04::speed(int timeout)
{
    // calls the distance() function above
    start_distance = this->distance(timeout) * 0.01   // to m conversion

    // giving a time gap of 1 sec
    delay(1000);

    // calls the distance() function above a second time
    end_distance = this->distance(timeout) * 0.01     // to m conversion

    // formula change in distance divided by change in time
    // as the time gap is 1 sec we divide it by 1.
    speed = (end_distance - start_distance) / 1.0  // m/s

    return speed
}


void HCSR04::recordPulseLength()
{
    startTimeUsec = micros();
    while ( digitalRead(echo) == HIGH );
    endTimeUsec = micros();
}

As we can see, one thing has changed:

• I have converted the speed from cm/s to m/s.

Programming our Wrapper code

The next step is to build our ROS wrapper for this driver.

The sensor_msgs have information about the sensor, that is for example the Field of View. These should possibly be loaded via parameters. Also, depending on the sensor, the minimum and maximum range from which a sensor can measure can be adjusted to your sensor. Also here it is recommended to load parameters. And last but not least, you can of course select the pins for the trigger and for the echo differently. Here, too, it would be conceivable that they load parameters.

This looks like the following for example:

#include <ros/ros.h>
#include "libHCSR04.h" // maybe with a path
#include <memory>
#include <map>
#include <string>
#include <vector>
#include <sensor_msgs/Range.h>

class UltrasonicHCSR04Wrapper
{
private:
    std::unique_ptr<HCSR04> ultrasonic_;
    ros::Publisher current_distance_publisher_;
    ros::Publisher current_velocity_publisher_;
    ros::Timer current_distance_timer_;
    ros::Timer current_velocity_timer_;
    int min_range_;
    int max_range_;
    double fov_;
public:
    UltrasonicHCSR04Wrapper(ros::NodeHandle *nh)
    {
        if (!ros::param::get("~minimum_range", min_range_))
        {
            min_range_ = 3;
        }

        if (!ros::param::get("~maximum_range", max_range_))
        {
            max_range_ = 400;
        }

        if (!ros::param::get("~field_of_view", fov_))
        {
            fov_ = 15;
        }

        int trigger_pin;
        if (!ros::param::get("~trigger_pin", trigger_pin))
        {
            trigger_pin = 1;
        }

        int echo_pin;
        if (!ros::param::get("~echo_pin", echo_pin))
        {
            echo_pin = 5;
        }

        ultrasonic_.reset(new HCSR04(trigger_pin, echo_pin));

        current_distance_publisher_ = nh->advertise<sensor_msgs::Range>("/ultrasonic/distance", 10);
        current_velocity_publisher_ = nh->advertise<sensor_msgs::Range>("/ultrasonic/relative_velocity", 10);

        current_distance_timer_ = nh->createTimer(ros::Duration(0.1), &MotorDriverROSWrapper::publishCurrentDistance, this);
        current_velocity_timer_ = nh->createTimer(ros::Duration(0.1), &MotorDriverROSWrapper::publishCurrentVelocity, this);
    }

    void publishCurrentDistance(const ros::TimerEvent &event)
    {
        distance = ultrasonic_->ultrasonic.distance(1000000);

        sensor_msgs::Range msg;
        msg.data = ultrasonic_->getSpeed();

        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "/base_link";
        msg.radiation_type = Range.ULTRASOUND;
        msg.field_of_view = fov_ * 0.0174532925199433;
        msg.min_range = min_range_;
        msg.max_range = max_range_;

        msg.range = distance;

        current_distance_publisher_.publish(msg);
    }

    void publishCurrentVelocity(const ros::TimerEvent &event)
    {
        speed = ultrasonic_->ultrasonic.speed(1000000);

        sensor_msgs::Range msg;
        msg.data = ultrasonic_->getSpeed();

        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "/base_link";
        msg.radiation_type = Range.ULTRASOUND;
        msg.field_of_view = fov_ * 0.0174532925199433;
        msg.min_range = min_range_;
        msg.max_range = max_range_;

        msg.range = speed;

        current_velocity_publisher_.publish(msg);
    }

    void stop()
    {
        current_distance_publisher_.shutdown();
        current_velocity_publisher_.shutdown();
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ultrasonic_driver");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(4);
    spinner.start();

    UltrasonicHCSR04Wrapper ultrasonic_wrapper(&nh);
    ROS_INFO("Ultrasonic driver is now started");

    ros::waitForShutdown();
    ultrasonic_wrapper.stop();
}

In the next tutorial series, we'll look at how to implement an equivalent code to the HC-SR04 by using the Parallax PING))) Ultrasonic sensor. You can choose if you want to continue with Python or C++: Distance measurement with ultrasonic sensor Parallax PING))) (Python) or Distance measurement with ultrasonic sensor Parallax PING))) (C++).