SHM
共有メモリを用いた高速で扱いやすいプロセス間通信マネージャ
shm_action.hpp
Go to the documentation of this file.
1 
13 #ifndef __SHM_ACTION_LIB_H__
14 #define __SHM_ACTION_LIB_H__
15 
16 #include <string>
17 #include <thread>
18 #include "shm_base.hpp"
19 
20 namespace irlab
21 {
22 
23 namespace shm
24 {
25 
26 enum ACTION_STATUS : uint8_t
27 {
28  ACTIVE,
29  REJECTED,
30  SUCCEEDED,
31  PREEMPTED,
32 };
33 
34 // ****************************************************************************
46 // ****************************************************************************
47 template <class Goal, class Result, class Feedback>
48 class ActionServer
49 {
50 public:
51  ActionServer(std::string name, PERM perm = DEFAULT_PERM);
52  ~ActionServer();
53 
54  void waitNewGoalAvailable();
55  Goal acceptNewGoal();
56  void rejectNewGoal();
57 
58  bool isPreemptRequested();
59  void setPreempted();
60 
61  void publishResult(const Result& result);
62  void publishFeedback(const Feedback& feedback);
63 
64 private:
65  void initializeExclusiveAccess();
66 
67  pthread_t thread;
68 
69  std::string shm_name;
70  PERM shm_perm;
71  SharedMemory *shared_memory;
72 
73  uint64_t start_timestamp_us;
74 
75  uint8_t *memory_ptr;
76 
77  pthread_mutex_t *goal_mutex;
78  pthread_cond_t *goal_condition;
79  uint64_t *goal_timestamp_us;
80  Goal *goal_ptr;
81  pthread_mutex_t *result_mutex;
82  pthread_cond_t *result_condition;
83  uint64_t *result_timestamp_us;
84  Result *result_ptr;
85  Feedback *feedback_ptr;
86  ACTION_STATUS *status_ptr;
87  uint64_t *cancel_timestamp_us;
88 
89  uint64_t current_goal_timestamp_usec;
90 };
91 
92 // ****************************************************************************
97 // ****************************************************************************
98 template <class Goal, class Result, class Feedback>
99 class ActionClient
100 {
101 public:
102  ActionClient(std::string name);
103  ~ActionClient();
104 
105  void cancelGoal();
106  Result getResult();
107  Feedback getFeedback();
108  ACTION_STATUS getStatus();
109  bool isServerConnected();
110  bool sendGoal(Goal goal);
111  bool waitForResult(unsigned long wait_time_us);
112  bool waitForServer(unsigned long wait_time_us);
113 
114 private:
115  std::string shm_name;
116  SharedMemory *shared_memory;
117 
118  uint8_t *memory_ptr;
119 
120  pthread_mutex_t *goal_mutex;
121  pthread_cond_t *goal_condition;
122  uint64_t *goal_timestamp_us;
123  Goal *goal_ptr;
124  pthread_mutex_t *result_mutex;
125  pthread_cond_t *result_condition;
126  uint64_t *result_timestamp_us;
127  Result *result_ptr;
128  Feedback *feedback_ptr;
129  ACTION_STATUS *status_ptr;
130  uint64_t *cancel_timestamp_us;
131 
132  uint64_t current_result_timestamp_usec;
133 };
134 
135 // ****************************************************************************
136 // 関数定義
137 // （テンプレートクラス内の関数の定義はコンパイル時に実体化するのでヘッダに書く）
138 // ****************************************************************************
139 template <class Goal, class Result, class Feedback>
140 ActionServer<Goal, Result, Feedback>::ActionServer(std::string name, PERM perm)
141 : shm_name(name)
142 , shm_perm(perm)
143 , shared_memory(nullptr)
144 , memory_ptr(nullptr)
145 {
146  if (!std::is_standard_layout<Goal>::value
147  || !std::is_standard_layout<Result>::value
148  || !std::is_standard_layout<Feedback>::value)
149  {
150  throw std::runtime_error("shm::ActionServer: Be setted not POD class!");
151  }
152 
153  shared_memory = new SharedMemoryPosix(shm_name, O_RDWR|O_CREAT, shm_perm);
154  shared_memory->connect( (sizeof(pthread_mutex_t)+sizeof(pthread_cond_t)+sizeof(uint64_t)) * 2 + sizeof(Goal) + sizeof(Result) + sizeof(Feedback) + sizeof(ACTION_STATUS) + sizeof(uint64_t));
155  if (shared_memory->isDisconnected())
156  {
157  throw std::runtime_error("shm::ActionServer: Cannot get memory!");
158  }
159 
160  uint8_t *data_ptr = shared_memory->getPtr();
161  memory_ptr = data_ptr;
162  goal_mutex = reinterpret_cast<pthread_mutex_t *>(data_ptr);
163  data_ptr += sizeof(pthread_mutex_t);
164  goal_condition = reinterpret_cast<pthread_cond_t *>(data_ptr);
165  data_ptr += sizeof(pthread_cond_t);
166  goal_timestamp_us = reinterpret_cast<uint64_t *>(data_ptr);
167  data_ptr += sizeof(uint64_t);
168  goal_ptr = reinterpret_cast<Goal *>(data_ptr);
169  data_ptr += sizeof(Goal);
170  result_mutex = reinterpret_cast<pthread_mutex_t *>(data_ptr);
171  data_ptr += sizeof(pthread_mutex_t);
172  result_condition = reinterpret_cast<pthread_cond_t *>(data_ptr);
173  data_ptr += sizeof(pthread_cond_t);
174  result_timestamp_us = reinterpret_cast<uint64_t *>(data_ptr);
175  data_ptr += sizeof(uint64_t);
176  result_ptr = reinterpret_cast<Result *>(data_ptr);
177  data_ptr += sizeof(Result);
178  feedback_ptr = reinterpret_cast<Feedback *>(data_ptr);
179  data_ptr += sizeof(Feedback);
180  status_ptr = reinterpret_cast<ACTION_STATUS *>(data_ptr);
181  data_ptr += sizeof(ACTION_STATUS);
182  cancel_timestamp_us = reinterpret_cast<uint64_t *>(data_ptr);
183 
184  initializeExclusiveAccess();
185 
186  *status_ptr = SUCCEEDED;
187 
188  struct timespec ts;
189  clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
190  *cancel_timestamp_us = ((uint64_t) ts.tv_sec * 1000000L) + ((uint64_t) ts.tv_nsec / 1000L);
191  start_timestamp_us = *cancel_timestamp_us;
192  *goal_timestamp_us = *cancel_timestamp_us;
193  *result_timestamp_us = *cancel_timestamp_us;
194 
195  current_goal_timestamp_usec = *goal_timestamp_us;
196 }
197 
198 template <class Goal, class Result, class Feedback>
199 ActionServer<Goal, Result, Feedback>::~ActionServer()
200 {
201  shared_memory->disconnect();
202  if (shared_memory != nullptr)
203  {
204  delete shared_memory;
205  }
206 }
207 
208 template <class Goal, class Result, class Feedback>
209 void
210 ActionServer<Goal, Result, Feedback>::initializeExclusiveAccess()
211 {
212  pthread_condattr_t goal_cond_attr;
213  pthread_condattr_init(&goal_cond_attr);
214  pthread_condattr_setpshared(&goal_cond_attr, PTHREAD_PROCESS_SHARED);
215  pthread_cond_init(goal_condition, &goal_cond_attr);
216  pthread_condattr_destroy(&goal_cond_attr);
217 
218  pthread_mutexattr_t goal_m_attr;
219  pthread_mutexattr_init(&goal_m_attr);
220  pthread_mutexattr_setpshared(&goal_m_attr, PTHREAD_PROCESS_SHARED);
221  pthread_mutex_init(goal_mutex, &goal_m_attr);
222  pthread_mutexattr_destroy(&goal_m_attr);
223 
224  pthread_condattr_t result_cond_attr;
225  pthread_condattr_init(&result_cond_attr);
226  pthread_condattr_setpshared(&result_cond_attr, PTHREAD_PROCESS_SHARED);
227  pthread_cond_init(result_condition, &result_cond_attr);
228  pthread_condattr_destroy(&result_cond_attr);
229 
230  pthread_mutexattr_t result_m_attr;
231  pthread_mutexattr_init(&result_m_attr);
232  pthread_mutexattr_setpshared(&result_m_attr, PTHREAD_PROCESS_SHARED);
233  pthread_mutex_init(result_mutex, &result_m_attr);
234  pthread_mutexattr_destroy(&result_m_attr);
235 }
236 
237 template <class Goal, class Result, class Feedback>
238 void
239 ActionServer<Goal, Result, Feedback>::waitNewGoalAvailable()
240 {
241  while (current_goal_timestamp_usec >= *goal_timestamp_us)
242  {
243  // Wait on the condvar
244  pthread_mutex_lock(goal_mutex);
245  pthread_cond_wait(goal_condition, goal_mutex);
246  pthread_mutex_unlock(goal_mutex);
247  }
248 }
249 
250 template <class Goal, class Result, class Feedback>
251 Goal
252 ActionServer<Goal, Result, Feedback>::acceptNewGoal()
253 {
254  *status_ptr = ACTIVE;
255  struct timespec ts;
256  clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
257  start_timestamp_us = ((uint64_t) ts.tv_sec * 1000000L) + ((uint64_t) ts.tv_nsec / 1000L);
258  current_goal_timestamp_usec = *goal_timestamp_us;
259 
260  return *goal_ptr;
261 }
262 
263 template <class Goal, class Result, class Feedback>
264 void
265 ActionServer<Goal, Result, Feedback>::rejectNewGoal()
266 {
267  *status_ptr = REJECTED;
268  current_goal_timestamp_usec = *goal_timestamp_us;
269  pthread_cond_broadcast(result_condition);
270 }
271 
272 template <class Goal, class Result, class Feedback>
273 bool
274 ActionServer<Goal, Result, Feedback>::isPreemptRequested()
275 {
276  if (start_timestamp_us < *cancel_timestamp_us)
277  {
278  return true;
279  }
280  return false;
281 }
282 
283 template <class Goal, class Result, class Feedback>
284 void
285 ActionServer<Goal, Result, Feedback>::setPreempted()
286 {
287  *status_ptr = PREEMPTED;
288 
289  struct timespec ts;
290  clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
291  *result_timestamp_us = ((uint64_t) ts.tv_sec * 1000000L) + ((uint64_t) ts.tv_nsec / 1000L);
292 
293  pthread_cond_broadcast(result_condition);
294 }
295 
296 template <class Goal, class Result, class Feedback>
297 void
298 ActionServer<Goal, Result, Feedback>::publishResult(const Result& result)
299 {
300  *result_ptr = result;
301  *status_ptr = SUCCEEDED;
302 
303  struct timespec ts;
304  clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
305  *result_timestamp_us = ((uint64_t) ts.tv_sec * 1000000L) + ((uint64_t) ts.tv_nsec / 1000L);
306 
307  pthread_cond_broadcast(result_condition);
308 }
309 
310 template <class Goal, class Result, class Feedback>
311 void
312 ActionServer<Goal, Result, Feedback>::publishFeedback(const Feedback& feedback)
313 {
314  *feedback_ptr = feedback;
315 }
316 
317 
318 template <class Goal, class Result, class Feedback>
319 ActionClient<Goal, Result, Feedback>::ActionClient(std::string name)
320 : shm_name(name)
321 , shared_memory(nullptr)
322 {
323  if (!std::is_standard_layout<Goal>::value
324  || !std::is_standard_layout<Result>::value
325  || !std::is_standard_layout<Feedback>::value)
326  {
327  throw std::runtime_error("shm::ActionClient: Be setted not POD class!");
328  }
329  shared_memory = new SharedMemoryPosix(shm_name, O_RDWR, static_cast<PERM>(0));
330 }
331 
332 template <class Goal, class Result, class Feedback>
333 ActionClient<Goal, Result, Feedback>::~ActionClient()
334 {
335  if (shared_memory != nullptr)
336  {
337  delete shared_memory;
338  }
339 }
340 
341 template <class Goal, class Result, class Feedback>
342 bool
343 ActionClient<Goal, Result, Feedback>::isServerConnected()
344 {
345  // Check the action shared memory existance.
346  if (shared_memory->isDisconnected())
347  {
348  shared_memory->connect();
349  if (shared_memory->isDisconnected())
350  {
351  return false;
352  }
353  uint8_t *data_ptr = shared_memory->getPtr();
354  memory_ptr = data_ptr;
355  goal_mutex = reinterpret_cast<pthread_mutex_t *>(data_ptr);
356  data_ptr += sizeof(pthread_mutex_t);
357  goal_condition = reinterpret_cast<pthread_cond_t *>(data_ptr);
358  data_ptr += sizeof(pthread_cond_t);
359  goal_timestamp_us = reinterpret_cast<uint64_t *>(data_ptr);
360  data_ptr += sizeof(uint64_t);
361  goal_ptr = reinterpret_cast<Goal *>(data_ptr);
362  data_ptr += sizeof(Goal);
363  result_mutex = reinterpret_cast<pthread_mutex_t *>(data_ptr);
364  data_ptr += sizeof(pthread_mutex_t);
365  result_condition = reinterpret_cast<pthread_cond_t *>(data_ptr);
366  data_ptr += sizeof(pthread_cond_t);
367  result_timestamp_us = reinterpret_cast<uint64_t *>(data_ptr);
368  data_ptr += sizeof(uint64_t);
369  result_ptr = reinterpret_cast<Result *>(data_ptr);
370  data_ptr += sizeof(Result);
371  feedback_ptr = reinterpret_cast<Feedback *>(data_ptr);
372  data_ptr += sizeof(Feedback);
373  status_ptr = reinterpret_cast<ACTION_STATUS *>(data_ptr);
374  data_ptr += sizeof(ACTION_STATUS);
375  cancel_timestamp_us = reinterpret_cast<uint64_t *>(data_ptr);
376  }
377 
378  return true;
379 }
380 
381 template <class Goal, class Result, class Feedback>
382 bool
383 ActionClient<Goal, Result, Feedback>::sendGoal(Goal goal)
384 {
385  // Check the action shared memory existance.
386  if (!(isServerConnected()))
387  {
388  return false;
389  }
390 
391  current_result_timestamp_usec = *result_timestamp_us;
392 
393  // Set request to shared memory
394  *goal_ptr = goal;
395  struct timespec ts;
396  clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
397  *goal_timestamp_us = ((uint64_t) ts.tv_sec * 1000000L) + ((uint64_t) ts.tv_nsec / 1000L);
398  pthread_cond_broadcast(goal_condition);
399 
400  return true;
401 }
402 
403 template <class Goal, class Result, class Feedback>
404 Result
405 ActionClient<Goal, Result, Feedback>::getResult()
406 {
407  return *result_ptr;
408 }
409 
410 template <class Goal, class Result, class Feedback>
411 Feedback
412 ActionClient<Goal, Result, Feedback>::getFeedback()
413 {
414  return *feedback_ptr;
415 }
416 
417 template <class Goal, class Result, class Feedback>
418 ACTION_STATUS
419 ActionClient<Goal, Result, Feedback>::getStatus()
420 {
421  return *status_ptr;
422 }
423 
424 template <class Goal, class Result, class Feedback>
425 void
426 ActionClient<Goal, Result, Feedback>::cancelGoal()
427 {
428  struct timespec ts;
429  clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
430  *cancel_timestamp_us = ((uint64_t) ts.tv_sec * 1000000L) + ((uint64_t) ts.tv_nsec / 1000L);
431 }
432 
433 template <class Goal, class Result, class Feedback>
434 bool
435 ActionClient<Goal, Result, Feedback>::waitForResult(unsigned long wait_time_us)
436 {
437  struct timespec ts;
438  long sec = static_cast<long>(wait_time_us / 1000000);
439  long mod_sec = static_cast<long>(wait_time_us % 1000000);
440  clock_gettime(CLOCK_REALTIME, &ts);
441  ts.tv_sec += sec;
442  ts.tv_nsec+= mod_sec * 1000;
443  if (1000000000 <= ts.tv_nsec)
444  {
445  ts.tv_nsec -= 1000000000;
446  ts.tv_sec += 1;
447  }
448 
449  while (current_result_timestamp_usec >= *result_timestamp_us)
450  {
451  // Wait on the condvar
452  pthread_mutex_lock(result_mutex);
453  int ret = pthread_cond_timedwait(result_condition, result_mutex, &ts);
454  pthread_mutex_unlock(result_mutex);
455  if (ret == ETIMEDOUT)
456  {
457  return false;
458  }
459  }
460  return true;
461 }
462 
463 template <class Goal, class Result, class Feedback>
464 bool
465 ActionClient<Goal, Result, Feedback>::waitForServer(uint64_t wait_time_us)
466 {
467  static const uint64_t SLEEP_PERIOD_us = 100000;
468 
469  if (isServerConnected())
470  {
471  return true;
472  }
473 
474  for (uint64_t time_index = 0; time_index < wait_time_us / SLEEP_PERIOD_us; time_index++)
475  {
476  if (isServerConnected())
477  {
478  return true;
479  }
480  usleep(SLEEP_PERIOD_us);
481  }
482 
483  return false;
484 }
485 
486 }
487 
488 }
489 
490 #endif //__SHM_ACTION_LIB_H__
shm_base.hpp
共有メモリへのアクセス方法やリングバッファなどの基本的なクラスの定義
irlab::shm::PERM
PERM
Definition: shm_base.hpp:40
irlab::shm::ActionServer
共有メモリで受信したリクエストからレスポンスを返すサーバーを表現するクラス
Definition: shm_action.hpp:48
irlab::shm::SharedMemory
共有メモリへのアクセス方法を抽象化したクラス
Definition: shm_base.hpp:84
irlab::shm::ActionClient
共有メモリからトピックを取得する購読者を表現するクラス
Definition: shm_action.hpp:99
Generated on Sun Mar 12 2023 14:59:22 for SHM by   doxygen 1.8.17