ARGoS 3
A parallel, multi-engine simulator for swarm robotics
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
11namespace 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
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}
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition datatypes.h:97
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition argos_exception.h:111
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
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::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::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::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