00001 #!/usr/bin/env python 00002 # -*- coding: euc-jp -*- 00003 00004 ## 00005 # @file InPort.py 00006 # @brief InPort template class 00007 # @date $Date: 2007/09/20 $ 00008 # @author Noriaki Ando <n-ando@aist.go.jp> and Shinji Kurihara 00009 # 00010 # Copyright (C) 2003-2008 00011 # Task-intelligence Research Group, 00012 # Intelligent Systems Research Institute, 00013 # National Institute of 00014 # Advanced Industrial Science and Technology (AIST), Japan 00015 # All rights reserved. 00016 00017 from omniORB import any 00018 import sys 00019 import traceback 00020 00021 import OpenRTM 00022 00023 TIMEOUT_TICK_USEC = 10.0 00024 USEC_PER_SEC = 1000000.0 00025 TIMEOUT_TICK_SEC = TIMEOUT_TICK_USEC/USEC_PER_SEC 00026 00027 00028 import time 00029 00030 00031 00032 ## 00033 # @if jp 00034 # @class Time 00035 # @brief 時間管理用クラス 00036 # 00037 # 指定した時間値を保持するためのクラス。 00038 # 00039 # @since 0.4.1 00040 # 00041 # @else 00042 # 00043 # @endif 00044 class Time: 00045 00046 00047 00048 ## 00049 # @if jp 00050 # @brief コンストラクタ 00051 # 00052 # コンストラクタ。 00053 # 00054 # @param self 00055 # 00056 # @else 00057 # @brief Constructor. 00058 # 00059 # Constructor. 00060 # 00061 # @param self 00062 # 00063 # @endif 00064 def __init__(self): 00065 tm = time.time() 00066 tm_f = tm - int(tm) # 小数部の取り出し 00067 self.sec = int(tm - tm_f) # 整数部の取り出し 00068 self.usec = int(tm_f * USEC_PER_SEC) # sec -> usec (micro second) 00069 00070 00071 00072 ## 00073 # @if jp 00074 # 00075 # @class InPort 00076 # 00077 # @brief InPort クラス 00078 # 00079 # InPort の実装クラス。 00080 # InPort は内部にリングバッファを持ち、外部から送信されたデータを順次 00081 # このリングバッファに格納する。リングバッファのサイズはデフォルトで64と 00082 # なっているが、コンストラクタ引数によりサイズを指定することができる。 00083 # データはフラグによって未読、既読状態が管理され、isNew(), getNewDataLen() 00084 # getNewList(), getNewListReverse() 等のメソッドによりハンドリングすることが 00085 # できる。 00086 # 00087 # @since 0.2.0 00088 # 00089 # @else 00090 # 00091 # @class InPort 00092 # 00093 # @brief InPort template class 00094 # 00095 # This class template provides interfaces to input port. 00096 # Component developer can define input value, which act as input 00097 # port from other components, using this template. 00098 # This is class template. This class have to be incarnated class as port 00099 # value types. This value types are previously define RtComponent IDL. 00100 # ex. type T: TimedFload, TimedLong etc... 00101 # 00102 # @since 0.2.0 00103 # 00104 # @endif 00105 class InPort: 00106 """ 00107 """ 00108 00109 00110 00111 ## 00112 # @if jp 00113 # 00114 # @brief コンストラクタ 00115 # 00116 # コンストラクタ。 00117 # 00118 # @param self 00119 # @param name InPort 名。InPortBase:name() により参照される。 00120 # @param value この InPort にバインドされる変数 00121 # @param buffer_ InPort が内部に保持するバッファ 00122 # @param read_block 読込ブロックフラグ。 00123 # データ読込時に未読データがない場合、次のデータ受信までブロックする 00124 # かどうかを設定(デフォルト値:False) 00125 # @param write_block 書込ブロックフラグ。 00126 # データ書込時にバッファがフルであった場合、バッファに空きができる 00127 # までブロックするかどうかを設定(デフォルト値:False) 00128 # @param read_timeout 読込ブロックを指定していない場合の、データ読取タイム 00129 # アウト時間(ミリ秒)(デフォルト値:0) 00130 # @param write_timeout 書込ブロックを指定していない場合の、データ書込タイム 00131 # アウト時間(ミリ秒)(デフォルト値:0) 00132 # 00133 # @else 00134 # 00135 # @brief A constructor. 00136 # 00137 # Setting channel name and registering channel value. 00138 # 00139 # @param self 00140 # @param name A name of the InPort. This name is referred by 00141 # InPortBase::name(). 00142 # @param value A channel value related with the channel. 00143 # @param buffer_ Buffer length of internal ring buffer of InPort 00144 # @param read_block 00145 # @param write_block 00146 # @param read_timeout 00147 # @param write_timeout 00148 # 00149 # @endif 00150 def __init__(self, name, value, buffer_, 00151 read_block=False, write_block=False, 00152 read_timeout=0, write_timeout = 0): 00153 self._buffer = buffer_ 00154 self._name = name 00155 self._value = value 00156 self._readBlock = read_block 00157 self._readTimeout = read_timeout 00158 self._writeBlock = write_block 00159 self._writeTimeout = write_timeout 00160 self._OnWrite = None 00161 self._OnWriteConvert = None 00162 self._OnRead = None 00163 self._OnReadConvert = None 00164 self._OnOverflow = None 00165 self._OnUnderflow = None 00166 00167 00168 ## 00169 # @if jp 00170 # @brief 最新データか確認 00171 # 00172 # 現在のバッファ位置に格納されているデータが最新データか確認する。 00173 # 00174 # @param self 00175 # 00176 # @return 最新データ確認結果 00177 # ( true:最新データ.データはまだ読み出されていない 00178 # false:過去のデータ.データは既に読み出されている) 00179 # 00180 # @else 00181 # 00182 # @endif 00183 def isNew(self): 00184 return self._buffer.isNew() 00185 00186 00187 ## 00188 # @if jp 00189 # @brief ポート名称を取得する。 00190 # 00191 # ポート名称を取得する。 00192 # 00193 # @param self 00194 # 00195 # @return ポート名称 00196 # 00197 # @else 00198 # 00199 # @endif 00200 def name(self): 00201 return self._name 00202 00203 00204 ## 00205 # @if jp 00206 # 00207 # @brief DataPort に値を書き込む 00208 # 00209 # DataPort に値を書き込む。 00210 # 00211 # - コールバックファンクタ OnWrite がセットされている場合、 00212 # InPort が保持するバッファに書き込む前に OnWrite が呼ばれる。 00213 # - InPort が保持するバッファがオーバーフローを検出できるバッファであり、 00214 # かつ、書き込む際にバッファがオーバーフローを検出した場合、 00215 # コールバックファンクタ OnOverflow が呼ばれる。 00216 # - コールバックファンクタ OnWriteConvert がセットされている場合、 00217 # バッファ書き込み時に、OnWriteConvert の operator()() の戻り値が 00218 # バッファに書き込まれる。 00219 # - setWriteTimeout() により書き込み時のタイムアウトが設定されている場合、 00220 # タイムアウト時間だけバッファフル状態が解除するのを待ち、 00221 # OnOverflowがセットされていればこれを呼び出して戻る。 00222 # 00223 # @param self 00224 # @param value 書込対象データ 00225 # 00226 # @return 書込処理結果(書込成功:true、書込失敗:false) 00227 # 00228 # @else 00229 # 00230 # @brief 00231 # 00232 # @endif 00233 def write(self, value): 00234 if self._OnWrite: 00235 self._OnWrite(value) 00236 00237 timeout = self._writeTimeout 00238 00239 tm_pre = Time() 00240 00241 # blocking and timeout wait 00242 while self._writeBlock and self._buffer.isFull(): 00243 if self._writeTimeout < 0: 00244 time.sleep(TIMEOUT_TICK_SEC) 00245 continue 00246 00247 # timeout wait 00248 tm_cur = Time() 00249 00250 sec = tm_cur.sec - tm_pre.sec 00251 usec = tm_cur.usec - tm_pre.usec 00252 00253 timeout -= (sec * USEC_PER_SEC + usec) 00254 00255 if timeout < 0: 00256 break 00257 00258 tm_pre = tm_cur 00259 time.sleep(TIMEOUT_TICK_USEC) 00260 00261 if self._buffer.isFull() and self._OnOverflow: 00262 self._OnOverflow(value) 00263 return False 00264 00265 if not self._OnWriteConvert: 00266 self._buffer.put(value) 00267 else: 00268 self._buffer.put(self._OnWriteConvert(value)) 00269 00270 return True 00271 00272 00273 ## 00274 # @if jp 00275 # 00276 # @brief DataPort から値を読み出す 00277 # 00278 # DataPort から値を読み出す 00279 # 00280 # - コールバックファンクタ OnRead がセットされている場合、 00281 # DataPort が保持するバッファから読み出す前に OnRead が呼ばれる。 00282 # - DataPort が保持するバッファがアンダーフローを検出できるバッファで、 00283 # かつ、読み出す際にバッファがアンダーフローを検出した場合、 00284 # コールバックファンクタ OnUnderflow が呼ばれる。 00285 # - コールバックファンクタ OnReadConvert がセットされている場合、 00286 # バッファ書き込み時に、OnReadConvert の operator()() の戻り値が 00287 # read()の戻り値となる。 00288 # - setReadTimeout() により読み出し時のタイムアウトが設定されている場合、 00289 # バッファアンダーフロー状態が解除されるまでタイムアウト時間だけ待ち、 00290 # OnUnderflowがセットされていればこれを呼び出して戻る 00291 # 00292 # @param self 00293 # 00294 # @return 読み出したデータ 00295 # 00296 # @else 00297 # 00298 # @brief [CORBA interface] Put data on InPort 00299 # 00300 # @endif 00301 def read(self): 00302 if self._OnRead: 00303 self._OnRead() 00304 00305 timeout = self._readTimeout 00306 00307 tm_pre = Time() 00308 00309 # blocking and timeout wait 00310 while self._readBlock and self._buffer.isEmpty(): 00311 if self._readTimeout < 0: 00312 time.sleep(TIMEOUT_TICK_SEC) 00313 continue 00314 00315 # timeout wait 00316 tm_cur = Time() 00317 00318 sec = tm_cur.sec - tm_pre.sec 00319 usec = tm_cur.usec - tm_pre.usec 00320 00321 timeout -= (sec * USEC_PER_SEC + usec) 00322 00323 if timeout < 0: 00324 break 00325 00326 tm_pre = tm_cur 00327 time.sleep(TIMEOUT_TICK_SEC) 00328 00329 if self._buffer.isEmpty(): 00330 if self._OnUnderflow: 00331 self._value = self._OnUnderflow() 00332 return self._value 00333 00334 if not self._OnReadConvert: 00335 self._value = self._buffer.get() 00336 return self._value 00337 else: 00338 self._value = self._OnReadConvert(self._buffer.get()) 00339 return self._value 00340 00341 # never comes here 00342 return self._value 00343 00344 00345 ## 00346 # @if jp 00347 # 00348 # @brief InPort 内のリングバッファの値を初期化(サブクラス実装用) 00349 # 00350 # InPort 内のリングバッファの値を指定した値で初期化する。<BR> 00351 # ※サブクラスでの実装時参照用 00352 # 00353 # @param self 00354 # @param value 初期化対象データ 00355 # 00356 # @else 00357 # 00358 # @brief Initialize ring buffer value of InPort 00359 # 00360 # @endif 00361 def init(self, value): 00362 pass 00363 00364 00365 ## 00366 # @if jp 00367 # 00368 # @brief バインドされた変数に InPort バッファの最新値を読み込む 00369 # 00370 # バインドされたデータに InPort の最新値を読み込む。 00371 # コンストラクタで変数と InPort がバインドされていなければならない。 00372 # このメソッドはポリモーフィックに使用される事を前提としているため、 00373 # 型に依存しない引数、戻り値となっている。 00374 # 00375 # @param self 00376 # 00377 # @else 00378 # 00379 # @brief Read into bound T-type data from current InPort 00380 # 00381 # @endif 00382 def update(self): 00383 try: 00384 self._value = self._buffer.get() 00385 except: 00386 if self._OnUnderflow: 00387 self._OnUnderflow() 00388 else: 00389 traceback.print_exception(*sys.exc_info()) 00390 00391 return 00392 00393 00394 ## 00395 # @if jp 00396 # 00397 # @brief 未読の新しいデータ数を取得する 00398 # 00399 # バッファ内の未読データ数を取得する。 00400 # 00401 # @param self 00402 # 00403 # @return 未読データ数 00404 # 00405 # @else 00406 # 00407 # @brief Get number of new data to be read. 00408 # 00409 # @endif 00410 def getNewDataLen(self): 00411 return self._buffer.new_data_len() 00412 00413 00414 ## 00415 # @if jp 00416 # 00417 # @brief 未読の新しいデータを取得する 00418 # 00419 # バッファ内の未読データリストを取得する。 00420 # 00421 # @param self 00422 # 00423 # @return 未読データリスト 00424 # 00425 # @else 00426 # 00427 # \brief Get new data to be read. 00428 # 00429 # @endif 00430 def getNewList(self): 00431 return self._buffer.get_new_list() 00432 00433 00434 ## 00435 # @if jp 00436 # 00437 # @brief 未読の新しいデータを逆順(新->古)で取得する 00438 # 00439 # バッファ内の未読データを逆順(新->古)でリスト化し、取得する。 00440 # 00441 # @param self 00442 # 00443 # @return 未読データリスト 00444 # 00445 # @else 00446 # 00447 # \brief Get new data to be read. 00448 # 00449 # @endif 00450 def getNewListReverse(self): 00451 return self._buffer.get_new_rlist() 00452 00453 00454 ## 00455 # @if jp 00456 # 00457 # @brief InPort バッファへデータ入力時のコールバックの設定 00458 # 00459 # InPort が持つバッファにデータがputされたときに呼ばれるコールバック 00460 # オブジェクトを設定する。設定されるコールバックオブジェクトは 00461 # 引数に value を持ち、戻り値 void の __call__ 関数を実装している必要がある。 00462 # 00463 # <pre> 00464 # class MyOnWrite: 00465 # def __call__(self, value): 00466 # 処理<br> 00467 # </pre> 00468 # のようにコールバックオブジェクトを実装し、<br> 00469 # m_inport.setOnWrite(new MyOnWrite());<br> 00470 # のようにコールバックオブジェクトをセットする。 00471 # 00472 # @param self 00473 # @param on_write 設定対象コールバックオブジェクト 00474 # 00475 # @else 00476 # 00477 # @brief Get new data to be read. 00478 # 00479 # @endif 00480 def setOnWrite(self, on_write): 00481 self._OnWrite = on_write 00482 00483 00484 ## 00485 # @if jp 00486 # 00487 # @brief InPort バッファへデータ書き込み時のコールバックの設定 00488 # 00489 # InPort が持つバッファにデータ書き込まれる時に呼ばれるコールバック 00490 # オブジェクトを設定する。バッファにはコールバックオブジェクトの 00491 # 戻り値が設定される。 00492 # 00493 # @param self 00494 # @param on_wconvert 設定対象コールバックオブジェクト 00495 # 00496 # @else 00497 # 00498 # @endif 00499 def setOnWriteConvert(self, on_wconvert): 00500 self._OnWriteConvert = on_wconvert 00501 00502 00503 ## 00504 # @if jp 00505 # 00506 # @brief InPort バッファへデータ読み込み時のコールバックの設定 00507 # 00508 # InPort が持つバッファからデータが読み込まれる直前に呼ばれるコールバック 00509 # オブジェクトを設定する。 00510 # 00511 # @param self 00512 # @param on_read 設定対象コールバックオブジェクト 00513 # 00514 # @else 00515 # 00516 # @endif 00517 def setOnRead(self, on_read): 00518 self._OnRead = on_read 00519 00520 00521 ## 00522 # @if jp 00523 # 00524 # @brief InPort バッファへデータ読み出し時のコールバックの設定 00525 # 00526 # InPort が持つバッファからデータが読み出される際に呼ばれるコールバック 00527 # オブジェクトを設定する。コールバックオブジェクトの戻り値がread()メソッド 00528 # の呼出結果となる。 00529 # 00530 # @param self 00531 # @param on_rconvert 設定対象コールバックオブジェクト 00532 # 00533 # @else 00534 # 00535 # @endif 00536 def setOnReadConvert(self, on_rconvert): 00537 self._OnReadConvert = on_rconvert 00538 00539 00540 ## 00541 # @if jp 00542 # 00543 # @brief InPort バッファへバッファオーバーフロー時のコールバックの設定 00544 # 00545 # InPort が持つバッファでバッファオーバーフローが検出された際に呼び出される 00546 # コールバックオブジェクトを設定する。 00547 # 00548 # @param self 00549 # @param on_overflow 設定対象コールバックオブジェクト 00550 # 00551 # @else 00552 # 00553 # @endif 00554 def setOnOverflow(self, on_overflow): 00555 self._OnOverflow = on_overflow 00556 00557 00558 ## 00559 # @if jp 00560 # 00561 # @brief InPort バッファへバッファアンダーフロー時のコールバックの設定 00562 # 00563 # InPort が持つバッファでバッファアンダーフローが検出された際に呼び出される 00564 # コールバックオブジェクトを設定する。 00565 # 00566 # @param self 00567 # @param on_underflow 設定対象コールバックオブジェクト 00568 # 00569 # @else 00570 # 00571 # @endif 00572 def setOnUnderflow(self, on_underflow): 00573 self._OnUnderflow = on_underflow 00574 00575 00576 ## 00577 # @if jp 00578 # 00579 # @brief データ型名取得用メソッド 00580 # 00581 # データの型名を取得するため、InPortCorbaProviderから呼ばれる。 00582 # 00583 # @param self 00584 # 00585 # @return バッファに設定されているデータの型名 00586 # 00587 # @else 00588 # 00589 # @endif 00590 def getPortDataType(self): 00591 val = any.to_any(self._value) 00592 return str(val.typecode().name())