view main/robots/odr/PixyApp.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/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"

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

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
}

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

PixyApp::PixyApp()
    : m_helpRequested( false ),
      m_runloopActive( false ),
      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_timerSendPixyDataUpdate.setPeriodicInterval( 750 /* milliseconds */ );
    m_timerSendPixyDataUpdate.start(
            Poco::TimerCallback< PixyApp >( *this, &PixyApp::sendPixyDataUpdate ) );

    m_runloopActive = true;
    while ( m_runloopActive ) Poco::Thread::sleep( 5000 /* milliseconds */ );

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

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