example.py

from vehicle import Driver
import math

driver = Driver()

# list all devices
for i in range(driver.getNumberOfDevices()):
    device = driver.getDeviceByIndex(i)
    print( i, device.getName(), type(device) )

depth = driver.getRangeFinder( "zedcam left depth" )
depth.enable(10)
      
display = driver.getDisplay("display")

class Intrinsic:
    def __init__(self, camera):
        self.x = depth.getWidth() /2
        self.y = depth.getHeight() /2
        self.fx = self.x / math.tan( depth.getFov() / 2 )
        self.fy = self.fx
        self.s = 0
        
    def matrix(self):
        return (self.fx, self.s, self.x, 0, self.fy, self.y, 0, 0, 1)

    def coordinates_from_pixel(self, x, y, depth):
        return (x - self.x) / self.fx * depth, \
               (y - self.y) / self.fy * depth, \
               depth

intrinsic = Intrinsic(depth)
         
# we are going to limit the region we look at to ignore the sky/car etc
regionTop = int( depth.getHeight()*0.51 )
regionBase = int( depth.getHeight()*0.68 )     
         
prevTime = 0

#fill the whole background with black
display.setColor( 0x000000 )
display.fillRectangle( 0, 0, display.getWidth(), display.getHeight() )
   
while driver.step() != -1:
    # get a range image
    image = depth.getRangeImageArray()
    
    # highlight the region of the image we are actually checking.
    display.setColor( 0x333333 )
    display.fillRectangle( 0, regionTop, display.getWidth(), regionBase-regionTop )
    display.setColor( 0xFFFFFF );
   
    cones = [[0,0,0],[0,0,0]]
    
    # calculate intrinsic camera parameters
    for x in range( depth.getWidth() ):
        for y in range( regionTop, regionBase ): 
            if image[x][y] - 0.5 > image[x][y+1]: # depth is measured in meters so 0.1 is 10cm
                # calculate position of point relative to camera position
                rx, ry, rz = intrinsic.coordinates_from_pixel( x, y+1, image[x][y+1] )
                      
                # going to calculate the averages of the cone points on the left and right
                onRight = rx > 0
                cones[onRight][0] += rx
                cones[onRight][1] += 1

                display.drawPixel( x, y )
      
    # calc the averages, if no points found then skip the rest of this iteration
    try:         
        for i in cones: i[2] = i[0] / i[1]
    except ZeroDivisionError: continue

    # average of the left and right cones
    center = ( cones[0][2] + cones[1][2]) / 2;
        
    # aim for a point half way between the cones and 10 meters down the track
    steer = math.atan2( center, 10 );   

    driver.setCruisingSpeed( 100 )
    driver.setSteeringAngle( steer )