Writing your GP2Y0A02YKOF driver (Python)

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

It is assumed that you have already read Distance measurement with IR sensor GP2Y0A02YKOF (Python) and Speed measurement with IR sensor GP2Y0A02YKOF (Python).

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 (Python) and Package and Test Your Driver.

Programming our Driver code

Important: I would like to write two drivers. Namely, our MCP3008 analog-to-digital converter gets its own driver. Later we want to include this driver in our infrared scanning sensor driver. The MCP3008 can also be reused for other analog sensors on the Raspberry Pi with it. The code looks like this:

import sys
import spidev
import time

class MCP3008(object):

    def __init__(self, bus, device):
        self.spi = spidev.SpiDev()
        self.spi.open(bus, device) # bus = 0, device = 0
        self.spi.max_speed_hz = 1000000

    def readIRAdc(self, channel = 0):
        # analog to digital conversion
        # http://arduinomega.blogspot.com/2011/05/infrared-long-range-sensor-gift-of.html
        adc = self.spi.xfer2([1,(8 + channel) << 4, 0])
        data = ((adc[1] & 3) << 8) + adc[2]

        return data

    def close(self):
        self.spi.close()

This driver is universal and can be used independently of ROS on any Raspberry Pi for the MCP3008.

Following then the driver for our infrared distance sensor:

import os
import sys
import getpass
import time
from mcp3008 import MCP3008

class IRGP2Y0A02YKOF(object):

    def __init__(self, channel):
        self.mcp = MCP3008(0, 0)
        self.channel = channel

    def distance(self):
        read = self.mcp.readIRAdc(self.channel)

        if read > 0:
            # 10650.08 * x ^ (-0.935)
            distance = 10650.08 * pow(read,-0.935)
        else:
            distance = 0

        if distance >= 0:
            return distance
        else:
            return -1

    def speed(self):
        start_time = time.time()

        start_distance = self.distance() * 0.01     # to m conversion
        end_distance = self.distance() * 0.01       # to m conversion

        end_time = time.time()

        speed = (end_distance - start_distance) / (end_time - start_time)   # m/s

        return speed

As you can see, I used a different formula for calculating the distance towards the end.

Writing our ROS Wrapper

Finally the ROS Wrapper looks like this:

import rospy
from sensor_msgs.msg import Range
# import custom message type for Relative Velocity
from ir_irgp2y0a02ykof import IRGP2Y0A02YKOF # import our driver module

class IRGP2Y0A02YKOFWrapper(object):

    def __init__(self):
        self.min_range = rospy.get_param("~minimum_range", 2)
        self.max_range = rospy.get_param("~maximum_range", 150)
        self.fov = rospy.get_param("~field_of_view", 5) * 0.0174532925199433 # degree to radian conversion

        channel = rospy.get_param("~channel", 0)

        self.irPub = rospy.Publisher('/infrared/distance', Range, queue_size=10)
        self.irVelocityPub = rospy.Publisher('/infrared/relative_velocity', Range, queue_size=10)

        # loading the driver
        self.ir_sensor = IRGP2Y0A02YKOF(channel)

        self.rate = rospy.Rate(10) # 10hz
        rospy.Timer(self.rate, self.publish_current_distance)
        rospy.Timer(self.rate, self.publish_current_velocity)

    def publish_current_distance(self):
        distance = self.ir_sensor.distance()

        message_str = "Distance: %s cm" % distance
        rospy.loginfo(message_str)

        #for distance in ranges:
        r = Range()

        r.header.stamp = rospy.Time.now()
        r.header.frame_id = "/base_link"
        r.radiation_type = Range.INFRARED
        r.field_of_view = self.fov
        r.min_range = self.min_range
        r.max_range = self.max_range

        r.range = distance

        self.irPub.publish(r)

    def publish_current_velocity(self):
        relative_velocity = self.ir_sensor.speed()

        message_str = " Speed: %s m/s" % relative_velocity
        rospy.loginfo(message_str)

        rv = Range() # using range instead of a custom message type

        rv.header.stamp = rospy.Time.now()
        rv.header.frame_id = "/base_link"
        rv.radiation_type = Range.INFRARED
        rv.field_of_view = self.fov
        rv.min_range = self.min_range
        rv.max_range = self.max_range

        rv.rage = relative_velocity # using range instead of a custom message type with a field relative velocity

        self.irVelocityPub.publish(rv)

    def stop(self):
        self.irPub.unregister()
        self.irVelocityPub.unregister()

# Main function.
if __name__ == '__main__':
    # Initialize the node and name it.
    rospy.init_node("tof_driver", anonymous=False)

    ir_wrapper = IRGP2Y0A02YKOFWrapper()

    rospy.on_shutdown(ir_wrapper.stop)
    rospy.loginfo("Ultrasonic driver is now started.")

    rospy.spin()