9 #include <argos3/core/simulator/simulator.h>
10 #include <argos3/core/utility/profiler/profiler.h>
25 static void CleanupUpdateThread(
void* p_data) {
30 SCleanupUpdateThreadData& sData =
31 *
reinterpret_cast<SCleanupUpdateThreadData*
>(p_data);
32 pthread_mutex_unlock(sData.SenseControlStepConditionalMutex);
33 pthread_mutex_unlock(sData.ActConditionalMutex);
34 pthread_mutex_unlock(sData.PhysicsConditionalMutex);
35 pthread_mutex_unlock(sData.MediaConditionalMutex);
39 LOG.AddThreadSafeBuffer();
40 LOGERR.AddThreadSafeBuffer();
41 CSpaceMultiThreadBalanceQuantity::SUpdateThreadData* psData =
reinterpret_cast<CSpaceMultiThreadBalanceQuantity::SUpdateThreadData*
>(p_data);
42 psData->Space->UpdateThread(psData->ThreadId);
50 m_psUpdateThreadData(NULL),
51 m_ptUpdateThreads(NULL),
52 m_bIsControllableEntityAssignmentRecalculationNeeded(true) {}
68 if((nErrors = pthread_mutex_init(&m_tSenseControlStepConditionalMutex, NULL)) ||
69 (nErrors = pthread_mutex_init(&m_tActConditionalMutex, NULL)) ||
70 (nErrors = pthread_mutex_init(&m_tPhysicsConditionalMutex, NULL)) ||
71 (nErrors = pthread_mutex_init(&m_tMediaConditionalMutex, NULL))) {
75 if((nErrors = pthread_cond_init(&m_tSenseControlStepConditional, NULL)) ||
76 (nErrors = pthread_cond_init(&m_tActConditional, NULL)) ||
77 (nErrors = pthread_cond_init(&m_tPhysicsConditional, NULL)) ||
78 (nErrors = pthread_cond_init(&m_tMediaConditional, NULL))) {
88 void CSpaceMultiThreadBalanceQuantity::StartThreads() {
95 m_psUpdateThreadData[i] =
new SUpdateThreadData(i,
this);
97 if((nErrors = pthread_create(m_ptUpdateThreads + i,
100 reinterpret_cast<void*
>(m_psUpdateThreadData[i])))) {
112 if(m_ptUpdateThreads != NULL) {
114 if((nErrors = pthread_cancel(m_ptUpdateThreads[i]))) {
115 THROW_ARGOSEXCEPTION(
"Error canceling controllable entities update threads " << ::strerror(nErrors));
120 if((nErrors = pthread_join(m_ptUpdateThreads[i], ppJoinResult + i))) {
121 THROW_ARGOSEXCEPTION(
"Error joining controllable entities update threads " << ::strerror(nErrors));
123 if(ppJoinResult[i] != PTHREAD_CANCELED) {
124 LOGERR <<
"[WARNING] Controllable entities update thread #" << i<<
" not canceled" << std::endl;
127 delete[] ppJoinResult;
129 delete[] m_ptUpdateThreads;
131 if(m_psUpdateThreadData != NULL) {
133 delete m_psUpdateThreadData[i];
136 delete[] m_psUpdateThreadData;
137 pthread_mutex_destroy(&m_tSenseControlStepConditionalMutex);
138 pthread_mutex_destroy(&m_tActConditionalMutex);
139 pthread_mutex_destroy(&m_tPhysicsConditionalMutex);
140 pthread_mutex_destroy(&m_tMediaConditionalMutex);
141 pthread_cond_destroy(&m_tSenseControlStepConditional);
142 pthread_cond_destroy(&m_tActConditional);
143 pthread_cond_destroy(&m_tPhysicsConditional);
144 pthread_cond_destroy(&m_tMediaConditional);
153 m_bIsControllableEntityAssignmentRecalculationNeeded =
true;
161 m_bIsControllableEntityAssignmentRecalculationNeeded =
true;
168 #define MAIN_SEND_GO_FOR_PHASE(PHASE) \
171 pthread_mutex_lock(&m_t ## PHASE ## ConditionalMutex); \
172 m_un ## PHASE ## PhaseDoneCounter = 0; \
173 pthread_cond_broadcast(&m_t ## PHASE ## Conditional); \
174 pthread_mutex_unlock(&m_t ## PHASE ## ConditionalMutex);
176 #define MAIN_WAIT_FOR_PHASE_END(PHASE) \
177 pthread_mutex_lock(&m_t ## PHASE ## ConditionalMutex); \
178 while(m_un ## PHASE ## PhaseDoneCounter < CSimulator::GetInstance().GetNumThreads()) { \
179 pthread_cond_wait(&m_t ## PHASE ## Conditional, &m_t ## PHASE ## ConditionalMutex); \
181 pthread_mutex_unlock(&m_t ## PHASE ## ConditionalMutex);
187 m_bIsControllableEntityAssignmentRecalculationNeeded =
false;
200 (*m_ptPhysicsEngines)[i]->TransferEntities();
221 m_bIsControllableEntityAssignmentRecalculationNeeded =
false;
227 #define THREAD_WAIT_FOR_GO_SIGNAL(PHASE) \
228 pthread_mutex_lock(&m_t ## PHASE ## ConditionalMutex); \
229 while(m_un ## PHASE ## PhaseDoneCounter == CSimulator::GetInstance().GetNumThreads()) { \
230 pthread_cond_wait(&m_t ## PHASE ## Conditional, &m_t ## PHASE ## ConditionalMutex); \
232 pthread_mutex_unlock(&m_t ## PHASE ## ConditionalMutex); \
233 pthread_testcancel();
235 #define THREAD_SIGNAL_PHASE_DONE(PHASE) \
236 pthread_mutex_lock(&m_t ## PHASE ## ConditionalMutex); \
237 ++m_un ## PHASE ## PhaseDoneCounter; \
238 pthread_cond_broadcast(&m_t ## PHASE ## Conditional); \
239 pthread_mutex_unlock(&m_t ## PHASE ## ConditionalMutex); \
240 pthread_testcancel();
243 size_t un_tot_plugins) {
249 if(unMinPortion == 0) {
251 if(un_id < unExtraPortion) {
262 if(un_id < unExtraPortion) {
265 (un_id+1) * (unMinPortion+1));
269 return CRange<size_t>(unExtraPortion * (unMinPortion+1) + (un_id-unExtraPortion) * unMinPortion,
270 unExtraPortion * (unMinPortion+1) + (un_id-unExtraPortion+1) * unMinPortion);
275 void CSpaceMultiThreadBalanceQuantity::UpdateThread(
UInt32 un_id) {
279 SCleanupUpdateThreadData sCancelData;
280 sCancelData.SenseControlStepConditionalMutex = &m_tSenseControlStepConditionalMutex;
281 sCancelData.ActConditionalMutex = &m_tActConditionalMutex;
282 sCancelData.PhysicsConditionalMutex = &m_tPhysicsConditionalMutex;
283 sCancelData.MediaConditionalMutex = &m_tMediaConditionalMutex;
284 pthread_cleanup_push(CleanupUpdateThread, &sCancelData);
290 CRange<size_t> cEntityRange;
294 if(m_bIsControllableEntityAssignmentRecalculationNeeded) {
298 if(cEntityRange.GetSpan() > 0) {
301 for(
size_t i = cEntityRange.GetMin(); i < cEntityRange.GetMax(); ++i) {
304 pthread_testcancel();
313 if(cPhysicsRange.GetSpan() > 0) {
315 for(
size_t i = cPhysicsRange.GetMin(); i < cPhysicsRange.GetMax(); ++i) {
316 (*m_ptPhysicsEngines)[i]->Update();
318 pthread_testcancel();
327 if(cMediaRange.GetSpan() > 0) {
329 for(
size_t i = cMediaRange.GetMin(); i < cMediaRange.GetMax(); ++i) {
330 (*m_ptMedia)[i]->Update();
332 pthread_testcancel();
342 if(cEntityRange.GetSpan() > 0) {
344 for(
size_t i = cEntityRange.GetMin(); i < cEntityRange.GetMax(); ++i) {
348 pthread_testcancel();
356 pthread_cleanup_pop(1);