Orocos Real-Time Toolkit  2.6.0
TimerThread.cpp
00001 /***************************************************************************
00002  tag: Peter Soetens  Wed Apr 17 16:01:31 CEST 2002  TimerThread.cpp
00003 
00004                        TimerThread.cpp -  description
00005                           -------------------
00006    begin                : Wed April 17 2002
00007    copyright            : (C) 2002 Peter Soetens
00008    email                : peter.soetens@mech.kuleuven.ac.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU General Public                   *
00013  *   License as published by the Free Software Foundation;                 *
00014  *   version 2 of the License.                                             *
00015  *                                                                         *
00016  *   As a special exception, you may use this file as part of a free       *
00017  *   software library without restriction.  Specifically, if other files   *
00018  *   instantiate templates or use macros or inline functions from this     *
00019  *   file, or you compile this file and link it with other files to        *
00020  *   produce an executable, this file does not by itself cause the         *
00021  *   resulting executable to be covered by the GNU General Public          *
00022  *   License.  This exception does not however invalidate any other        *
00023  *   reasons why the executable file might be covered by the GNU General   *
00024  *   Public License.                                                       *
00025  *                                                                         *
00026  *   This library is distributed in the hope that it will be useful,       *
00027  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00028  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00029  *   Lesser General Public License for more details.                       *
00030  *                                                                         *
00031  *   You should have received a copy of the GNU General Public             *
00032  *   License along with this library; if not, write to the Free Software   *
00033  *   Foundation, Inc., 59 Temple Place,                                    *
00034  *   Suite 330, Boston, MA  02111-1307  USA                                *
00035  *                                                                         *
00036  ***************************************************************************/
00037 
00038 #include "TimerThread.hpp"
00039 #include "PeriodicActivity.hpp"
00040 
00041 #include "rtt-config.h"
00042 
00043 #include "../Time.hpp"
00044 #include "../Logger.hpp"
00045 #include <algorithm>
00046 #include "../os/MutexLock.hpp"
00047 
00048 namespace RTT {
00049     using namespace extras;
00050     using namespace base;
00051     using os::MutexLock;
00052     using namespace std;
00053 
00054     TimerThread::TimerThreadList TimerThread::TimerThreads;
00055 
00056     TimerThreadPtr TimerThread::Instance(int pri, double per)
00057     {
00058         return Instance(ORO_SCHED_RT, pri, per);
00059     }
00060 
00061     TimerThreadPtr TimerThread::Instance(int scheduler, int pri, double per)
00062     {
00063       return Instance(scheduler, pri, per, ~0);
00064     }
00065 
00066     TimerThreadPtr TimerThread::Instance(int scheduler, int pri, double per, unsigned cpu_affinity)
00067     {
00068         // Since the period is stored as nsecs, we convert per to NS in order
00069         // to get a match.
00070         os::CheckPriority(scheduler, pri);
00071         TimerThreadList::iterator it = TimerThreads.begin();
00072         while ( it != TimerThreads.end() ) {
00073             TimerThreadPtr tptr = it->lock();
00074             // detect old pointer.
00075             if ( !tptr ) {
00076                 TimerThreads.erase(it);
00077                 it = TimerThreads.begin();
00078                 continue;
00079             }
00080             if ( tptr->getScheduler() == scheduler && tptr->getPriority() == pri && tptr->getPeriodNS() == Seconds_to_nsecs(per) ) {
00081                 return tptr;
00082             }
00083             ++it;
00084         }
00085         TimerThreadPtr ret( new TimerThread(scheduler, pri, "TimerThreadInstance", per, cpu_affinity) );
00086         TimerThreads.push_back( ret );
00087         return ret;
00088     }
00089 
00090     TimerThread::TimerThread(int priority, const std::string& name, double periodicity, unsigned cpu_affinity)
00091         : Thread( ORO_SCHED_RT, priority, periodicity, cpu_affinity, name), cleanup(false)
00092     {
00093         tasks.reserve(MAX_ACTIVITIES);
00094     }
00095 
00096     TimerThread::TimerThread(int scheduler, int priority, const std::string& name, double periodicity, unsigned cpu_affinity)
00097         : Thread(scheduler, priority, periodicity, cpu_affinity, name), cleanup(false)
00098     {
00099         tasks.reserve(MAX_ACTIVITIES);
00100     }
00101 
00102     TimerThread::~TimerThread()
00103     {
00104         // make sure the thread does not run when we start deleting clocks...
00105         this->stop();
00106     }
00107 
00108     bool TimerThread::addActivity( PeriodicActivity* t ) {
00109         MutexLock lock(mutex);
00110         if ( tasks.size() == MAX_ACTIVITIES ) {
00111 //             Logger::log() << Logger:: << "TimerThread : tasks queue full, failed to add Activity : "<< t << Logger::endl;
00112             return false;
00113         }
00114         tasks.push_back( t );
00115 //         Logger::log() << Logger::Debug << "TimerThread : successfully started Activity : "<< t  << Logger::endl;
00116         return true;
00117     }
00118 
00119     bool TimerThread::removeActivity( PeriodicActivity* t ) {
00120         MutexLock lock(mutex);
00121         ActivityList::iterator it = find(tasks.begin(), tasks.end(), t);
00122         if ( it != tasks.end() ) {
00123             *it = 0; // clear task away
00124             cleanup = true;
00125             return true;
00126         }
00127 //         Logger::log() << Logger::Debug << "TimerThread : failed to stop Activity : "<< t->getPeriod() << Logger::endl;
00128         return false;
00129     }
00130 
00131     bool TimerThread::initialize() {
00132         return true;
00133     }
00134 
00135     void TimerThread::finalize() {
00136         MutexLock lock(mutex);
00137 
00138         for( ActivityList::iterator t_iter = tasks.begin(); t_iter != tasks.end(); ++t_iter)
00139             if ( *t_iter )
00140                 (*t_iter)->stop(); // stop() calls us back to removeActivity (recursive mutex).
00141         if ( cleanup )
00142             this->reorderList();
00143     }
00144 
00145     void TimerThread::step() {
00146         MutexLock lock(mutex);
00147 
00148         // The size of the tasks vector does not change during add/remove, thus
00149         // t_iter is never invalidated.
00150         for( ActivityList::iterator t_iter = tasks.begin(); t_iter != tasks.end(); ++t_iter)
00151             if ( *t_iter )
00152                 (*t_iter)->step();
00153 
00154         if ( cleanup )
00155             this->reorderList();
00156     }
00157 
00158     void TimerThread::reorderList() {
00159         // reorder the list to remove clear'ed tasks
00160         ActivityList::iterator begin = tasks.begin();
00161         // first zero :
00162         PeriodicActivity* nullActivity = 0;
00163         ActivityList::iterator it = tasks.begin();
00164 
00165         it = find( tasks.begin(), tasks.end(), nullActivity); // First zero
00166         begin = it+1;
00167         while ( it != tasks.end() ) {
00168             // Look for first non-zero after 'it' :
00169             while ( begin != tasks.end() &&  *begin == 0  )
00170                 ++begin;
00171             if ( begin == tasks.end() )  { // if no task found after zero :
00172 //                 Logger::log() << Logger::Error << "beginBefore resize :"<< tasks.size() << Logger::endl;
00173                 tasks.resize( it - tasks.begin() ); // cut out the items after 'it'
00174 //                 Logger::log() << Logger::Error << "beginAfter resize :"<< tasks.size() << Logger::endl;
00175                 break; // This is our exit condition !
00176             }
00177             // first zero after begin :
00178             ActivityList::iterator end = find ( begin, tasks.end(), nullActivity);
00179             // if end == tasks.end() ==> next while will copy all
00180             // if end != tasks.end() ==> next while will copy first zero's
00181             while ( begin != end ) {
00182                 *it = *begin; // copy operation
00183                 ++begin;
00184                 ++it; // go to next slot to inspect.
00185             }
00186         }
00187 
00188         cleanup = false;
00189     }
00190 
00191 
00192 }