src/robofsm/scheduler.hpp

   1 #include <boost/statechart/event_processor.hpp>
   2 #include <boost/statechart/event.hpp>
   3 #include <deque>
   4 #include <boost/statechart/fifo_scheduler.hpp>
   5
   6 #include <stdio.h>
   7 #include <pthread.h>
   8 #include <semaphore.h>
   9 #include <guard.hpp>
  10 #include <vector>
  11 #include <timerlist.h>
  12 #include <cstdlib>
  13
  14 #ifndef SCHEDULER_HPP
  15 #define SCHEDULER_HPP
  16
  17 namespace sc = boost::statechart;
  18
  19 #define BOOST_STATECHART_USE_NATIVE_RTTI
  20 //#define USE_FIFO_SCHEDULER
  21 #ifdef USE_FIFO_SCHEDULER
  22 typedef sc::fifo_scheduler<> Scheduler;
  23 #else
  24 class Scheduler {
  25         typedef sc::event_processor< Scheduler > processor_base_type;
  26         typedef boost::intrusive_ptr< const sc::event_base > event_ptr_type;
  27         struct processor_queue {
  28               processor_base_type *processor_;
  29               std::deque<event_ptr_type> event_queue_;
  30               TimerList timer_list_;
  31               const char* state_name;
  32         };
  33 public:
  34         typedef processor_queue *processor_handle;
  35
  36         class processor_context {
  37                 processor_context(Scheduler & scheduler, const processor_handle & handle) :
  38                         scheduler_( scheduler ),
  39                         handle_( handle )
  40                         {}
  41
  42                 Scheduler & my_scheduler() const { return scheduler_; }
  43                 const processor_handle & my_handle() const { return handle_; }
  44
  45                 Scheduler & scheduler_;
  46                 const processor_handle handle_;
  47
  48                 friend class sc::event_processor< Scheduler >;
  49                 friend class Scheduler;
  50         };
  51
  52         Scheduler() : terminated_(false) {
  53                 pthread_mutex_init(& queue_lock, NULL);
  54                 sem_init(&waiting_sem, 0, 0);
  55         }
  56
  57         template<class Processor>
  58         processor_handle create_processor() {
  59                 processor_queue *my_processor = new processor_queue();
  60                 printf("Processor creation.\n");
  61                 processor_handle handle = my_processor;
  62                 Processor *p =  new Processor( processor_context(*this, (my_processor)) );
  63                 my_processor->processor_ = p;
  64                 processor_queue_list.push_back(my_processor);
  65                 actual_processor = processor_queue_list.back();
  66                 return handle;
  67         }
  68
  69         void terminate() {
  70                 terminated_ = true;
  71                 sem_post(&waiting_sem);
  72         }
  73
  74         void queue_event(const processor_handle & processor, const event_ptr_type & pEvent) {
  75                 Guard g(queue_lock);
  76                 processor->event_queue_.push_back(pEvent);
  77                 sem_post(&waiting_sem);
  78         }
  79
  80         void operator()() {
  81                 int ret;
  82                 int sem_val;
  83                 for(unsigned i = 0; i<processor_queue_list.size();i++) processor_queue_list[i]->processor_->initiate();
  84                 while (!terminated_) {
  85                         sem_getvalue(&waiting_sem,&sem_val);
  86                         if(sem_val == 0) {
  87                                 if(check_timer()) {
  88                                         ret = sem_timedwait(&waiting_sem, &(actual_timer->ts));
  89                                 }
  90                                 else {
  91                                         ret = sem_wait(&waiting_sem);
  92                                 }
  93                         }
  94                         else ret = sem_wait(&waiting_sem);
  95                         if(ret != 0) {
  96                                 switch (errno) {
  97                                         case ETIMEDOUT: {
  98                                                 __fsm_timespec_invalidate(&(actual_timer->ts));
  99                                                 actual_processor->processor_->process_event(*(actual_timer->ev));
 100                                                 break; /* exit switch */
 101                                         }
 102                                         case EINTR:
 103                                                 continue;
 104                                         default:
 105                                                 perror("wait_for_event");
 106                                                 __fsm_timespec_invalidate(&(actual_timer->ts));
 107                                                 continue;
 108                                 }
 109                         }
 110                         else {
 111                                 for(unsigned i = 0; i<processor_queue_list.size();i++) {
 112                                         if(processor_queue_list[i]->event_queue_.size()>0) {
 113                                                 actual_processor = processor_queue_list[i];
 114                                                 processor_queue_list[i]->processor_->process_event(*(processor_queue_list[i]->event_queue_.front()));
 115                                                 printf("stav: %s\n",actual_processor->state_name);
 116                                                 Guard g(queue_lock);
 117                                                 processor_queue_list[i]->event_queue_.pop_front();
 118                                         }
 119                                 }
 120                         }
 121                 }
 122                 for(unsigned i = 0; i<processor_queue_list.size();i++) {
 123                         processor_queue_list[i]->processor_->terminate();
 124                         delete processor_queue_list[i]->processor_;
 125                         delete processor_queue_list[i];
 126                 }
 127         }
 128         processor_handle getActualHandle() {
 129                 return actual_processor;
 130         }
 131
 132     private:
 133         bool check_timer() {
 134                 static int ret = 0;
 135                 actual_timer = NULL;
 136                 for(unsigned i = 0; i<processor_queue_list.size();i++) {
 137                         if(processor_queue_list[i]->timer_list_.timeouts.size()>0) {
 138                                 if(!timespec_valid(&(processor_queue_list[i]->timer_list_.timeouts.front()->ts))) {
 139                                         processor_queue_list[i]->timer_list_.timeouts.pop_front();
 140                                         i--;
 141                                 }
 142                                 else {
 143                                         if(i == 0 || !(actual_timer)) {
 144                                                 actual_timer = processor_queue_list[i]->timer_list_.timeouts.front();
 145                                                 actual_processor = processor_queue_list[i];
 146                                         }
 147                                         else {
 148                                                 ret = __fsm_timespec_cmp(&(actual_timer->ts), &(processor_queue_list[i]->timer_list_.timeouts.front()->ts));
 149                                                 if(ret == 1) {
 150                                                         actual_timer = processor_queue_list[i]->timer_list_.timeouts.front();
 151                                                         actual_processor = processor_queue_list[i];
 152                                                 }
 153                                         }
 154                                 }
 155                         }
 156                 }
 157                 if(!actual_timer) return false;
 158                 return true;
 159         }
 160         std::vector<processor_queue *> processor_queue_list;
 161         pthread_mutex_t queue_lock;
 162         sem_t waiting_sem;
 163         bool terminated_;
 164         processor_queue *actual_processor;
 165         Timer *actual_timer;
 166 };
 167 #endif
 168
 169 #endif