Writing your Parallax PING))) driver (C++)

This tutorial will guide you through combining distance measurement and relative velocity measurement of the Parallax PING))) 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 Parallax PING))) (C++) and Speed measurement with ultrasonic sensor Parallax PING))) (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 Parallax PING))) 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 Parallax PING))) 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 "libParallaxPING.h"

ParallaxPING::ParallaxPING(){}

void ParallaxPING::init(int signal)
{
    this->signal = signal;
    this->timeout = 0.05;
}

double ParallaxPING::distance()
{
    arrivalTime = 0;
    startTime = 0;

    // set to low
    pinMode(signal, OUTPUT);
    digitalWrite(signal, LOW);
    delay(500);

    delay(10);

    // set to high
    digitalWrite(signal, HIGH);
    delayMicroseconds(10);

    // set to low
    digitalWrite(signal, LOW);

    // set to input
    digitalWrite(signal, INPUT)

    timeout_start = micros();

    // Count microseconds that SIG was high
    while(digitalRead(signal) == 0) {
        startTime = micros();

        if(startTime - timeout_start > timeout) {
            return -1;
        }
    }

    while(digitalRead(signal) == 0) {
        arrivalTime = micros();

        if(startTime - timeout_start > timeout) {
            return -1;
        }
    }

    if(startTime != 0 && arrivalTime != 0) {
        pulse_duration = arrivalTime - startTime;
        distance = 100*((pulse_duration/1000000.0)*340.29)/2;

        if(distance >= 0) {
            return distance;
        } else {
            return -1;
        }
    } else {
        return -1;
    }
}

double ParallaxPING::speed()
{
    // 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
}

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 "libParallaxPING.h" // maybe with a path
#include <memory>
#include <map>
#include <string>
#include <vector>
#include <sensor_msgs/Range.h>

class UltrasonicParallaxPINGWrapper
{
private:
    std::unique_ptr<ParallaxPING> 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:
    UltrasonicParallaxPINGWrapper(ros::NodeHandle *nh)
    {
        if (!ros::param::get("~minimum_range", min_range_))
        {
            min_range_ = 3;
        }

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

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

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

        ultrasonic_.reset(new ParallaxPING(signal_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();

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

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