view main/robots/odr/PixyReader.cpp @ 265:1d246b3260c4 main

Add a new module to interface with the Pixycam. Very incomplete. Other tweaking to the ODR app.
author Bob Cook <bob@bobcookdev.com>
date Fri, 25 Mar 2016 12:11:00 -0700
parents
children bf47051f7919
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;
Poco::Timestamp PixyReader::sm_pixyLastUpdate;

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

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" );
    }
}

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

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;
}

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

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()" ) );

    int result = pixy_init();
    if ( result != 0 )
    {
        log.error(
            Poco::Logger::format(
                "PixyReader::configurePixyCam() failed to init: $0 ($1)",
                PixyErrorCodeToString( result ),
                Poco::NumberFormatter::format( result ) ) );
        return false;
    }

    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 )
    {
        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 )
    {
        log.error(
            Poco::Logger::format(
                "PixyReader::processPixyBlocks() failed to read blocks: $0 ($1)",
                PixyErrorCodeToString( numBlocks ),
                Poco::NumberFormatter::format( numBlocks ) ) );
        return;
    }

    // the returned block x,y is the center of the rectangle
    // we should probably filter out things of the wrong aspect ratio

#if 0
    for ( int i = 0; i < numBlocks; ++i )
    {
        log.information(
            Poco::Logger::format( "received pixy x: $0 y: $1 height: $2 width: $3",
                Poco::NumberFormatter::format( pixyBlocks[ i ].x ),
                Poco::NumberFormatter::format( pixyBlocks[ i ].y ),
                Poco::NumberFormatter::format( pixyBlocks[ i ].height ),
                Poco::NumberFormatter::format( pixyBlocks[ i ].width ) ) );
    }
#endif
}

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

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;
            }

            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 );
    }
}

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