example projects in py and cpp

controllers/example/example.py

@@ -0,0 +1,82 @@
+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)
+
+prevTime = 0
+
+while driver.step() != -1:
+    # get a range image
+    image = depth.getRangeImageArray()
+
+    # clear the display    
+    display.setColor( 0x000000 )
+    display.fillRectangle( 0, 0, display.getWidth(), display.getHeight() )
+    display.setColor( 0xFFFFFF )
+
+    # we are going to limit the region we look at to ignore the sky/car etc
+    regionTop = 370
+    regionBase = 496
+
+    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 a top down view of the cones
+                scaleFactor = display.getHeight() / depth.getMaxRange() 
+
+                px = int(rx * scaleFactor)
+                py = int(rz * scaleFactor)
+                display.drawPixel( display.getWidth()//2+px, display.getHeight()-py )
+
+    # 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 )

controllers/example/runtime.ini

@@ -0,0 +1,3 @@
+[environment variables with relative paths]
+
+WEBOTS_LIBRARY_PATH=$(WEBOTS_HOME)/projects/automobile/libraries/car:$(WEBOTS_HOME)/projects/automobile/libraries/driver:$(WEBOTS_HOME)/projects/automobile/libraries/CppCar:$(WEBOTS_HOME)/projects/automobile/libraries/CppDriver:$(WEBOTS_HOME)/projects/automobile/libraries/python:$(WEBOTS_LIBRARY_PATH)

controllers/examplecpp/examplecpp.cpp

@@ -0,0 +1,111 @@
+#include <webots/vehicle/Driver.hpp>
+#include <webots/RangeFinder.hpp>
+#include <webots/Display.hpp>
+using namespace webots;
+
+#include <iostream>
+#include <memory>
+#include <math.h>
+
+/* Calculate the intrinsic parameters for the camera
+  we can use these values to figure out the relative real world
+  position of pixels if we know the distance to that point */
+class Intrinsic
+{
+public:
+    const float x, y, fx, fy, s;
+
+    Intrinsic( const RangeFinder &camera ) :
+        x( camera.getWidth()*0.5 ),
+        y( camera.getHeight()*0.5 ),
+        fx( x / tan( camera.getFov()*0.5 ) ), fy( fx ),
+        s( 0 )
+    {}
+
+    std::array<float,9> matrix()
+    {
+        return {fx, s, x, 0, fy, y, 0, 0, 1};
+    }
+
+    std::array<float,3> coordinates_from_pixel( int x, int y, float depth )
+    {
+        return { (x - this->x) / fx * depth,
+                 (y - this->y) / fy * depth,
+                 depth };
+    }
+};
+
+int main(int argc, char **argv) 
+{
+    Driver driver;
+
+    std::unique_ptr<RangeFinder> depth = 
+        std::unique_ptr<RangeFinder>( driver.getRangeFinder( "zedcam left depth" ) );
+    if( !depth ) return 1;
+    depth->enable( 10 );
+
+    std::unique_ptr<Display> display = 
+        std::unique_ptr<Display>( driver.getDisplay( "display" ) );
+    if( !display ) return 2;
+
+    Intrinsic intrinsic( *depth );
+
+    // we are going to limit the region we look at to ignore the sky/car etc
+    const int regionTop = 370;
+    const int regionBase = 496;
+    static_assert( regionTop < regionBase );
+
+    while( driver.step() != -1 )
+    {
+        std::array<std::array<float,3>,2> cones;
+        cones[0] = {0,0,0};
+        cones[1] = {0,0,0};
+
+        display->setColor( 0x000000 );
+        display->fillRectangle( 0, 0, display->getWidth(), display->getHeight() );
+        display->setColor( 0xFFFFFF );
+
+        for( int x=0; x<depth->getWidth()-1; ++x )
+        {
+            for( int y=regionTop; y<regionBase; ++y )
+            {
+                if( depth->getRangeImage() == nullptr ) continue;
+
+                float distance1 = RangeFinder::rangeImageGetDepth( depth->getRangeImage(), depth->getWidth(), x, y );
+                float distance2 = RangeFinder::rangeImageGetDepth( depth->getRangeImage(), depth->getWidth(), x, y+1 );
+
+                if( distance1 - 0.5 > distance2 )
+                {
+                    // calculate position of point relative to camera position
+                    std::array<float,3> coords = intrinsic.coordinates_from_pixel( x, y+1, distance2 );
+
+                    // going to calculate the averages of the cone points on the left and right
+                    const bool onRight = coords[0] > 0;    
+                    cones[onRight][0] += coords[0];
+                    cones[onRight][1] += 1;
+
+                    // display a top down view of the cones
+                    float scaleFactor = display->getHeight() / depth->getMaxRange(); 
+
+                    int px = coords[0] * scaleFactor;
+                    int py = coords[2] * scaleFactor;
+                    display->drawPixel( display->getWidth()/2+px, display->getHeight()-py );
+                }
+            }
+        }
+
+        for( auto& i : cones )
+            if( i[1] != 0 ) i[2] = i[0] / i[1];
+
+        // average of the left and right cones
+        float center = ( cones[0][2] + cones[1][2]) / 2.f;
+
+        // aim for a point half way between the cones and 10 meters down the track
+        float steer = atan2( center, 10 );   
+
+        driver.setCruisingSpeed( 100 );
+        driver.setSteeringAngle( steer );
+    }
+
+    return 0;
+}

controllers/examplecpp/runtime.ini

@@ -0,0 +1,3 @@
+[environment variables with relative paths]
+
+WEBOTS_LIBRARY_PATH=$(WEBOTS_HOME)/projects/automobile/libraries/car:$(WEBOTS_HOME)/projects/automobile/libraries/driver:$(WEBOTS_HOME)/projects/automobile/libraries/CppCar:$(WEBOTS_HOME)/projects/automobile/libraries/CppDriver:$(WEBOTS_HOME)/projects/automobile/libraries/python:$(WEBOTS_LIBRARY_PATH)