space_multi_thread_balance_length.cpp
Go to the documentation of this file.
1 
7 #include "space_multi_thread_balance_length.h"
8 #include <argos3/core/simulator/simulator.h>
9 #include <argos3/core/utility/profiler/profiler.h>
10 
11 namespace argos {
12 
13  /****************************************/
14  /****************************************/
15 
16  struct SCleanupThreadData {
17  pthread_mutex_t* StartSenseControlPhaseMutex;
18  pthread_mutex_t* StartActPhaseMutex;
19  pthread_mutex_t* StartPhysicsPhaseMutex;
20  pthread_mutex_t* StartMediaPhaseMutex;
21  pthread_mutex_t* StartEntityIterPhaseMutex;
22  pthread_mutex_t* FetchTaskMutex;
23  };
24 
25  static void CleanupThread(void* p_data) {
26  CSimulator& cSimulator = CSimulator::GetInstance();
27  if(cSimulator.IsProfiling()) {
28  cSimulator.GetProfiler().CollectThreadResourceUsage();
29  }
30  SCleanupThreadData& sData =
31  *reinterpret_cast<SCleanupThreadData*>(p_data);
32  pthread_mutex_unlock(sData.FetchTaskMutex);
33  pthread_mutex_unlock(sData.StartSenseControlPhaseMutex);
34  pthread_mutex_unlock(sData.StartActPhaseMutex);
35  pthread_mutex_unlock(sData.StartPhysicsPhaseMutex);
36  pthread_mutex_unlock(sData.StartMediaPhaseMutex);
37  pthread_mutex_unlock(sData.StartEntityIterPhaseMutex);
38  }
39 
40  void* LaunchThreadBalanceLength(void* p_data) {
41  /* Set up thread-safe buffers for this new thread */
42  LOG.AddThreadSafeBuffer();
43  LOGERR.AddThreadSafeBuffer();
44  /* Make this thread cancellable */
45  pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, nullptr);
46  pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, nullptr);
47  /* Get a handle to the thread launch data */
48  auto* psData = reinterpret_cast<CSpaceMultiThreadBalanceLength::SThreadLaunchData*>(p_data);
49  /* Create cancellation data */
50  SCleanupThreadData sCancelData;
51  sCancelData.StartSenseControlPhaseMutex = &(psData->Space->m_tStartSenseControlPhaseMutex);
52  sCancelData.StartActPhaseMutex = &(psData->Space->m_tStartActPhaseMutex);
53  sCancelData.StartPhysicsPhaseMutex = &(psData->Space->m_tStartPhysicsPhaseMutex);
54  sCancelData.StartMediaPhaseMutex = &(psData->Space->m_tStartMediaPhaseMutex);
55  sCancelData.StartEntityIterPhaseMutex = &(psData->Space->m_tStartEntityIterPhaseMutex);
56  sCancelData.FetchTaskMutex = &(psData->Space->m_tFetchTaskMutex);
57  pthread_cleanup_push(CleanupThread, &sCancelData);
58  psData->Space->SlaveThread();
59  /* Dispose of cancellation data */
60  pthread_cleanup_pop(1);
61  return nullptr;
62  }
63 
64  /****************************************/
65  /****************************************/
66 
67  void CSpaceMultiThreadBalanceLength::Init(TConfigurationNode& t_tree) {
68  /* Initialize the space */
69  CSpace::Init(t_tree);
70  /* Initialize thread related structures */
71  int nErrors;
72  /* Init mutexes */
73  if((nErrors = pthread_mutex_init(&m_tStartSenseControlPhaseMutex, nullptr)) ||
74  (nErrors = pthread_mutex_init(&m_tStartActPhaseMutex, nullptr)) ||
75  (nErrors = pthread_mutex_init(&m_tStartPhysicsPhaseMutex, nullptr)) ||
76  (nErrors = pthread_mutex_init(&m_tStartMediaPhaseMutex, nullptr)) ||
77  (nErrors = pthread_mutex_init(&m_tStartEntityIterPhaseMutex, nullptr)) ||
78  (nErrors = pthread_mutex_init(&m_tFetchTaskMutex, nullptr))) {
79  THROW_ARGOSEXCEPTION("Error creating thread mutexes " << ::strerror(nErrors));
80  }
81  /* Init conditionals */
82  if((nErrors = pthread_cond_init(&m_tStartSenseControlPhaseCond, nullptr)) ||
83  (nErrors = pthread_cond_init(&m_tStartActPhaseCond, nullptr)) ||
84  (nErrors = pthread_cond_init(&m_tStartPhysicsPhaseCond, nullptr)) ||
85  (nErrors = pthread_cond_init(&m_tStartMediaPhaseCond, nullptr)) ||
86  (nErrors = pthread_cond_init(&m_tStartEntityIterPhaseCond, nullptr)) ||
87  (nErrors = pthread_cond_init(&m_tFetchTaskCond, nullptr))) {
88  THROW_ARGOSEXCEPTION("Error creating thread conditionals " << ::strerror(nErrors));
89  }
90  /* Reset the idle thread count */
91  m_unSenseControlPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
92  m_unActPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
93  m_unPhysicsPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
94  m_unMediaPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
95  m_unEntityIterPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
96  /* Start threads */
97  StartThreads();
98  }
99 
100  /****************************************/
101  /****************************************/
102 
103  void CSpaceMultiThreadBalanceLength::Destroy() {
104  /* Destroy the threads to update the controllable entities */
105  int nErrors;
106  if(m_ptThreads != nullptr) {
107  for(UInt32 i = 0; i < CSimulator::GetInstance().GetNumThreads(); ++i) {
108  if((nErrors = pthread_cancel(m_ptThreads[i]))) {
109  THROW_ARGOSEXCEPTION("Error canceling threads " << ::strerror(nErrors));
110  }
111  }
112  auto** ppJoinResult = new void*[CSimulator::GetInstance().GetNumThreads()];
113  for(UInt32 i = 0; i < CSimulator::GetInstance().GetNumThreads(); ++i) {
114  if((nErrors = pthread_join(m_ptThreads[i], ppJoinResult + i))) {
115  THROW_ARGOSEXCEPTION("Error joining threads " << ::strerror(nErrors));
116  }
117  if(ppJoinResult[i] != PTHREAD_CANCELED) {
118  LOGERR << "[WARNING] Thread #" << i<< " not canceled" << std::endl;
119  }
120  }
121  delete[] ppJoinResult;
122  }
123  delete[] m_ptThreads;
124  /* Destroy the thread launch info */
125  if(m_psThreadData != nullptr) {
126  for(UInt32 i = 0; i < CSimulator::GetInstance().GetNumThreads(); ++i) {
127  delete m_psThreadData[i];
128  }
129  }
130  delete[] m_psThreadData;
131  pthread_mutex_destroy(&m_tStartSenseControlPhaseMutex);
132  pthread_mutex_destroy(&m_tStartActPhaseMutex);
133  pthread_mutex_destroy(&m_tStartPhysicsPhaseMutex);
134  pthread_mutex_destroy(&m_tStartMediaPhaseMutex);
135  pthread_mutex_destroy(&m_tStartEntityIterPhaseMutex);
136  pthread_mutex_destroy(&m_tFetchTaskMutex);
137 
138  pthread_cond_destroy(&m_tStartSenseControlPhaseCond);
139  pthread_cond_destroy(&m_tStartActPhaseCond);
140  pthread_cond_destroy(&m_tStartPhysicsPhaseCond);
141  pthread_cond_destroy(&m_tStartMediaPhaseCond);
142  pthread_cond_destroy(&m_tStartEntityIterPhaseCond);
143  pthread_cond_destroy(&m_tFetchTaskCond);
144 
145  /* Destroy the base space */
146  CSpace::Destroy();
147  }
148 
149  /****************************************/
150  /****************************************/
151 
152  void CSpaceMultiThreadBalanceLength::Update() {
153  /* Reset the idle thread count */
154  m_unSenseControlPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
155  m_unActPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
156  m_unPhysicsPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
157  m_unMediaPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
158  m_unEntityIterPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads();
159  /* Update the space */
160  CSpace::Update();
161  }
162 
163  /****************************************/
164  /****************************************/
165 
166 #define MAIN_START_PHASE(PHASE) \
167  pthread_mutex_lock(&m_tStart ## PHASE ## PhaseMutex); \
168  m_un ## PHASE ## PhaseIdleCounter = 0; \
169  m_unTaskIndex = 0; \
170  pthread_cond_broadcast(&m_tStart ## PHASE ## PhaseCond); \
171  pthread_mutex_unlock(&m_tStart ## PHASE ## PhaseMutex);
172 
173 #define MAIN_WAIT_FOR_END_OF(PHASE) \
174  pthread_mutex_lock(&m_tStart ## PHASE ## PhaseMutex); \
175  while(m_un ## PHASE ## PhaseIdleCounter < CSimulator::GetInstance().GetNumThreads()) { \
176  pthread_cond_wait(&m_tStart ## PHASE ## PhaseCond, &m_tStart ## PHASE ## PhaseMutex); \
177  } \
178  pthread_mutex_unlock(&m_tStart ## PHASE ## PhaseMutex);
179 
180  void CSpaceMultiThreadBalanceLength::UpdateControllableEntitiesAct() {
181  /* Act phase */
182  MAIN_START_PHASE(Act);
183  MAIN_WAIT_FOR_END_OF(Act);
184  }
185 
186  /****************************************/
187  /****************************************/
188 
189  void CSpaceMultiThreadBalanceLength::UpdatePhysics() {
190  /* Physics phase */
191  MAIN_START_PHASE(Physics);
192  MAIN_WAIT_FOR_END_OF(Physics);
193  /* Perform entity transfer from engine to engine, if needed */
194  for(size_t i = 0; i < m_ptPhysicsEngines->size(); ++i) {
195  if((*m_ptPhysicsEngines)[i]->IsEntityTransferNeeded()) {
196  (*m_ptPhysicsEngines)[i]->TransferEntities();
197  }
198  }
199  }
200 
201  /****************************************/
202  /****************************************/
203 
204  void CSpaceMultiThreadBalanceLength::UpdateMedia() {
205  /* Media phase */
206  MAIN_START_PHASE(Media);
207  MAIN_WAIT_FOR_END_OF(Media);
208  }
209 
210  /****************************************/
211  /****************************************/
212 
213  void CSpaceMultiThreadBalanceLength::IterateOverControllableEntities(
214  const TControllableEntityIterCBType &c_cb) {
215  m_cbControllableEntityIter = c_cb;
216  /* Iterate over all robots in the swarm */
217  MAIN_START_PHASE(EntityIter);
218  MAIN_WAIT_FOR_END_OF(EntityIter);
219  } /* IterateOverControllableEntities() */
220 
221 
222  /****************************************/
223  /****************************************/
224 
225  void CSpaceMultiThreadBalanceLength::UpdateControllableEntitiesSenseStep() {
226  /* Sense/control phase */
227  MAIN_START_PHASE(SenseControl);
228  MAIN_WAIT_FOR_END_OF(SenseControl);
229  }
230 
231  /****************************************/
232  /****************************************/
233 
234  void CSpaceMultiThreadBalanceLength::StartThreads() {
235  int nErrors;
236  /* Create the threads to update the controllable entities */
237  m_ptThreads = new pthread_t[CSimulator::GetInstance().GetNumThreads()];
238  m_psThreadData = new SThreadLaunchData*[CSimulator::GetInstance().GetNumThreads()];
239  for(UInt32 i = 0; i < CSimulator::GetInstance().GetNumThreads(); ++i) {
240  /* Create the struct with the info to launch the thread */
241  m_psThreadData[i] = new SThreadLaunchData(i, this);
242  /* Create the thread */
243  if((nErrors = pthread_create(m_ptThreads + i,
244  nullptr,
245  LaunchThreadBalanceLength,
246  reinterpret_cast<void*>(m_psThreadData[i])))) {
247  THROW_ARGOSEXCEPTION("Error creating thread: " << ::strerror(nErrors));
248  }
249  }
250  }
251 
252  /****************************************/
253  /****************************************/
254 
255 #define THREAD_WAIT_FOR_START_OF(PHASE) \
256  pthread_mutex_lock(&m_tStart ## PHASE ## PhaseMutex); \
257  while(m_un ## PHASE ## PhaseIdleCounter == CSimulator::GetInstance().GetNumThreads()) { \
258  pthread_cond_wait(&m_tStart ## PHASE ## PhaseCond, &m_tStart ## PHASE ## PhaseMutex); \
259  } \
260  pthread_mutex_unlock(&m_tStart ## PHASE ## PhaseMutex); \
261  pthread_testcancel();
262 
263 #define THREAD_PERFORM_TASK(PHASE, TASKVEC, CONDITION, SNIPPET) \
264  while(1) { \
265  pthread_mutex_lock(&m_tFetchTaskMutex); \
266  if((CONDITION) && m_unTaskIndex < (TASKVEC).size()) { \
267  unTaskIndex = m_unTaskIndex; \
268  ++m_unTaskIndex; \
269  pthread_mutex_unlock(&m_tFetchTaskMutex); \
270  pthread_testcancel(); \
271  { \
272  SNIPPET; \
273  } \
274  pthread_testcancel(); \
275  } \
276  else { \
277  pthread_mutex_unlock(&m_tFetchTaskMutex); \
278  pthread_testcancel(); \
279  pthread_mutex_lock(&m_tStart ## PHASE ## PhaseMutex); \
280  ++m_un ## PHASE ## PhaseIdleCounter; \
281  pthread_cond_broadcast(&m_tStart ## PHASE ## PhaseCond); \
282  pthread_mutex_unlock(&m_tStart ## PHASE ## PhaseMutex); \
283  pthread_testcancel(); \
284  break; \
285  } \
286  } \
287  pthread_testcancel();
288 
289  void CSpaceMultiThreadBalanceLength::SlaveThread() {
290  /* Task index */
291  size_t unTaskIndex;
292  while(1) {
293  THREAD_WAIT_FOR_START_OF(Act);
294  THREAD_PERFORM_TASK(
295  Act,
296  m_vecControllableEntities,
297  true,
298  if(m_vecControllableEntities[unTaskIndex]->IsEnabled()) m_vecControllableEntities[unTaskIndex]->Act();
299  );
300  THREAD_WAIT_FOR_START_OF(Physics);
301  THREAD_PERFORM_TASK(
302  Physics,
303  *m_ptPhysicsEngines,
304  true,
305  (*m_ptPhysicsEngines)[unTaskIndex]->Update();
306  );
307  THREAD_WAIT_FOR_START_OF(Media);
308  THREAD_PERFORM_TASK(
309  Media,
310  *m_ptMedia,
311  true,
312  (*m_ptMedia)[unTaskIndex]->Update();
313  );
314  /* loop functions PreStep() */
315  THREAD_WAIT_FOR_START_OF(EntityIter);
316  THREAD_PERFORM_TASK(
317  EntityIter,
318  m_vecControllableEntities,
319  ControllableEntityIterationEnabled(),
320  m_cbControllableEntityIter(m_vecControllableEntities[unTaskIndex]));
321  THREAD_WAIT_FOR_START_OF(SenseControl);
322  THREAD_PERFORM_TASK(
323  SenseControl,
324  m_vecControllableEntities,
325  true,
326  if(m_vecControllableEntities[unTaskIndex]->IsEnabled()) {
327  m_vecControllableEntities[unTaskIndex]->Sense();
328  m_vecControllableEntities[unTaskIndex]->ControlStep();
329  }
330  );
331  /* loop functions PostStep() */
332  THREAD_WAIT_FOR_START_OF(EntityIter);
333  THREAD_PERFORM_TASK(
334  EntityIter,
335  m_vecControllableEntities,
336  ControllableEntityIterationEnabled(),
337  m_cbControllableEntityIter(m_vecControllableEntities[unTaskIndex]));
338  } /* while(1) */
339  }
340 
341  /****************************************/
342  /****************************************/
343 
344 }
THREAD_WAIT_FOR_START_OF
#define THREAD_WAIT_FOR_START_OF(PHASE)
Definition: space_multi_thread_balance_length.cpp:255
MAIN_WAIT_FOR_END_OF
#define MAIN_WAIT_FOR_END_OF(PHASE)
Definition: space_multi_thread_balance_length.cpp:173
THREAD_PERFORM_TASK
#define THREAD_PERFORM_TASK(PHASE, TASKVEC, CONDITION, SNIPPET)
Definition: space_multi_thread_balance_length.cpp:263
MAIN_START_PHASE
#define MAIN_START_PHASE(PHASE)
Definition: space_multi_thread_balance_length.cpp:166
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::LOGERR
CARGoSLog LOGERR(std::cerr, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_RED))
Definition: argos_log.h:180
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::LOG
CARGoSLog LOG(std::cout, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_GREEN))
Definition: argos_log.h:179
argos::LaunchThreadBalanceLength
void * LaunchThreadBalanceLength(void *p_data)
Definition: space_multi_thread_balance_length.cpp:40
argos::CSimulator
The core class of ARGOS.
Definition: simulator.h:62
argos::CSimulator::GetProfiler
CProfiler & GetProfiler()
Returns a reference to the profiler.
Definition: simulator.h:174
argos::CSimulator::GetInstance
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
argos::CSimulator::GetNumThreads
UInt32 GetNumThreads() const
Returns the number of threads used during the experiment.
Definition: simulator.h:260
argos::CSimulator::IsProfiling
bool IsProfiling() const
Returns true if ARGoS is being profiled.
Definition: simulator.h:182
argos::CSpace::m_vecControllableEntities
CControllableEntity::TVector m_vecControllableEntities
A vector of controllable entities.
Definition: space.h:491
argos::CSpace::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the space using the <arena> section of the XML configuration file.
Definition: space.cpp:37
argos::CSpace::Destroy
virtual void Destroy()
Destroys the space and all its entities.
Definition: space.cpp:85
argos::CSpace::ControllableEntityIterationEnabled
bool ControllableEntityIterationEnabled() const
Definition: space.h:449
argos::CSpace::Update
virtual void Update()
Updates the space.
Definition: space.cpp:119
argos::CSpace::m_ptPhysicsEngines
CPhysicsEngine::TVector * m_ptPhysicsEngines
A pointer to the list of physics engines.
Definition: space.h:497
argos::CSpace::m_cbControllableEntityIter
TControllableEntityIterCBType m_cbControllableEntityIter
Callback for iterating over entities from within the loop functions.
Definition: space.h:503
argos::CSpace::m_ptMedia
CMedium::TVector * m_ptMedia
A pointer to the list of media.
Definition: space.h:500
argos::CSpace::TControllableEntityIterCBType
std::function< void(CControllableEntity *)> TControllableEntityIterCBType
The callback type for iteration over controllable entities within the PreStep() and/or PostStep() par...
Definition: space.h:90
argos::CSpaceMultiThreadBalanceLength::IterateOverControllableEntities
virtual void IterateOverControllableEntities(const TControllableEntityIterCBType &c_cb)
Given a callback specified in the loop functions, iterate over all controllable entities currently pr...
Definition: space_multi_thread_balance_length.cpp:213
argos::CSpaceMultiThreadBalanceLength::Destroy
virtual void Destroy()
Destroys the space and all its entities.
Definition: space_multi_thread_balance_length.cpp:103
argos::CSpaceMultiThreadBalanceLength::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the space using the <arena> section of the XML configuration file.
Definition: space_multi_thread_balance_length.cpp:67
argos::CProfiler::CollectThreadResourceUsage
void CollectThreadResourceUsage()
Definition: profiler.cpp:172