Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "rtabmap/utilite/UThread.h"
00021 #include "rtabmap/utilite/ULogger.h"
00022 #ifdef __APPLE__
00023 #include <mach/thread_policy.h>
00024 #include <mach/mach.h>
00025 #endif
00026
00027 #define PRINT_DEBUG 0
00028
00030
00032
00033 UThread::UThread(Priority priority) :
00034 state_(kSIdle),
00035 priority_(priority),
00036 handle_(0),
00037 threadId_(0),
00038 cpuAffinity_(-1)
00039 {}
00040
00041 UThread::~UThread()
00042 {
00043 #if PRINT_DEBUG
00044 ULOGGER_DEBUG("");
00045 #endif
00046 }
00047
00048 void UThread::kill()
00049 {
00050 #if PRINT_DEBUG
00051 ULOGGER_DEBUG("");
00052 #endif
00053 killSafelyMutex_.lock();
00054 {
00055 if(this->isRunning())
00056 {
00057
00058 while(state_ == kSCreating)
00059 {
00060 uSleep(1);
00061 }
00062
00063 if(state_ == kSRunning)
00064 {
00065 state_ = kSKilled;
00066
00067
00068 mainLoopKill();
00069 }
00070 else
00071 {
00072 UERROR("thread (%d) is supposed to be running...", threadId_);
00073 }
00074 }
00075 else
00076 {
00077 #if PRINT_DEBUG
00078 UDEBUG("thread (%d) is not running...", threadId_);
00079 #endif
00080 }
00081 }
00082 killSafelyMutex_.unlock();
00083 }
00084
00085 void UThread::join(bool killFirst)
00086 {
00087
00088 while(this->isCreating())
00089 {
00090 uSleep(1);
00091 }
00092
00093 #ifdef _WIN32
00094 #if PRINT_DEBUG
00095 UDEBUG("Thread %d joining %d", UThreadC<void>::Self(), threadId_);
00096 #endif
00097 if(UThreadC<void>::Self() == threadId_)
00098 #else
00099 #if PRINT_DEBUG
00100 UDEBUG("Thread %d joining %d", UThreadC<void>::Self(), handle_);
00101 #endif
00102 if(UThreadC<void>::Self() == handle_)
00103 #endif
00104 {
00105 UERROR("Thread cannot join itself");
00106 return;
00107 }
00108
00109 if(killFirst)
00110 {
00111 this->kill();
00112 }
00113
00114 runningMutex_.lock();
00115 runningMutex_.unlock();
00116
00117 #if PRINT_DEBUG
00118 UDEBUG("Join ended for %d", UThreadC<void>::Self());
00119 #endif
00120 }
00121
00122 void UThread::start()
00123 {
00124 #if PRINT_DEBUG
00125 ULOGGER_DEBUG("");
00126 #endif
00127
00128 if(state_ == kSIdle || state_ == kSKilled)
00129 {
00130 if(state_ == kSKilled)
00131 {
00132
00133 runningMutex_.lock();
00134 runningMutex_.unlock();
00135 }
00136
00137 state_ = kSCreating;
00138 int r = UThreadC<void>::Create(threadId_, &handle_, true);
00139 if(r)
00140 {
00141 UERROR("Failed to create a thread! errno=%d", r);
00142 threadId_=0;
00143 handle_=0;
00144 state_ = kSIdle;
00145 }
00146 else
00147 {
00148 #if PRINT_DEBUG
00149 ULOGGER_DEBUG("StateThread::startThread() thread id=%d _handle=%d", threadId_, handle_);
00150 #endif
00151 }
00152 }
00153 }
00154
00155
00156 void UThread::setPriority(Priority priority)
00157 {
00158 priority_ = priority;
00159 }
00160
00161
00162 void UThread::applyPriority()
00163 {
00164 if(handle_)
00165 {
00166 #ifdef _WIN32
00167 int p = THREAD_PRIORITY_NORMAL;
00168 switch(priority_)
00169 {
00170 case kPLow:
00171 p = THREAD_PRIORITY_LOWEST;
00172 break;
00173
00174 case kPBelowNormal:
00175 p = THREAD_PRIORITY_BELOW_NORMAL;
00176 break;
00177
00178 case kPNormal:
00179 p = THREAD_PRIORITY_NORMAL;
00180 break;
00181
00182 case kPAboveNormal:
00183 p = THREAD_PRIORITY_ABOVE_NORMAL;
00184 break;
00185
00186 case kPRealTime:
00187 p = THREAD_PRIORITY_TIME_CRITICAL;
00188 break;
00189
00190 default:
00191 break;
00192 }
00193 SetThreadPriority(handle_, p);
00194 #endif
00195 }
00196 }
00197
00198 void UThread::setAffinity(int cpu)
00199 {
00200 cpuAffinity_ = cpu;
00201 if(cpuAffinity_<0)
00202 {
00203 cpuAffinity_ = 0;
00204 }
00205 }
00206
00207
00208 void UThread::applyAffinity()
00209 {
00210 if(cpuAffinity_>0)
00211 {
00212 #ifdef _WIN32
00213 #elif __APPLE__
00214 thread_affinity_policy_data_t affPolicy;
00215 affPolicy.affinity_tag = cpuAffinity_;
00216 kern_return_t ret = thread_policy_set(
00217 mach_thread_self(),
00218 THREAD_AFFINITY_POLICY,
00219 (integer_t*) &affPolicy,
00220 THREAD_AFFINITY_POLICY_COUNT);
00221 if(ret != KERN_SUCCESS)
00222 {
00223 UERROR("thread_policy_set returned %d", ret);
00224 }
00225 #else
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236 #endif
00237 }
00238 }
00239
00240 bool UThread::isCreating() const
00241 {
00242 return state_ == kSCreating;
00243 }
00244
00245 bool UThread::isRunning() const
00246 {
00247 return state_ == kSRunning || state_ == kSCreating;
00248 }
00249
00250 bool UThread::isIdle() const
00251 {
00252 return state_ == kSIdle;
00253 }
00254
00255 bool UThread::isKilled() const
00256 {
00257 return state_ == kSKilled;
00258 }
00259
00261
00263
00264 void UThread::ThreadMain()
00265 {
00266 runningMutex_.lock();
00267 applyPriority();
00268 applyAffinity();
00269
00270 #if PRINT_DEBUG
00271 ULOGGER_DEBUG("before mainLoopBegin()");
00272 #endif
00273
00274 state_ = kSRunning;
00275 mainLoopBegin();
00276
00277 #if PRINT_DEBUG
00278 ULOGGER_DEBUG("before mainLoop()");
00279 #endif
00280
00281 while(state_ == kSRunning)
00282 {
00283 mainLoop();
00284 }
00285
00286 #if PRINT_DEBUG
00287 ULOGGER_DEBUG("before mainLoopEnd()");
00288 #endif
00289
00290 mainLoopEnd();
00291
00292 handle_ = 0;
00293 threadId_ = 0;
00294 state_ = kSIdle;
00295
00296 runningMutex_.unlock();
00297 #if PRINT_DEBUG
00298 ULOGGER_DEBUG("Exiting thread loop");
00299 #endif
00300 }
00301