view main/robots/odr/ODRApp.cpp @ 230:50414c680910 main

Split the IMU reader out to a separate application. Rework the scoreboard accordingly. Also remove all the legacy "odr-controller" logic, and improve the management of "emergency" situations for all nodes.
author Bob Cook <bob@bobcookdev.com>
date Sat, 12 Jul 2014 17:21:10 -0700
parents c4009187a522
children b9288cad0bfb
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/odr/ODRApp.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//
//  Application object implementation for the outdoor robot project.
//
//  Copyright (c) 2011-2013 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 "ODRApp.h"

#include "AvoidBySonarTask.h"
#include "CruisingTask.h"
#include "MotorsAndServos.h"
#include "NavigateTask.h"
#include "SafetyTask.h"
#include "Scoreboard.h"
#include "Sonar.h"
#include "SquareCourseTask.h"
#include "StartupTask.h"
#include "TaskObject.h"
#include "TrackWaypointsTask.h"

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

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

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

ODRApp::ODRApp()
    : m_helpRequested( false ),
      m_runTaskTimer(),
      m_tasks(),
      m_runloopActive( false ) 
{
}

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

ODRApp::~ODRApp()
{
}

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

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

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

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

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

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

void ODRApp::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<ODRApp>( this, &ODRApp::handleHelp ) ) );
}

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

void ODRApp::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 ODRApp::main( const std::vector<std::string>& args )
{
    if ( m_helpRequested )
    {
        return Poco::Util::Application::EXIT_OK;
    }

    loadConfiguration();

    logger().information( "------------------------------------------------------" );
    logger().information( "ODRApp::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 scoreboardThread;
    Scoreboard   scoreboard( logger().name() );
    scoreboardThread.start( scoreboard );

    m_tasks.clear();

    m_tasks.push_back(
            Poco::SharedPtr< TaskObject >( new SafetyTask( logger().name() ) ) );

    m_tasks.push_back(
            Poco::SharedPtr< TaskObject >( new StartupTask( logger().name() ) ) );

    m_tasks.push_back(
            Poco::SharedPtr< TaskObject >( new AvoidBySonarTask( logger().name() ) ) );

//    m_tasks.push_back(
//            Poco::SharedPtr< TaskObject >( new TrackWaypointsTask( logger().name() ) ) );

//    m_tasks.push_back(
//            Poco::SharedPtr< TaskObject >( new SquareCourseTask( logger().name() ) ) );

    m_tasks.push_back(
            Poco::SharedPtr< TaskObject >( new NavigateTask( logger().name() ) ) );

    m_tasks.push_back(
            Poco::SharedPtr< TaskObject >( new CruisingTask( logger().name() ) ) );

    m_runTaskTimer.setPeriodicInterval( 50 ); // every 50ms, 20 Hz
    Poco::TimerCallback< ODRApp > cb( *this, &ODRApp::runTasks );
    m_runTaskTimer.start( cb );

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

    Scoreboard::sendManagerMessage( "Goodbye" );
    Poco::Thread::sleep( 5000 );

    scoreboard.timeToQuit();
    scoreboardThread.join();

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

    logger().information( "ODR shutdown" );

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

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

void ODRApp::runTasks( Poco::Timer& timer )
{
    // refresh all the tasks
   
    for ( TaskVectorType::iterator itr = m_tasks.begin(); itr != m_tasks.end(); ++itr )
    {
        try
        {
            (*itr)->update();
        }
        catch ( const Poco::Exception& ex )
        {
            logger().error(
                    Poco::Logger::format(
                        "Failed in runTasks(), Poco::Exception: $0", ex.displayText() ) );
        }
        catch ( const std::exception& ex )
        {
            logger().error(
                    Poco::Logger::format(
                        "Failed in runTasks(), std::exception: $0", ex.what() ) );
        }
        catch ( ... )
        {
            logger().error( "Failed in runTasks(), unknown exception" );
        }
    }

    // give control to the highest priority task
    
    for ( TaskVectorType::iterator itr = m_tasks.begin(); itr != m_tasks.end(); ++itr )
    {
        try
        {
            if ( (*itr)->wantsControl() )
            {
                (*itr)->takeControl();
                return;
            }
        }
        catch ( const Poco::Exception& ex )
        {
            logger().error(
                    Poco::Logger::format(
                        "Failed in runTasks(), Poco::Exception: $0", ex.displayText() ) );
        }
        catch ( const std::exception& ex )
        {
            logger().error(
                    Poco::Logger::format(
                        "Failed in runTasks(), std::exception: $0", ex.what() ) );
        }
        catch ( ... )
        {
            logger().error( "Failed in runTasks(), unknown exception" );
        }
    }

    logger().error( "No task accepted control" );
}

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