view main/robots/odr/PixyReader.cpp @ 267:bf47051f7919 main

PixyCam control software.
author Bob Cook <bob@bobcookdev.com>
date Sun, 01 May 2016 14:22:14 -0700
parents 1d246b3260c4
children f2546f8b4ba7
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/odr/PixyReader.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Runnable object that reads and parses messages from a Pixycam.
//
//  Copyright (c) 2016 Bob Cook
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//
// ----------------------------------------------------------------------------------------

#include "PixyReader.h"

#include <stdio.h>
#include <pixy.h>

//#include <algorithm>
//#include <cmath>
//#include <stdexcept>
//#include <utility>
//#include <vector>

#include <Poco/Exception.h>
#include <Poco/Logger.h>
#include <Poco/NumberFormatter.h>
#include <Poco/NumberParser.h>
#include <Poco/Thread.h>

#include "packages/common/can/can_helpers.h"
#include "packages/common/can/can_messages.h"
#include "packages/common/can/can_nodes.h"

#include "packages/common/util/misc.h"

#include "packages/linux/can/CANMessage.h"

// ----------------------------------------------------------------------------------------

Poco::RWLock    PixyReader::sm_rwLock;
bool            PixyReader::sm_isPixyAlive = false;
Poco::Timestamp PixyReader::sm_pixyLastUpdate;
bool            PixyReader::sm_hasTarget = false;
int8_t          PixyReader::sm_targetPosition;

// ----------------------------------------------------------------------------------------

static std::string PixyErrorCodeToString( int error )
{
    switch ( error )
    {
        case 0:
            return std::string( "NO_ERROR" );

        case PIXY_ERROR_USB_IO:
            return std::string( "USB_IO" );

        case PIXY_ERROR_USB_NOT_FOUND:
            return std::string( "USB_NOT_FOUND" );

        case PIXY_ERROR_USB_BUSY:
            return std::string( "USB_BUSY" );

        case PIXY_ERROR_USB_NO_DEVICE:
            return std::string( "USB_NO_DEVICE" );

        case PIXY_ERROR_INVALID_PARAMETER:
            return std::string( "INVALID_PARAMETER" );

        case PIXY_ERROR_CHIRP:
            return std::string( "CHIRP" );

        case PIXY_ERROR_INVALID_COMMAND:
            return std::string( "INVALID_COMMAND" );

        default:
            return std::string( "unknown" );
    }
}

// ----------------------------------------------------------------------------------------

static const int8_t MAX_LUT_VALUE =  10;
static const int8_t MIN_LUT_VALUE = -10;

static int8_t XCoordinateToPosition( uint16_t xCoord )
{
    static std::vector< int8_t > s_positionLut;

    if ( s_positionLut.size() == 0 )
    {
        s_positionLut.reserve( PIXY_MAX_X - PIXY_MIN_X + 1 );

        double scale_factor = static_cast< double >( MAX_LUT_VALUE * 2 );
        scale_factor /= static_cast< double >( PIXY_MAX_X - PIXY_MIN_X );

        for ( uint16_t i = PIXY_MIN_X; i <= PIXY_MAX_X; i++ )
        {
            double v = static_cast< double >( i );

            if ( i < ( PIXY_MAX_X / 2 ) )
            {
                v = static_cast< double >( PIXY_MAX_X / 2 ) - v;
                v *= -1.0;
            }
            else
            {
                v -= static_cast< double >( PIXY_MAX_X / 2 );
            }

            v *= scale_factor;

            s_positionLut.push_back( static_cast< int8_t >( v ) );
        }

        s_positionLut[ 0 ] = MIN_LUT_VALUE; // force boundries
    }

    if ( s_positionLut.size() <= xCoord )
    {
        return MAX_LUT_VALUE;
    }

    return s_positionLut[ xCoord ];
}

// ----------------------------------------------------------------------------------------

void PixyReader::pixyIsOnline()
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isPixyAlive = true;
}

// ----------------------------------------------------------------------------------------

void PixyReader::pixyIsOffline()
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isPixyAlive = false;
}

// ----------------------------------------------------------------------------------------

bool PixyReader::isPixyAlive()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );

    if ( ! sm_isPixyAlive )
    {
        return false;
    }

    // check if the IMU is actually offline; more than 2 seconds silence means yes

    static const Poco::Timestamp::TimeDiff TwoSeconds = Poco::Timestamp::resolution() * 2;

    if ( sm_pixyLastUpdate.elapsed() > TwoSeconds )
    {
        sm_isPixyAlive = false;
    }

    return sm_isPixyAlive;
}

// ----------------------------------------------------------------------------------------

bool PixyReader::hasTarget( int8_t* position )
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );

    if ( sm_hasTarget && position != NULL )
    {
        *position = sm_targetPosition;
    }
    
    return sm_hasTarget;
}

// ----------------------------------------------------------------------------------------

PixyReader::PixyReader( const std::string& loggerName )
    : Poco::Runnable(),
      m_loggerName( loggerName ),
      m_quitEvent( false /* not auto-reset */ )
{
}

// ----------------------------------------------------------------------------------------

PixyReader::~PixyReader()
{
    closePixyCam();
}

// ----------------------------------------------------------------------------------------

void PixyReader::timeToQuit()
{
    m_quitEvent.set();
}

// ----------------------------------------------------------------------------------------

void PixyReader::closePixyCam()
{
    pixy_close();
}

// ----------------------------------------------------------------------------------------

bool PixyReader::configurePixyCam()
{
    Poco::Logger& log = Poco::Logger::get( m_loggerName );

    log.information( std::string( "PixyReader::configurePixyCam()" ) );

#if 0
    for ( uint16_t i = PIXY_MIN_X; i <= PIXY_MAX_X; i++ )
    {
        log.information(
            Poco::Logger::format(
                "PixyReader::configurePixyCam(): $0 = $1",
                Poco::NumberFormatter::format( i ),
                Poco::NumberFormatter::format( XCoordinateToPosition( i ) ) ) );
    }
#endif

    int result = pixy_init();
    if ( result != 0 )
    {
        pixyIsOffline();

        log.error(
            Poco::Logger::format(
                "PixyReader::configurePixyCam() failed to init: $0 ($1)",
                PixyErrorCodeToString( result ),
                Poco::NumberFormatter::format( result ) ) );

        return false;
    }

    pixyIsOnline();

    uint16_t pixy_version_major;
    uint16_t pixy_version_minor;
    uint16_t pixy_version_build;

    result = pixy_get_firmware_version( &pixy_version_major,
                                        &pixy_version_minor,
                                        &pixy_version_build );
    if ( result != 0 )
    {
        pixyIsOffline();

        log.error(
            Poco::Logger::format(
                "PixyReader::configurePixyCam() failed to read f/w version: $0 ($1)",
                PixyErrorCodeToString( result ),
                Poco::NumberFormatter::format( result ) ) );

        return false;
    }

    log.information(
        Poco::Logger::format(
            "PixyReader::configurePixyCam() firmware version $0.$1.$2",
            Poco::NumberFormatter::format( pixy_version_major ),
            Poco::NumberFormatter::format( pixy_version_minor ),
            Poco::NumberFormatter::format( pixy_version_build ) ) );

    log.information( "PixyReader::configurePixyCam() successfully initalized" );

    return true;
}

// ----------------------------------------------------------------------------------------

void PixyReader::processPixyBlocks()
{
    Poco::Logger& log = Poco::Logger::get( m_loggerName );

    // maximum number of Pixy Blocks to process at once
    static const int PIXY_BLOCK_BUFFER_SIZE = 25;

    // Pixy Block buffer
    struct Block pixyBlocks[ PIXY_BLOCK_BUFFER_SIZE ];

    int numBlocks = pixy_get_blocks( PIXY_BLOCK_BUFFER_SIZE, pixyBlocks );

    if ( numBlocks < 0 )
    {
        pixyIsOffline();

        log.error(
            Poco::Logger::format(
                "PixyReader::processPixyBlocks() failed to read blocks: $0 ($1)",
                PixyErrorCodeToString( numBlocks ),
                Poco::NumberFormatter::format( numBlocks ) ) );
        return;
    }

    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    if ( numBlocks == 0 )
    {
        sm_hasTarget = false;
        return; // weird but true
    }

    // compute the ratio of height to width and surface area for each block, rejecting
    // blocks that are really the wrong ratio

    double heightToWidthRatio[ PIXY_BLOCK_BUFFER_SIZE ];
    int    surfaceArea[ PIXY_BLOCK_BUFFER_SIZE ];

    for ( int i = 0; i < numBlocks; ++i )
    {
        if ( pixyBlocks[ i ].height < 10 || pixyBlocks[ i ].width < 10 )
        {
            heightToWidthRatio[ i ] = 0;
            surfaceArea[ i ] = 0;
            continue;
        }

        double ratio = (double)(pixyBlocks[ i ].height) / (double)(pixyBlocks[ i ].width);

        if ( ratio < 1.0 || ratio > 2.0 )
        {
            heightToWidthRatio[ i ] = 0;
            surfaceArea[ i ] = 0;
            continue;
        }

        surfaceArea[ i ] = pixyBlocks[ i ].height * pixyBlocks[ i ].width;
        heightToWidthRatio[ i ] = ratio;

        if ( surfaceArea[ i ] < 1000 )
        {
            heightToWidthRatio[ i ] = 0;
            surfaceArea[ i ] = 0;
        }
    }

    // find the largest block

    int largestSurfaceArea = surfaceArea[ 0 ];
    int indexOfLargest = 0;

    for ( int i = 1; i < numBlocks; ++i )
    {
        if ( surfaceArea[ i ] > largestSurfaceArea )
        {
            largestSurfaceArea = surfaceArea[ i ];
            indexOfLargest = i;
        }
    }

    for ( int i = 0; i < numBlocks; ++i )
    {
        if ( surfaceArea[ i ] == 0 )
        {
            continue;
        }

#if 1
        log.information(
            Poco::Logger::format( "received pixy x: $0 y: $1 ratio: $2 area: $3",
                Poco::NumberFormatter::format( pixyBlocks[ i ].x ),
                Poco::NumberFormatter::format( pixyBlocks[ i ].y ),
                Poco::NumberFormatter::format( heightToWidthRatio[ i ], 6, 4 ),
                Poco::NumberFormatter::format( surfaceArea[ i ] ) ) );
#endif
    }

    if ( largestSurfaceArea > 0 )
    {
        sm_hasTarget      = true;
        sm_targetPosition = XCoordinateToPosition( pixyBlocks[ indexOfLargest ].x );

        log.information(
            Poco::Logger::format( "choose candidate block $0 (ratio $1, area $2, pos $3)",
                Poco::NumberFormatter::format( indexOfLargest ),
                Poco::NumberFormatter::format( heightToWidthRatio[ indexOfLargest ], 6, 4 ),
                Poco::NumberFormatter::format( surfaceArea[ indexOfLargest ] ),
                Poco::NumberFormatter::format( sm_targetPosition ) ) );
    }
    else
    {
        sm_hasTarget = false;
    }
}

// ----------------------------------------------------------------------------------------

void PixyReader::run()
{
    Poco::Logger& log = Poco::Logger::get( m_loggerName );

    log.information( std::string( "PixyReader::run() starting" ) );

    for ( ;; )
    {
        try
        {
            // now try to find the PixyCam

            if ( ! configurePixyCam() )
            {
                Poco::Thread::sleep( 10000 ); // 10 sec
                continue;
            }

            // read data from the PixyCam until told to quit

            for ( ;; )
            {
                if ( m_quitEvent.tryWait( 0 ) )
                {
                    log.information( std::string( "PixyReader::run() stopping" ) );
                    return;
                }

                if ( pixy_blocks_are_new() > 0 )
                {
                    processPixyBlocks();
                }

                //Poco::Thread::sleep( 1000 ); // 1 sec
            }
#if 0
            resetParseState();

            // keep the last 10 values to average together (approx 0.5 seconds elapsed)

            std::vector< double > valuesYaw;
            valuesYaw.reserve( 10 );

            // timeout after 10 consecutive runs of not getting data

            int timeoutNoData = 0;

            // loop while we are still getting data

            for ( ;; )
            {
                if ( m_quitEvent.tryWait( 0 ) )
                {
                    log.information( std::string( "PixyReader::run() stopping" ) );
                    return;
                }

                char   buffer[ 64 ];
                size_t length = array_sizeof( buffer );

                if ( ! readFromSerialPort( buffer, &length ) )
                {
                    break; // error, drop out and reinit the port
                }

                if ( length == 0 )
                {
                    if ( ++timeoutNoData == 10 )
                    {
                        PixyReader::imuIsOffline();
                    }
                    continue;
                }

                // got some bytes, try to parse them

                timeoutNoData = 0;

                if ( processPixyData( buffer, length ) )
                {
                    // new values available, save them

                    valuesYaw.push_back( calibrateYawValue( m_parseValueYaw ) );

                    // if we've reached 10 values, compute the average and update the
                    // scoreboard; then clear the vector making room for new values

                    if ( valuesYaw.size() == 10 )
                    {
                        double averageYaw = computeAverageYaw( valuesYaw );

                        PixyReader::imuUpdate( m_parseValueRoll,
                                              m_parseValuePitch,
                                              averageYaw );

                        // clear the vector, to accumulate more samples

                        valuesYaw.clear();
                    }
                }
            }
#endif
        }
        catch ( const Poco::Exception& ex )
        {
            log.error(
                Poco::Logger::format(
                    "PixyReader::run() caught Poco::Exception: $0", ex.displayText() ) );
        }
        catch ( const std::exception& ex )
        {
            log.error(
                Poco::Logger::format(
                    "PixyReader::run() caught std::exception: $0", ex.what() ) );
        }
        catch ( ... )
        {
            log.error( "PixyReader::run() caught unknown exception" );
        }

        // sleep for 1/2 second after exception processing, but don't exit
        Poco::Thread::sleep( 500 );
    }
}

// ----------------------------------------------------------------------------------------