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 import threading
00019 import OpenRTM_aist
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 class RingBuffer(OpenRTM_aist.BufferBase):
00042 """
00043 """
00044
00045 RINGBUFFER_DEFAULT_LENGTH = 8
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 def __init__(self, length=RINGBUFFER_DEFAULT_LENGTH):
00072 self._overwrite = True
00073 self._readback = True
00074 self._timedwrite = False
00075 self._timedread = False
00076 self._wtimeout = OpenRTM_aist.TimeValue(1,0)
00077 self._rtimeout = OpenRTM_aist.TimeValue(1,0)
00078 self._length = length
00079 self._wpos = 0
00080 self._rpos = 0
00081 self._fillcount = 0
00082 self._wcount = 0
00083 self._buffer = [None for i in range(self._length)]
00084 self._pos_mutex = threading.RLock()
00085 self._full_mutex = threading.RLock()
00086 self._empty_mutex = threading.RLock()
00087 self._full_cond = threading.Condition(self._full_mutex)
00088 self._empty_cond = threading.Condition(self._empty_mutex)
00089
00090
00091 self.reset()
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134 def init(self, prop):
00135 self.__initLength(prop)
00136 self.__initWritePolicy(prop)
00137 self.__initReadPolicy(prop)
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158 def length(self, n = None):
00159 if n is None:
00160 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00161 return self._length
00162
00163 if n < 1:
00164 return OpenRTM_aist.BufferStatus.NOT_SUPPORTED
00165
00166 self._buffer = [None for i in range(n)]
00167 self._length = n
00168 self.reset()
00169 return OpenRTM_aist.BufferStatus.BUFFER_OK
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195 def reset(self):
00196 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00197 self._fillcount = 0
00198 self._wcount = 0
00199 self._wpos = 0
00200 self._rpos = 0
00201 return OpenRTM_aist.BufferStatus.BUFFER_OK
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225 def wptr(self, n = 0):
00226 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00227 return self._buffer[(self._wpos + n + self._length) % self._length]
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254 def advanceWptr(self, n = 1):
00255
00256
00257
00258
00259
00260
00261
00262 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00263 if (n > 0 and n > (self._length - self._fillcount)) or \
00264 (n < 0 and n < (-self._fillcount)):
00265 return OpenRTM_aist.BufferStatus.PRECONDITION_NOT_MET
00266
00267 self._wpos = (self._wpos + n + self._length) % self._length
00268 self._fillcount += n
00269 return OpenRTM_aist.BufferStatus.BUFFER_OK
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300 def put(self, value):
00301 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00302 self._buffer[self._wpos] = value
00303 return OpenRTM_aist.BufferStatus.BUFFER_OK
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348 def write(self, value, sec = -1, nsec = 0):
00349 try:
00350 self._full_cond.acquire()
00351 if self.full():
00352 timedwrite = self._timedwrite
00353 overwrite = self._overwrite
00354
00355 if not (sec < 0):
00356 timedwrite = True
00357 overwrite = False
00358
00359 if overwrite and not timedwrite:
00360 self.advanceRptr()
00361
00362 elif not overwrite and not timedwrite:
00363 self._full_cond.release()
00364 return OpenRTM_aist.BufferStatus.BUFFER_FULL
00365
00366 elif not overwrite and timedwrite:
00367
00368 if sec < 0:
00369 sec = self._wtimeout.sec()
00370 nsec = self._wtimeout.usec() * 1000
00371
00372
00373 if not self._full_cond.wait(sec + (nsec/1000000000.0)):
00374 self._full_cond.release()
00375 return OpenRTM_aist.BufferStatus.TIMEOUT
00376
00377 else:
00378 self._full_cond.release()
00379 return OpenRTM_aist.BufferStatus.PRECONDITION_NOT_MET
00380
00381 self._full_cond.release()
00382
00383 self.put(value)
00384
00385 self._empty_cond.acquire()
00386 empty = self.empty()
00387 if empty:
00388 self.advanceWptr(1)
00389 self._empty_cond.notify()
00390 else:
00391 self.advanceWptr(1)
00392 self._empty_cond.release()
00393
00394 return OpenRTM_aist.BufferStatus.BUFFER_OK
00395 except:
00396 return OpenRTM_aist.BufferStatus.BUFFER_OK
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421 def writable(self):
00422 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00423 return self._length - self._fillcount
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446 def full(self):
00447 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00448 return self._length == self._fillcount
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470 def rptr(self, n = 0):
00471 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00472 return self._buffer[(self._rpos + n + self._length) % self._length]
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497 def advanceRptr(self, n = 1):
00498
00499
00500
00501
00502
00503
00504 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00505 if (n > 0 and n > self._fillcount) or \
00506 (n < 0 and n < (self._fillcount - self._length)):
00507 return OpenRTM_aist.BufferStatus.PRECONDITION_NOT_MET
00508
00509 self._rpos = (self._rpos + n + self._length) % self._length
00510 self._fillcount -= n
00511 return OpenRTM_aist.BufferStatus.BUFFER_OK
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540 def get(self, value=None):
00541 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00542 if value is None:
00543 return self._buffer[self._rpos]
00544
00545 value[0] = self._buffer[self._rpos]
00546 return OpenRTM_aist.BufferStatus.BUFFER_OK
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592 def read(self, value, sec = -1, nsec = 0):
00593 self._empty_cond.acquire()
00594
00595 if self.empty():
00596 timedread = self._timedread
00597 readback = self._readback
00598
00599 if not (sec < 0):
00600 timedread = True
00601 readback = False
00602 sec = self._rtimeout.sec()
00603 nsec = self._rtimeout.usec() * 1000
00604
00605 if readback and not timedread:
00606 if not self._wcount > 0:
00607 self._empty_cond.release()
00608 return OpenRTM_aist.BufferStatus.BUFFER_EMPTY
00609 self.advanceRptr(-1)
00610
00611 elif not readback and not timedread:
00612 self._empty_cond.release()
00613 return OpenRTM_aist.BufferStatus.BUFFER_EMPTY
00614
00615 elif not readback and timedread:
00616 if sec < 0:
00617 sec = self._rtimeout.sec()
00618 nsec = self._rtimeout.usec() * 1000
00619
00620 if not self._empty_cond.wait(sec + (nsec/1000000000.0)):
00621 self._empty_cond.release()
00622 return OpenRTM_aist.BufferStatus.TIMEOUT
00623
00624 else:
00625 self._empty_cond.release()
00626 return OpenRTM_aist.BufferStatus.PRECONDITION_NOT_MET
00627
00628 self._empty_cond.release()
00629
00630 val = self.get()
00631
00632 if len(value) > 0:
00633 value[0] = val
00634 else:
00635 value.append(val)
00636
00637 self._full_cond.acquire()
00638 full_ = self.full()
00639
00640 if full_:
00641 self.advanceRptr()
00642 self._full_cond.notify()
00643 else:
00644 self.advanceRptr()
00645
00646 self._full_cond.release()
00647
00648
00649 return OpenRTM_aist.BufferStatus.BUFFER_OK
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677 def readable(self):
00678 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00679 return self._fillcount
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702 def empty(self):
00703 guard = OpenRTM_aist.ScopedLock(self._pos_mutex)
00704 return self._fillcount == 0
00705
00706
00707
00708 def __initLength(self, prop):
00709 if prop.getProperty("length"):
00710 n = [0]
00711 if OpenRTM_aist.stringTo(n, prop.getProperty("length")):
00712 n = n[0]
00713 if n > 0:
00714 self.length(n)
00715
00716
00717
00718 def __initWritePolicy(self, prop):
00719 policy = OpenRTM_aist.normalize([prop.getProperty("write.full_policy")])
00720
00721 if policy == "overwrite":
00722 self._overwrite = True
00723 self._timedwrite = False
00724
00725 elif policy == "do_nothing":
00726 self._overwrite = False
00727 self._timedwrite = False
00728
00729 elif policy == "block":
00730 self._overwrite = False
00731 self._timedwrite = True
00732
00733 tm = [0.0]
00734 if OpenRTM_aist.stringTo(tm, prop.getProperty("write.timeout")):
00735 tm = tm[0]
00736 if not (tm < 0):
00737 self._wtimeout.set_time(tm)
00738
00739
00740
00741 def __initReadPolicy(self, prop):
00742 policy = prop.getProperty("read.empty_policy")
00743
00744 if policy == "readback":
00745 self._readback = True
00746 self._timedread = False
00747
00748 elif policy == "do_nothing":
00749 self._readback = False
00750 self._timedread = False
00751
00752 elif policy == "block":
00753 self._readback = False
00754 self._timedread = True
00755 tm = [0.0]
00756 if OpenRTM_aist.stringTo(tm, prop.getProperty("read.timeout")):
00757 self._rtimeout.set_time(tm[0])