periodicthread.cpp

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

    }

    bool PeriodicThread::run( RunnableInterface* r)
    {
        if ( isRunning() )
            return false;
        if (runComp)
            runComp->setThread(0);
        runComp = r;
        if (runComp)
            runComp->setThread(this);
        return true;
    }


    bool PeriodicThread::start() 
    {
        if ( running ) return false;

        Logger::log() << Logger::Debug << "Periodic Thread "<< rtos_task_get_name(&rtos_task) <<" started."<<Logger::endl;

        bool result;
        result = this->initialize();

        if (result == false) {
            Logger::log() << Logger::Critical << "Periodic Thread "<< rtos_task_get_name(&rtos_task) <<" failed to initialize()."<<Logger::endl;
            return false;
        }

        running=true;

        // be sure that confDone is set to zero
        // could be in case stop() times out.
        rtos_sem_trywait( &confDone );
        // signal start :
        rtos_task_make_periodic(&rtos_task, period );
        int ret = rtos_sem_signal(&sem);
        if ( ret != 0 )
            Logger::log() << Logger::Critical <<"PeriodicThread::start(): sem_signal returns "<< ret << Logger::endl;
        // do not wait, we did our job.

        return true;
    }

    bool PeriodicThread::stop() 
    {
        if ( !running ) return false;

        Logger::log() << Logger::Debug << "Periodic Thread "<< rtos_task_get_name(&rtos_task) <<" stopping...";

        running=false;
        int cnt = 0;
        int ret = -1;

        // wait until the loop detects running == false
        // we wait 10 times 5*period.
        while ( ret == -1 && cnt < 10 )
            {
                // given time in argument is relative to 'now'
                ret = rtos_sem_wait_timed( &confDone, 5*period );
                // if ret == 0, confDone was signaled.
                cnt++;
            } 

        if ( ret != 0 ) {
            Logger::log() << Logger::Debug << " failed."<<Logger::endl;
            Logger::log() << Logger::Critical << "The "<< rtos_task_get_name(&rtos_task) <<" thread seems to be blocked ( ret was "<< ret<<" .)"<<Logger::nl;
        }
        else
            Logger::log() << Logger::Debug << " done."<<Logger::endl;
        // drop out of periodic mode.
        rtos_task_make_periodic(&rtos_task, 0);

        //std::cout <<"Finalizing thread after "<<cnt<<" tries !"<<std::endl;
        // from now on, the thread waits on sem.

        this->finalize();

        return true;
    }

    bool PeriodicThread::isRunning() const
    {
        return running;
    }

    bool PeriodicThread::setScheduler(int sched_type)
    {
        if ( !running ) 
            {
                if ( OS::CheckScheduler(sched_type) == false)
                    return false;
                if ( this->getScheduler() == sched_type ) {
                    log(Debug) << "Scheduler type for Thread "<< rtos_task_get_name(&rtos_task) <<" is already configured as "<< sched_type << endlog();
                    return true;
                }

                log(Error) << "Setting scheduler type for Thread "<< rtos_task_get_name(&rtos_task) <<" to "<< sched_type;
                msched_type = sched_type; 
                rtos_sem_signal(&sem);
                rtos_sem_wait(&confDone);
            }
        else {
            log() << "Failed to change scheduler for "<< rtos_task_get_name(&rtos_task) <<" since thread is still running."<<endlog(Warning);
            return false;
        }
        if (msched_type != rtos_task_get_scheduler(&rtos_task) )
            {
                msched_type = rtos_task_get_scheduler(&rtos_task);
                log() << " failed."<<endlog(Error);
                return false;
            }
        log() << " done."<<endlog(Debug);
        return true;

#if 0 
        // Alternative is not possible for RTAI: can only rt_make_hard_realtime() of calling thread!
        if ( rtos_task_set_scheduler(&rtos_task, sched_type) == 0)
            return true;
        return false;
#endif
    }
    int PeriodicThread::getScheduler() const
    {
        return rtos_task_get_scheduler(&rtos_task);
    }

    void PeriodicThread::configure()
    {
        // reconfigure period
        if ( wait_for_step )
            rtos_task_set_period(&rtos_task, period );
        else
            rtos_task_set_period(&rtos_task, 0);

        if ( msched_type != rtos_task_get_scheduler(&rtos_task) )
            {
                rtos_task_set_scheduler( &rtos_task, msched_type );
                msched_type = rtos_task_get_scheduler(&rtos_task);
            }
    }
        

    void PeriodicThread::step()
    {
        if ( runComp )        
            runComp->step();
    }

    bool PeriodicThread::initialize()
    {
        if ( runComp )
            return runComp->initialize();
            
        return true;
    }

    void PeriodicThread::finalize()
    {
        if ( runComp )
            runComp->finalize();
    }

    bool PeriodicThread::setPeriod( double s )
    {
        if ( isRunning() )
            return false;

        period = Seconds_to_nsecs(s);

        // signal change to thread.
        rtos_sem_signal(&sem);
        rtos_sem_wait(&confDone);

        return true;
    }

    bool PeriodicThread::setPeriod(secs s, nsecs ns) 
    {
        if ( isRunning() ) return false;
        period = ns + 1000*1000*1000*s;

        // signal change to thread.
        rtos_sem_signal(&sem);
        rtos_sem_wait(&confDone);

        return true;
    }

    bool PeriodicThread::setPeriod( TIME_SPEC p) 
    {
        return this->setPeriod( p.tv_sec, p.tv_nsec );
    }

    void PeriodicThread::getPeriod(secs& s, nsecs& ns) const
    {
        s = period/(1000*1000*1000);
        ns = period - s*1000*1000*1000;
    }

    bool PeriodicThread::setPriority(int p) 
    {
        return rtos_task_set_priority(&rtos_task, p) == 0;
    }

    int PeriodicThread::getPriority() const 
    {
        return rtos_task_get_priority(&rtos_task);
    }

    double PeriodicThread::getPeriod() const
    {
        return nsecs_to_Seconds(period);
    }

    nsecs PeriodicThread::getPeriodNS() const
    {
        return period;
    }

    void PeriodicThread::yield() {
        rtos_task_yield( &rtos_task );
    }


    void PeriodicThread::continuousStepping(bool yes_no)
    {
        wait_for_step = !yes_no;
        // do not change the period here, do it in configure()
    }

    void PeriodicThread::terminate()
    {
        // avoid callling twice.
        if (prepareForExit) return;

        prepareForExit = true;
        rtos_sem_signal(&sem);
    
        rtos_sem_wait(&confDone);
        rtos_task_delete(&rtos_task);

    }

    bool PeriodicThread::setToStop()
    {
        running = false;
        this->finalize();
        return true;
    }

    const char* PeriodicThread::getName() const
    {
        return rtos_task_get_name(&rtos_task);
    }

    void PeriodicThread::setMaxOverrun( int m )
    {
        maxOverRun = m;
    }

    int PeriodicThread::getMaxOverrun() const
    {
        return maxOverRun;
    }

}}