MRXT: The Multi-Robot eXploration Tool
Multi-Robot autonomous exploration and mapping simulator.
|
00001 00002 #include "stepEvent.h" 00003 #include <errno.h> 00004 #include <time.h> 00005 00006 using namespace std; 00007 00008 stepEventManager::stepEventManager(){ 00009 currentStep = 0; 00010 } 00011 00012 void stepEventManager::waitForStep(int step){ 00013 //printf("Waiting for step %d, currenstep = %d\n", step, currentStep); 00014 if (step<=currentStep){ 00015 return; 00016 } 00017 else{ 00018 //printf("new waiting thread <<<<--------------------\n"); 00019 waitingThread* wth = new waitingThread(); 00020 //printf("setstep\n"); 00021 wth->setStep(step); 00022 //printf("lock\n"); 00023 mut.lock(); 00024 //printf("push_back\n"); 00025 waitingList.push_back(wth); 00026 //printf("unlock\n"); 00027 mut.unlock(); 00028 00029 //printf("waiting...\n"); 00030 wth->wait(); 00031 //printf("ok\n"); 00032 00033 mut.lock(); 00034 vector<waitingThread*>::iterator wListIter; 00035 for(wListIter = waitingList.begin(); wListIter != waitingList.end(); wListIter++){ 00036 if ((*wListIter)==wth) { 00037 waitingList.erase(wListIter); 00038 break; 00039 } 00040 } 00041 mut.unlock(); 00042 delete(wth); 00043 } 00044 } 00045 00046 void stepEventManager::step(){ 00047 currentStep++; 00048 vector<waitingThread*>::iterator wListIter; 00049 //printf("---------------- SEM STEP %d ------------------\n",currentStep); 00050 mut.lock(); 00051 for(wListIter = waitingList.begin(); wListIter != waitingList.end(); wListIter++){ 00052 //printf("STEP - a thread is waiting for step %d, in step %d\n",(*wListIter)->getStep(), currentStep); 00053 if ((*wListIter)->getStep() <= currentStep) { 00054 (*wListIter)->post(); 00055 } 00056 } 00057 mut.unlock(); 00058 } 00059 00060 00061 // ---------------------------------------------------------------------------------------------------- 00062 00063 00064 waitingThread::waitingThread(){ 00065 00066 int value; 00067 00068 // initialize a private semaphore 00069 value = 0; 00070 00071 #ifdef WIN32 00072 sem = CreateSemaphore(NULL,value, 1, NULL); 00073 #else 00074 pthread_cond_init(&cond,NULL); 00075 #endif 00076 } 00077 00078 waitingThread::~waitingThread(){ 00079 #ifdef WIN32 00080 CloseHandle(sem); 00081 #else 00082 pthread_cond_destroy(&cond); 00083 #endif 00084 } 00085 00086 void waitingThread::wait(){ 00087 #ifdef WIN32 00088 WaitForSingleObject(sem,INFINITE); 00089 #else 00090 //printf("Waiting Thread::wait step %d - cond: %p...\n", stepToUnclock, &cond); 00091 00092 pthread_mutex_t mutex; 00093 pthread_mutex_init(&mutex,NULL); 00094 pthread_mutex_lock(&mutex); 00095 pthread_cond_wait(&cond, &mutex); 00096 pthread_mutex_unlock(&mutex); 00097 pthread_mutex_destroy(&mutex); 00098 #endif 00099 posted = false; 00100 } 00101 00102 void waitingThread::post(){ 00103 #ifdef WIN32 00104 ReleaseSemaphore(sem, 1, NULL); 00105 #else 00106 pthread_cond_signal(&cond); 00107 #endif 00108 posted = true; 00109 } 00110 00111 void waitingThread::setStep(int step){ 00112 stepToUnclock = step; 00113 } 00114 00115 int waitingThread::getStep(){ 00116 return stepToUnclock; 00117 } 00118 00119 bool waitingThread::isPosted(){ 00120 return posted; 00121 } 00122 00123 production::production(int size): 00124 itemProd(0), 00125 totalSize(size), 00126 prodWaiting(0), 00127 consWaiting(0) 00128 { 00129 #ifndef WIN32 00130 pthread_mutex_init(&mutex,NULL); 00131 pthread_cond_init(&producerWaiting,NULL); 00132 pthread_cond_init(&consumerWaiting,NULL); 00133 #else 00134 mutex = CreateMutex(NULL,FALSE,NULL); 00135 producerWaiting = CreateSemaphore(NULL,0, 1, NULL); 00136 consumerWaiting = CreateSemaphore(NULL,0, 1, NULL); 00137 #endif 00138 } 00139 00140 production::~production(){ 00141 #ifndef WIN32 00142 pthread_cond_destroy(&consumerWaiting); 00143 pthread_cond_destroy(&producerWaiting); 00144 pthread_mutex_destroy(&mutex); 00145 #else 00146 CloseHandle(mutex); 00147 CloseHandle(producerWaiting); 00148 CloseHandle(consumerWaiting); 00149 #endif 00150 } 00151 00152 void production::beginProduction(){ 00153 #ifndef WIN32 00154 pthread_mutex_lock(&mutex); 00155 // printf("prod locks\n"); 00156 while (itemProd >= totalSize){ 00157 // printf("prod wait...\n"); 00158 prodWaiting++; 00159 pthread_cond_signal(&consumerWaiting); 00160 pthread_cond_wait(&producerWaiting,&mutex); 00161 // printf("producer signaled\n"); 00162 prodWaiting--; 00163 } 00164 pthread_mutex_unlock(&mutex); 00165 #else 00166 // printf("prod locks\n"); 00167 WaitForSingleObject(mutex,INFINITE); 00168 while (itemProd >= totalSize){ 00169 // printf("prod wait...\n"); 00170 prodWaiting++; 00171 ReleaseSemaphore(consumerWaiting, 1, NULL); 00172 SignalObjectAndWait(mutex,producerWaiting,INFINITE,false); 00173 // printf("producer signaled\n"); 00174 prodWaiting--; 00175 } 00176 ReleaseMutex(mutex); 00177 #endif 00178 } 00179 00180 void production::endProduction(){ 00181 #ifndef WIN32 00182 pthread_mutex_lock(&mutex); 00183 itemProd++; 00184 // printf("items = %d\n",itemProd); 00185 // printf("end prod\n"); 00186 if (consWaiting>0){ 00187 pthread_cond_signal(&consumerWaiting); 00188 } 00189 // printf("prod unlocks\n"); 00190 pthread_mutex_unlock(&mutex); 00191 #else 00192 WaitForSingleObject(mutex,INFINITE); 00193 itemProd++; 00194 // printf("items = %d\n",itemProd); 00195 // printf("end prod\n"); 00196 if (consWaiting>0){ 00197 ReleaseSemaphore(consumerWaiting, 1, NULL); 00198 } 00199 // printf("prod unlocks\n"); 00200 ReleaseMutex(mutex); 00201 #endif 00202 } 00203 00204 void production::beginConsumition(){ 00205 #ifndef WIN32 00206 pthread_mutex_lock(&mutex); 00207 // printf("cons locks\n"); 00208 while (itemProd == 0){ 00209 // printf("cons wait...\n"); 00210 consWaiting++; 00211 pthread_cond_signal(&producerWaiting); 00212 pthread_cond_wait(&consumerWaiting,&mutex); 00213 // printf("consumer signaled\n"); 00214 consWaiting--; 00215 } 00216 pthread_mutex_unlock(&mutex); 00217 #else 00218 // printf("\t\t\tcons locks\n"); 00219 WaitForSingleObject(mutex,INFINITE); 00220 while (itemProd == 0){ 00221 // printf("\t\t\tcons wait...\n"); 00222 consWaiting++; 00223 ReleaseSemaphore(producerWaiting, 1, NULL); 00224 SignalObjectAndWait(mutex,consumerWaiting,INFINITE,false); 00225 // printf("\t\t\tconsumer signaled\n"); 00226 consWaiting--; 00227 } 00228 ReleaseMutex(mutex); 00229 #endif 00230 } 00231 00232 void production::endConsumition(){ 00233 #ifndef WIN32 00234 pthread_mutex_lock(&mutex); 00235 itemProd--; 00236 // printf("items = %d\n",itemProd); 00237 // printf("end cons\n"); 00238 if (prodWaiting>0){ 00239 pthread_cond_signal(&producerWaiting); 00240 } 00241 // printf("cons unlocks\n"); 00242 pthread_mutex_unlock(&mutex); 00243 #else 00244 WaitForSingleObject(mutex,INFINITE); 00245 itemProd--; 00246 // printf("\t\t\titems = %d\n",itemProd); 00247 // printf("\t\t\tend cons\n"); 00248 if (prodWaiting>0){ 00249 ReleaseSemaphore(producerWaiting, 1, NULL); 00250 } 00251 // printf("\t\t\tcons unlocks\n"); 00252 ReleaseMutex(mutex); 00253 #endif 00254 } 00255 00256 00257 00258