examplecpp.cpp

#include <webots/vehicle/Driver.hpp>
#include <webots/RangeFinder.hpp>
#include <webots/Display.hpp>
using namespace webots;

#include <cassert>
#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;
        
    assert( depth->getWidth() != display->getWidth() );
    assert( depth->getHeight() != display->getHeight() );
        
    Intrinsic intrinsic( *depth );
    
    // we are going to limit the region we look at to ignore the sky/car etc
    const int regionTop = depth->getHeight()*0.51f;
    const int regionBase = depth->getHeight()*0.68f;
    assert( regionTop < regionBase );

    // fill the whole background with black
    display->setColor( 0x000000 );
    display->fillRectangle( 0, 0, display->getWidth(), display->getHeight() );

    while( driver.step() != -1 )
    {  
        std::array<std::array<float,3>,2> cones;
        cones[0] = {0,0,0};
        cones[1] = {0,0,0};
        
        // highlight the region of the image we are actually checking.
        display->setColor( 0x333333 );
        display->fillRectangle( 0, regionTop, display->getWidth(), regionBase-regionTop );
        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->drawPixel( x, y );
                }
            }
        }
        
        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;
}