periodicthread.cpp

机器人开源项目orocos的源代码

/***************************************************************************
  tag: Peter Soetens  Mon May 10 19:10:28 CEST 2004  PeriodicThread.cxx 

                        PeriodicThread.cxx -  description
                           -------------------
    begin                : Mon May 10 2004
    copyright            : (C) 2004 Peter Soetens
    email                : [e-mail]
 
 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU General Public                   *
 *   License as published by the Free Software Foundation;                 *
 *   version 2 of the License.                                             *
 *                                                                         *
 *   As a special exception, you may use this file as part of a free       *
 *   software library without restriction.  Specifically, if other files   *
 *   instantiate templates or use macros or inline functions from this     *
 *   file, or you compile this file and link it with other files to        *
 *   produce an executable, this file does not by itself cause the         *
 *   resulting executable to be covered by the GNU General Public          *
 *   License.  This exception does not however invalidate any other        *
 *   reasons why the executable file might be covered by the GNU General   *
 *   Public License.                                                       *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU General Public             *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/

#include "fosi_internal_interface.hpp"
#include <os/PeriodicThread.hpp>
#include <os/Time.hpp>
#include "os/threads.hpp"
#include "Logger.hpp"

#include "../rtt-config.h"

#ifdef OROPKG_OS_THREAD_SCOPE
# include "dev/DigitalOutInterface.hpp"
#endif

namespace RTT
{ namespace OS {
    using RTT::Logger;
    using namespace detail;

    void *periodicThread(void* t) 
    {
        /**
         * This is one time initialisation
         */
        PeriodicThread* task = static_cast<OS::PeriodicThread*> (t);
        // acquire the resulting scheduler type.
        //task->msched_type = rtos_task_get_scheduler( task->getTask() );
        task->configure();

#ifdef OROPKG_OS_THREAD_SCOPE
        // order thread scope toggle bit on thread number
        unsigned int bit = task->threadNumber();
        if ( task->d )
            task->d->switchOff( bit );
#endif // OROPKG_OS_THREAD_SCOPE
        int overruns = 0;
        while ( !task->prepareForExit ) {
#ifndef ORO_EMBEDDED
            try {
#endif // !ORO_EMBEDDED
                /**
                 * The real task starts here.
                 */
                while(1) 
                    {
                        if( !task->running ) { // more efficient than calling isRunning()
                            // consider this the 'configuration state'
                            overruns = 0;
                            // drop out of periodic mode:
                            rtos_task_set_period(task->getTask(), 0);
                            //                             Logger::log() << Logger::Info <<rtos_task_get_name(&rtos_task)<<"  signals done !" << Logger::endl;
                            rtos_sem_signal( &(task->confDone) ); // signal we are ready for command
                            //                             Logger::log() << Logger::Info <<rtos_task_get_name(&rtos_task)<<"  sleeps !" << Logger::endl;
                            rtos_sem_wait( &(task->sem) );      // wait for command.
                            //                             Logger::log() << Logger::Info <<rtos_task_get_name(&rtos_task)<<"  awoken !" << Logger::endl;
                            task->configure();             // check for reconfigure
                            //                             Logger::log() << Logger::Info <<rtos_task_get_name(&rtos_task)<<"  config done !" << Logger::endl;
                            if ( task->prepareForExit )    // check for exit
                                {
                                    break; // break while(1) {}
                                }
                            // end of configuration
                        } else {
                            // task->isRunning()
#ifdef OROPKG_OS_THREAD_SCOPE
                            if ( task->d )
                                task->d->switchOn( bit );
#endif
                            {
                                task->step(); // one cycle
                            }
#ifdef OROPKG_OS_THREAD_SCOPE
                            if ( task->d )
                                task->d->switchOff( bit );
#endif
                            // return non-zero to indicate overrun.
                            if ( rtos_task_wait_period( task->getTask() ) != 0) {
                                ++overruns;
                                if ( overruns == task->maxOverRun )
                                    break;
                            } else if ( overruns != 0 )
                                --overruns;
                        }
                    }
                if ( overruns == task->maxOverRun ) {
#ifdef OROPKG_OS_THREAD_SCOPE
                    if ( task->d )
                        task->d->switchOff( bit );
#endif
                    task->emergencyStop();
                    Logger::In in(rtos_task_get_name(task->getTask()));
                    Logger::log() << Logger::Fatal << rtos_task_get_name(task->getTask()) <<" got too many periodic overruns in step() ("<< overruns << " times), stopped Thread !"<<Logger::nl;
                    Logger::log() <<" See PeriodicThread::setMaxOverrun() for info." << Logger::endl;
                }
#ifndef ORO_EMBEDDED
            } catch( ... ) {
#ifdef OROPKG_OS_THREAD_SCOPE
                if ( task->d )
                    task->d->switchOff( bit );
#endif
                task->emergencyStop();
                Logger::In in(rtos_task_get_name(task->getTask()));
                Logger::log() << Logger::Fatal << rtos_task_get_name(task->getTask()) <<" caught a C++ exception, stopped thread !"<<Logger::endl;
            }
#endif // !ORO_EMBEDDED
        } // while (!prepareForExit)
    
        rtos_sem_signal( &(task->confDone));

        return 0;
    }

    void PeriodicThread::emergencyStop()
    {
        // set state to not running
        this->running = false;
        // execute finalize in current mode, even if hard.
        this->finalize();
    }

    PeriodicThread::PeriodicThread(int _priority, 
                                   const std::string & name, 
                                   Seconds periods, 
                                   RunnableInterface* r) :
        msched_type(ORO_SCHED_RT), running(false), prepareForExit(false),
        wait_for_step(true), runComp(r), 
        maxOverRun( OROSEM_OS_PERIODIC_THREADS_MAX_OVERRUN),
        period(Seconds_to_nsecs(periods))    // Do not call setPeriod(), since the semaphores are not yet used !
#ifdef OROPKG_OS_THREAD_SCOPE
							 ,d(NULL)
#endif
    {
        this->setup(_priority, name);
    }

    PeriodicThread::PeriodicThread(int scheduler, int _priority, 
                                   const std::string & name, 
                                   Seconds periods, 
                                   RunnableInterface* r) :
        msched_type(scheduler), running(false), prepareForExit(false),
        wait_for_step(true), runComp(r), 
        maxOverRun( OROSEM_OS_PERIODIC_THREADS_MAX_OVERRUN),
        period(Seconds_to_nsecs(periods))    // Do not call setPeriod(), since the semaphores are not yet used !
#ifdef OROPKG_OS_THREAD_SCOPE
							 ,d(NULL)
#endif
    {
        log(Info) << "Creating PeriodicThread for scheduler: "<< scheduler << endlog();
        this->setup(_priority, name);
    }

    void PeriodicThread::setup(int _priority, const std::string& name) 
    {
        int ret;
        
        ret = rtos_sem_init(&sem, 0);
        if ( ret != 0 ) {
            Logger::In in("PeriodicThread");
            Logger::log() << Logger::Critical << "Could not allocate configuration semaphore 'sem' for "<< rtos_task_get_name(&rtos_task) <<". Throwing std::bad_alloc."<<Logger::endl;
            rtos_sem_destroy( &sem );
#ifndef ORO_EMBEDDED
            throw std::bad_alloc();
#else
            return;
#endif
        }

        ret = rtos_sem_init(&confDone, 0);
        if ( ret != 0 ) {
            Logger::In in("PeriodicThread");
            Logger::log() << Logger::Critical << "Could not allocate configuration semaphore 'confDone' for "<< rtos_task_get_name(&rtos_task) <<". Throwing std::bad_alloc."<<Logger::endl;
            rtos_sem_destroy( &sem );
#ifndef ORO_EMBEDDED
            throw std::bad_alloc();
#else
            return;
#endif
        }

        if (runComp)
            runComp->setThread(this);
#ifdef OROPKG_OS_THREAD_SCOPE
        // Check if threadscope device already exists
        {
            Logger::In in("PeriodicThread");
            if ( DigitalOutInterface::nameserver.getObject("ThreadScope") ){
                d = DigitalOutInterface::nameserver.getObject("ThreadScope");
            }
            else{
                log(Warning) << "Failed to find 'ThreadScope' object in DigitalOutInterface::nameserver." << endlog();
            }
        }
#endif
        int rv = rtos_task_create(&rtos_task, _priority, name.c_str(), msched_type, periodicThread, this );
        if ( rv != 0 ) {
            Logger::In in("PeriodicThread");
            Logger::log() << Logger::Critical << "Could not create thread "
                          << rtos_task_get_name(&rtos_task) <<"."<<Logger::endl;
            rtos_sem_destroy( &sem );
            rtos_sem_destroy( &confDone );
#ifndef ORO_EMBEDDED
            throw std::bad_alloc();
#else
            return;
#endif
        }
 
        rtos_sem_wait(&confDone); // wait until thread is created.
 
        const char* modname = getName();
        Logger::In in(modname);
        log(Info)<< "PeriodicThread created with scheduler type '"<< getScheduler() <<"', priority " << getPriority()
                 <<" and period "<< getPeriod() << "." << endlog();
#ifdef OROPKG_OS_THREAD_SCOPE
        if (d){
            unsigned int bit = threadNumber();
            log(Info) << "ThreadScope :"<< modname <<" toggles bit "<< bit << endlog();
        }
#endif
    }
    
    PeriodicThread::~PeriodicThread() 
    {
        Logger::In in("~PeriodicThread");
        if (this->isRunning())
            this->stop();

        log(Debug) << "Terminating "<< this->getName() <<endlog();
        terminate();
        log(Debug) << " done"<< endlog();
        rtos_sem_destroy(&confDone);
        rtos_sem_destroy(&sem);

        if (runComp)
            runComp->setThread(0);