view main/robots/odr/PixyApp.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/PixyApp.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//
//  Application object for the Pixycam sensor bridge.
//
//  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 <iostream>
#include <memory>
#include <stdio.h>
#include <string>
#include <vector>

#include <Poco/Logger.h>
#include <Poco/Thread.h>
#include <Poco/Util/Application.h>
#include <Poco/Util/HelpFormatter.h>
#include <Poco/Util/LoggingSubsystem.h>
#include <Poco/Util/Option.h>
#include <Poco/Util/OptionSet.h>

#include "PixyApp.h"

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

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

#include "PixyReader.h"

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

// incoming messages: start tracking, stop tracking
// outgoing messages: heartbeat, track info

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

void PixyApp::sendHeartbeat( Poco::Timer& timer )
{
    // if the PixyCam is not online then we don't send any messages

    if ( ! PixyReader::isPixyAlive() )
    {
        return;
    }

    uint32_t heartbeatmsgid = can_build_message_id(
                    can_node_sensor_pixy, can_node_broadcast, can_dataid_heartbeat );

    CANMessage::QueueToSend( new CANMessage( heartbeatmsgid ) );
}

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

void PixyApp::sendPixyDataUpdate( Poco::Timer& timer )
{
    // if the PixyCam is not online then we don't send any messages

    if ( ! PixyReader::isPixyAlive() )
    {
        return;
    }

#if 0
    // ROLL

    imudatamsgid = can_build_message_id(
                    can_node_odr_manager, can_node_broadcast, can_dataid_imu_roll );

    data.data = static_cast< int32_t >( PixyReader::imuRoll() * can_data_imu_multiplier );

    CANMessage::QueueToSend(
            new CANMessage( imudatamsgid,
                            reinterpret_cast< uint8_t* >( &data ),
                            sizeof( data ) ) );
#endif
}

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

void PixyApp::recvSetPixyTracking( bool enabled )
{
    if ( enabled )
    {
        m_timerSendPixyDataUpdate.start(
            Poco::TimerCallback< PixyApp >( *this, &PixyApp::sendPixyDataUpdate ) );
    }
    else
    {
        m_timerSendPixyDataUpdate.stop();
    }
}

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

void PixyApp::recvEmergency( uint8_t srcNode )
{
#if 0
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    sm_isEmergencyActive = true;

    switch ( srcNode )
    {
        case can_node_odr_display:
            sm_DisplayEmergency = true;
            break;

        case can_node_sensor_gps:
            sm_GpsEmergency = true;
            break;

        case can_node_odr_motion:
            sm_MotionEmergency = true;
            break;

        case can_node_odr_sonar_front:
            sm_SonarFrontEmergency = true;
            break;
    }
#endif
}

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

void PixyApp::recvAllClear( uint8_t srcNode )
{
#if 0
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    switch ( srcNode )
    {
        case can_node_odr_display:
            sm_DisplayEmergency = false;
            break;

        case can_node_sensor_gps:
            sm_GpsEmergency = false;
            break;

        case can_node_odr_motion:
            sm_MotionEmergency = false;
            break;

        case can_node_odr_sonar_front:
            sm_SonarFrontEmergency = false;
            break;
    }
#endif
}

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

void PixyApp::recvHeartbeat( uint8_t srcNode )
{
#if 0
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    switch ( srcNode )
    {
        case can_node_odr_display:
            m_timerDisplayTimeout.restart();
            sm_isDisplayAlive = true;
            break;

        case can_node_sensor_gps:
            m_timerGpsTimeout.restart();
            sm_isGpsAlive = true;
            break;

        case can_node_odr_motion:
            m_timerMotionTimeout.restart();
            sm_isMotionAlive = true;
            break;
    }
#endif
}

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

void PixyApp::runLoop( const std::string& loggerName )
{
    Poco::Logger& log = Poco::Logger::get( loggerName );

    for ( ;; )
    {
        log.information( std::string( "PixyApp::runLoop() starting" ) );

        try
        {
            for ( ;; )
            {
                CANMessage* msg = CANMessage::WaitDequeueReceived( 250 ); // 250 ms

                if ( msg == 0 && m_quitEvent.tryWait( 0 ) )
                {
                    log.information( std::string( "PixyApp::runLoop() stopping" ) );
                    return;
                }

                if ( msg == 0 )
                {
                    continue;
                }

                uint8_t  srcNode;
                uint8_t  dstNode;
                uint16_t dataId;

                can_parse_message_id( 
                        msg->msgIdentifier(), &srcNode, &dstNode, &dataId );

                switch ( dataId )
                {
                    case can_dataid_emergency:
                        recvEmergency( srcNode );
                        break;

                    case can_dataid_all_clear:
                        recvAllClear( srcNode );
                        break;

                    case can_dataid_heartbeat:
                        recvHeartbeat( srcNode );
                        break;

                    case can_dataid_pixy_start_tracking:
                        recvSetPixyTracking( true );
                        break;

                    case can_dataid_pixy_stop_tracking:
                        recvSetPixyTracking( false );
                        break;
                }
            }
        }
        catch ( const Poco::Exception& ex )
        {
            log.error(
                Poco::Logger::format(
                    "PixyApp::runLoop() caught Poco::Exception: $0", ex.displayText() ) );
        }
        catch ( const std::exception& ex )
        {
            log.error(
                Poco::Logger::format(
                    "PixyApp::runLoop() caught std::exception: $0", ex.what() ) );
        }
        catch ( ... )
        {
            log.error( "PixyApp::runLoop() caught unknown exception" );
        }

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

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

PixyApp::PixyApp()
    : m_helpRequested( false ),
      m_quitEvent( false /* not auto-reset */ ),
      m_timerSendHeartbeat(),
      m_timerSendPixyDataUpdate()
{
}

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

PixyApp::~PixyApp()
{
}

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

void PixyApp::initialize( Poco::Util::Application& self )
{
    loadConfiguration();
    Poco::Util::ServerApplication::initialize( self );

    addSubsystem( new Poco::Util::LoggingSubsystem() );
}

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

void PixyApp::uninitialize()
{
    Poco::Util::ServerApplication::uninitialize();
}

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

void PixyApp::defineOptions( Poco::Util::OptionSet& options )
{
    Poco::Util::ServerApplication::defineOptions( options );

    options.addOption(
        Poco::Util::Option( "help", "h", "display argument help information" )
            .required( false )
            .repeatable( false )
            .callback( Poco::Util::OptionCallback<PixyApp>( this,
                                                            &PixyApp::handleHelp ) ) );
}

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

void PixyApp::handleHelp( const std::string& name, const std::string& value )
{
    Poco::Util::HelpFormatter helpFormatter( options() );
    helpFormatter.setCommand( commandName() );
    helpFormatter.format( std::cout );
    stopOptionsProcessing();
    m_helpRequested = true;
}

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

int PixyApp::main( const std::vector<std::string>& args )
{
    if ( m_helpRequested )
    {
        return Poco::Util::Application::EXIT_OK;
    }

    loadConfiguration();

    logger().information( "------------------------------------------------------" );
    logger().information( "PixyApp::main() started" );

    std::string canInterfaceName = config().getString( "can.interfaceName", "can0" );
    logger().information( "using CANbus interface name \"" + canInterfaceName + "\"" );

    Poco::Thread    canMsgProcessorThread;
    CANMsgProcessor canMsgProcessor( canInterfaceName, logger().name() );
    canMsgProcessor.logIncomingMessages( config().getBool( "can.logIncoming", "no" ) );
    canMsgProcessor.logOutgoingMessages( config().getBool( "can.logOutgoing", "no" ) );
    canMsgProcessorThread.start( canMsgProcessor );

    Poco::Thread pixyReaderThread;
    PixyReader   pixyReader( logger().name() );
    pixyReaderThread.start( pixyReader );

    m_timerSendHeartbeat.setPeriodicInterval( 750 /* milliseconds */ );
    m_timerSendHeartbeat.start(
            Poco::TimerCallback< PixyApp >( *this, &PixyApp::sendHeartbeat ) );

    m_timerSendPixyDataUpdate.setPeriodicInterval( 750 /* milliseconds */ );

    runLoop( logger().name() );

    pixyReader.timeToQuit();
    pixyReaderThread.join();

    canMsgProcessor.timeToQuit();
    canMsgProcessorThread.join();

    logger().information( "PixyApp::main() stopping" );

    return Poco::Util::Application::EXIT_OK;
}

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

int main( int argc, char** argv )
{
    int result = -1;

    try
    {
        PixyApp app;
        result = app.run( argc, argv );
    }
    catch ( const Poco::Exception& ex )
    {
        std::cerr << "Failed to start application, Poco::Exception: "
            << ex.displayText() << std::endl;
    }
    catch ( const std::exception& ex )
    {
        std::cerr << "Failed to start application, std::exception: "
            << ex.what() << std::endl;
    }
    catch ( ... )
    {
        std::cerr << "Failed to start application, unknown exception" << std::endl;
    }

    return result;
}

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