PeriodicExecutionContext.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: euc-jp -*-
3 
4 
16 
17 import sys
18 import copy
19 import threading
20 import time
21 from omniORB import CORBA, PortableServer
22 
23 import OpenRTM_aist
24 import OpenRTM
25 import RTC
26 
27 DEFAULT_PERIOD = 0.000001
28 
29 
43  """
44  """
45 
46 
47 
48 
61  class DFP:
62  """
63  """
64 
65 
78  def __init__(self, obj, id_):
79  self._obj = obj
80  self._active = True
81  self.ec_id = id_
83  self._sm.setListener(self)
84  self._sm.setEntryAction (RTC.ACTIVE_STATE,
85  self.on_activated)
86  self._sm.setDoAction (RTC.ACTIVE_STATE,
87  self.on_execute)
88  self._sm.setPostDoAction(RTC.ACTIVE_STATE,
89  self.on_state_update)
90  self._sm.setExitAction (RTC.ACTIVE_STATE,
91  self.on_deactivated)
92  self._sm.setEntryAction (RTC.ERROR_STATE,
93  self.on_aborting)
94  self._sm.setDoAction (RTC.ERROR_STATE,
95  self.on_error)
96  self._sm.setExitAction (RTC.ERROR_STATE,
97  self.on_reset)
99  st.prev = RTC.INACTIVE_STATE
100  st.curr = RTC.INACTIVE_STATE
101  st.next = RTC.INACTIVE_STATE
102  self._sm.setStartState(st)
103  self._sm.goTo(RTC.INACTIVE_STATE)
104 
105 
106 
120  def on_startup(self):
121  return self._obj.on_startup(self.ec_id)
122 
123 
124 
136  def on_shutdown(self):
137  return self._obj.on_shutdown(self.ec_id)
138 
139 
140 
155  def on_activated(self, st):
156  if self._obj.on_activated(self.ec_id) != RTC.RTC_OK:
157  self._sm.goTo(RTC.ERROR_STATE)
158  return
159  return
160 
161 
162 
176  def on_deactivated(self, st):
178 
179 
180 
195  def on_aborting(self, st):
196  self._obj.on_aborting(self.ec_id)
197 
198 
199 
214  def on_error(self, st):
215  self._obj.on_error(self.ec_id)
216 
217 
218 
231  def on_reset(self, st):
232  if self._obj.on_reset(self.ec_id) != RTC.RTC_OK:
233  self._sm.goTo(RTC.ERROR_STATE)
234  return
235  return
236 
237 
238 
254  def on_execute(self, st):
255  if self._obj.on_execute(self.ec_id) != RTC.RTC_OK:
256  self._sm.goTo(RTC.ERROR_STATE)
257  return
258  return
259 
260 
261 
277  def on_state_update(self, st):
278  if self._obj.on_state_update(self.ec_id) != RTC.RTC_OK:
279  self._sm.goTo(RTC.ERROR_STATE)
280  return
281  return
282 
283 
284 
296  def on_rate_changed(self):
298 
299 
300 
315  def worker(self):
316  return self._sm.worker()
317 
318 
319 
334  def get_state(self):
335  return self._sm.getState()
336 
337 
338 
352  def __init__(self, owner=None, rate=None):
353  self._rtcout = OpenRTM_aist.Manager.instance().getLogbuf("rtobject.periodic_ec")
354  self._rtcout.RTC_TRACE("PeriodicExecutionContext()")
355 
356  OpenRTM_aist.Task.__init__(self)
357 
358  self._nowait = False
359  self._running = False
361  self._worker = self.Worker()
363  global DEFAULT_PERIOD
364 
365  if rate is None:
366  self._period = OpenRTM_aist.TimeValue(DEFAULT_PERIOD)
367  else:
368  if rate == 0:
369  rate = 1.0 / DEFAULT_PERIOD
370  self._period = OpenRTM_aist.TimeValue(1.0 / rate)
371 
372  if self._period.sec() == 0 and self._period.usec() < 0.000001:
373  self._nowait = True
374 
375  self._rtcout.RTC_DEBUG("Actual rate: %d [sec], %d [usec]",
376  (self._period.sec(), self._period.usec()))
377 
378  self._comps = []
379  self._profile = RTC.ExecutionContextProfile(RTC.PERIODIC, (1.0/self._period.toDouble()), None, [], [])
380  self._ref = self._this()
381  self._mutex_del = threading.RLock()
382  return
383 
384 
385  def __del__(self, Task=OpenRTM_aist.Task):
386  self._rtcout.RTC_TRACE("~PeriodicExecutionContext()")
387  self._worker._cond.acquire()
388  self._worker._running = True
389  self._worker._cond.notify()
390  self._worker._cond.release()
391  self._running = False
392  #self.wait()
393 
394  self._profile.owner = None
395  self._profile.paarticipants = []
396  self._profile.properties = []
397  guard = OpenRTM_aist.ScopedLock(self._mutex_del)
398  Task.__del__(self)
399  del guard
400 
401 
415  def getObjRef(self):
416  return self._ref
417 
418 
419 
434  def svc(self):
435  self._rtcout.RTC_TRACE("svc()")
436  flag = True
437  count_ = 0
438 
439  guard = OpenRTM_aist.ScopedLock(self._mutex_del)
440  while flag:
441  self._worker._cond.acquire()
442  while not self._worker._running:
443  self._worker._cond.wait()
444 
445  t0_ = OpenRTM_aist.Time()
446 
447  if self._worker._running:
448  for comp in self._comps:
449  comp._sm.worker()
450 
451  self._worker._cond.release()
452 
453  t1_ = OpenRTM_aist.Time()
454 
455  if count_ > 1000:
456  exctm_ = (t1_ - t0_).getTime().toDouble()
457  slptm_ = self._period.toDouble() - exctm_
458  self._rtcout.RTC_PARANOID("Period: %f [s]", self._period.toDouble())
459  self._rtcout.RTC_PARANOID("Execution: %f [s]", exctm_)
460  self._rtcout.RTC_PARANOID("Sleep: %f [s]", slptm_)
461 
462  t2_ = OpenRTM_aist.Time()
463 
464  if not self._nowait and self._period.toDouble() > ((t1_ - t0_).getTime().toDouble()):
465  if count_ > 1000:
466  self._rtcout.RTC_PARANOID("sleeping...")
467  slptm_ = self._period.toDouble() - (t1_ - t0_).getTime().toDouble()
468  time.sleep(slptm_)
469 
470  if count_ > 1000:
471  t3_ = OpenRTM_aist.Time()
472  self._rtcout.RTC_PARANOID("Slept: %f [s]", (t3_ - t2_).getTime().toDouble())
473  count_ = 0
474  count_ += 1
475  flag = self._running
476  del guard
477  return 0
478 
479 
480 
502  def close(self, flags):
503  self._rtcout.RTC_TRACE("close()")
504  return 0
505 
506 
507 
530  def is_running(self):
531  self._rtcout.RTC_TRACE("is_running()")
532  return self._running
533 
534 
535 
562  def start(self):
563  self._rtcout.RTC_TRACE("start()")
564  if self._running:
565  return RTC.PRECONDITION_NOT_MET
566 
567  for comp in self._comps:
568  comp._sm.on_startup()
569 
570  self._running = True
571 
572  self._worker._cond.acquire()
573  self._worker._running = True
574  self._worker._cond.notify()
575  self._worker._cond.release()
576 
577  try:
578  self.activate()
579  except:
580  self._running = False
581 
582  self._worker._cond.acquire()
583  self._worker._running = False
584  self._worker._cond.notify()
585  self._worker._cond.release()
586  self._rtcout.RTC_ERROR(OpenRTM_aist.Logger.print_exception())
587 
588  return RTC.RTC_OK
589 
590 
591 
617  def stop(self):
618  self._rtcout.RTC_TRACE("stop()")
619  if not self._running:
620  return RTC.PRECONDITION_NOT_MET
621 
622  self._running = False
623  self._worker._cond.acquire()
624  self._worker._running = False
625  self._worker._cond.release()
626 
627  for comp in self._comps:
628  comp._sm.on_shutdown()
629 
630  #self.wait()
631  return RTC.RTC_OK
632 
633 
634 
652  def get_rate(self):
653  self._rtcout.RTC_TRACE("get_rate()")
654  return self._profile.rate
655 
656 
657 
682  def set_rate(self, rate):
683  self._rtcout.RTC_TRACE("set_rate(%f)", rate)
684  if rate > 0.0:
685  self._profile.rate = rate
686  self._period.set_time(1.0/rate)
687  if self._period.toDouble() == 0.0:
688  self._nowait = True
689 
690  for comp in self._comps:
691  comp._sm.on_rate_changed()
692  return RTC.RTC_OK
693  return RTC.BAD_PARAMETER
694 
695 
696 
725  def activate_component(self, comp):
726  self._rtcout.RTC_TRACE("activate_component()")
727  for compIn in self._comps:
728  if compIn._ref._is_equivalent(comp):
729  if not compIn._sm._sm.isIn(RTC.INACTIVE_STATE):
730  return RTC.PRECONDITION_NOT_MET
731  compIn._sm._sm.goTo(RTC.ACTIVE_STATE)
732  return RTC.RTC_OK
733 
734  return RTC.BAD_PARAMETER
735 
736 
737 
766  def deactivate_component(self, comp):
767  self._rtcout.RTC_TRACE("deactivate_component()")
768  for compIn in self._comps:
769  if compIn._ref._is_equivalent(comp):
770  if not compIn._sm._sm.isIn(RTC.ACTIVE_STATE):
771  return RTC.PRECONDITION_NOT_MET
772  compIn._sm._sm.goTo(RTC.INACTIVE_STATE)
773  count_ = 0
774  usec_per_sec_ = 1.0e6
775  sleeptime_ = 10.0 * usec_per_sec_ / float(self.get_rate())
776  self._rtcout.RTC_PARANOID("Sleep time is %f [us]", sleeptime_)
777  while compIn._sm._sm.isIn(RTC.ACTIVE_STATE):
778  self._rtcout.RTC_TRACE("Waiting to be the INACTIVE state %d %f", (count_, float(time.time())))
779  time.sleep(sleeptime_/usec_per_sec_)
780  if count_ > 1000:
781  self._rtcout.RTC_ERROR("The component is not responding.")
782  break
783  count_ += 1
784  if compIn._sm._sm.isIn(RTC.INACTIVE_STATE):
785  self._rtcout.RTC_TRACE("The component has been properly deactivated.")
786  return RTC.RTC_OK
787  self._rtcout.RTC_ERROR("The component could not be deactivated.")
788  return RTC.RTC_ERROR
789 
790  return RTC.BAD_PARAMETER
791 
792 
793 
821  def reset_component(self, comp):
822  self._rtcout.RTC_TRACE("reset_component()")
823  for compIn in self._comps:
824  if compIn._ref._is_equivalent(comp):
825  if not compIn._sm._sm.isIn(RTC.ERROR_STATE):
826  return RTC.PRECONDITION_NOT_MET
827  compIn._sm._sm.goTo(RTC.INACTIVE_STATE)
828  return RTC.RTC_OK
829 
830  return RTC.BAD_PARAMETER
831 
832 
833 
854  def get_component_state(self, comp):
855  self._rtcout.RTC_TRACE("get_component_state()")
856  for compIn in self._comps:
857  if compIn._ref._is_equivalent(comp):
858  return compIn._sm._sm.getState()
859 
860  return RTC.CREATED_STATE
861 
862 
863 
880  def get_kind(self):
881  self._rtcout.RTC_TRACE("get_kind()")
882  return self._profile.kind
883 
884 
885 
911  def add_component(self, comp):
912  self._rtcout.RTC_TRACE("add_component()")
913  if CORBA.is_nil(comp):
914  return RTC.BAD_PARAMETER
915  try:
916  dfp_ = comp._narrow(OpenRTM.DataFlowComponent)
917  rtc_ = comp._narrow(RTC.RTObject)
918  if CORBA.is_nil(dfp_) or CORBA.is_nil(rtc_):
919  return RTC.BAD_PARAMETER
920 
921  id_ = dfp_.attach_context(self._ref)
922  comp_ = self.Comp(ref=comp, dfp=dfp_, id=id_)
923  self._comps.append(comp_)
924  self._profile.participants.append(rtc_)
925  return RTC.RTC_OK
926  except CORBA.Exception:
927  self._rtcout.RTC_ERROR(OpenRTM_aist.Logger.print_exception())
928  return RTC.BAD_PARAMETER
929 
930  return RTC.RTC_OK
931 
932 
933  def bindComponent(self, rtc):
934  self._rtcout.RTC_TRACE("bindComponent()")
935  if rtc is None:
936  return RTC.BAD_PARAMETER
937 
938  comp_ = rtc.getObjRef()
939  dfp_ = comp_._narrow(OpenRTM.DataFlowComponent)
940  id_ = rtc.bindContext(self._ref)
941  if id_ < 0 or id_ > OpenRTM_aist.ECOTHER_OFFSET:
942  self._rtcout.RTC_ERROR("bindContext returns invalid id: %d", id_)
943  return RTC.RTC_ERROR
944 
945  self._rtcout.RTC_DEBUG("bindContext returns id = %d", id_)
946  # rtc is owner of this EC
947  self._comps.append(self.Comp(ref=comp_, dfp=dfp_, id=id_))
948  self._profile.owner = dfp_
949  return RTC.RTC_OK
950 
951 
952 
976  def remove_component(self, comp):
977  self._rtcout.RTC_TRACE("remove_component()")
978  len_ = len(self._comps)
979  for i in range(len_):
980  idx = (len_ - 1) - i
981  if self._comps[idx]._ref._is_equivalent(comp):
982  self._comps[idx]._ref.detach_context(self._comps[idx]._sm.ec_id)
983  del self._comps[idx]
984  rtcomp = comp._narrow(RTC.RTObject)
985  if CORBA.is_nil(rtcomp):
986  self._rtcout.RTC_ERROR("Invalid object reference.")
987  return RTC.RTC_ERROR
989  self.find_participant(rtcomp))
990  return RTC.RTC_OK
991 
992  return RTC.BAD_PARAMETER
993 
994 
995 
1013  def get_profile(self):
1014  self._rtcout.RTC_TRACE("get_profile()")
1015  return self._profile
1016 
1017 
1018  class find_participant:
1019  def __init__(self, comp):
1020  self._comp = comp
1021  return
1022 
1023  def __call__(self, comp):
1024  return self._comp._is_equivalent(comp)
1025 
1026 
1027 
1033  class Comp:
1034  def __init__(self, ref=None, dfp=None, id=None, comp=None):
1035  if comp is None:
1036  self._ref = ref
1038  else:
1039  self._ref = comp._ref
1040  self._sm = PeriodicExecutionContext.DFP(comp._sm._obj,comp._sm.ec_id)
1041 
1042 
1054  class Worker:
1056 
1067  def __init__(self):
1068  self._mutex = threading.RLock()
1069  self._cond = threading.Condition(self._mutex)
1070  self._running = False
1072 
1083 def PeriodicExecutionContextInit(manager):
1084  manager.registerECFactory("PeriodicExecutionContext",
1086  OpenRTM_aist.ECDelete)
1087 
def remove_component(self, comp)
Remove the RT-component from participant list.
def activate(self)
Definition: Task.py:48
def get_component_state(self, comp)
Get RT-component&#39;s state.
def is_running(self)
Check for ExecutionContext running state.
def append(dest, src)
Definition: NVUtil.py:386


openrtm_aist_python
Author(s): Shinji Kurihara
autogenerated on Mon Feb 28 2022 23:01:06