PeriodicExecutionContext.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: euc-jp -*-



import sys
import copy
import threading
import time
from omniORB import CORBA, PortableServer

import OpenRTM_aist
import OpenRTM
import RTC

DEFAULT_PERIOD = 0.000001


class PeriodicExecutionContext(OpenRTM_aist.ExecutionContextBase,                               OpenRTM_aist.Task):
  """
  """



  
  class DFP:
    """
    """

    
    def __init__(self, obj, id_):
      self._obj = obj
      self._active = True
      self.ec_id = id_
      self._sm = OpenRTM_aist.StateMachine(4)
      self._sm.setListener(self)
      self._sm.setEntryAction (RTC.ACTIVE_STATE,
                               self.on_activated)
      self._sm.setDoAction    (RTC.ACTIVE_STATE,
                               self.on_execute)
      self._sm.setPostDoAction(RTC.ACTIVE_STATE,
                               self.on_state_update)
      self._sm.setExitAction  (RTC.ACTIVE_STATE,
                               self.on_deactivated)
      self._sm.setEntryAction (RTC.ERROR_STATE,
                               self.on_aborting)
      self._sm.setDoAction    (RTC.ERROR_STATE,
                               self.on_error)
      self._sm.setExitAction  (RTC.ERROR_STATE,
                               self.on_reset)
      st = OpenRTM_aist.StateHolder()
      st.prev = RTC.INACTIVE_STATE
      st.curr = RTC.INACTIVE_STATE
      st.next = RTC.INACTIVE_STATE
      self._sm.setStartState(st)
      self._sm.goTo(RTC.INACTIVE_STATE)


    
    def on_startup(self):
      return self._obj.on_startup(self.ec_id)


    
    def on_shutdown(self):
      return self._obj.on_shutdown(self.ec_id)


    
    def on_activated(self, st):
      if self._obj.on_activated(self.ec_id) != RTC.RTC_OK:
        self._sm.goTo(RTC.ERROR_STATE)
        return
      return


    
    def on_deactivated(self, st):
      self._obj.on_deactivated(self.ec_id)


    
    def on_aborting(self, st):
      self._obj.on_aborting(self.ec_id)


    
    def on_error(self, st):
      self._obj.on_error(self.ec_id)


    
    def on_reset(self, st):
      if self._obj.on_reset(self.ec_id) != RTC.RTC_OK:
        self._sm.goTo(RTC.ERROR_STATE)
        return
      return


    
    def on_execute(self, st):
      if self._obj.on_execute(self.ec_id) != RTC.RTC_OK:
        self._sm.goTo(RTC.ERROR_STATE)
        return
      return


    
    def on_state_update(self, st):
      if self._obj.on_state_update(self.ec_id) != RTC.RTC_OK:
        self._sm.goTo(RTC.ERROR_STATE)
        return
      return


    
    def on_rate_changed(self):
      self._obj.on_rate_changed(self.ec_id)


    
    def worker(self):
      return self._sm.worker()


    
    def get_state(self):
      return self._sm.getState()


  
  def __init__(self, owner=None, rate=None):
    self._rtcout = OpenRTM_aist.Manager.instance().getLogbuf("rtobject.periodic_ec")
    self._rtcout.RTC_TRACE("PeriodicExecutionContext()")

    OpenRTM_aist.Task.__init__(self)

    self._nowait = False
    self._running = False

    self._worker = self.Worker()

    global DEFAULT_PERIOD

    if rate is None:
      self._period = OpenRTM_aist.TimeValue(DEFAULT_PERIOD)
    else:
      if rate == 0:
        rate = 1.0 / DEFAULT_PERIOD
      self._period = OpenRTM_aist.TimeValue(1.0 / rate)

      if self._period.sec() == 0  and self._period.usec() < 0.000001:
        self._nowait = True

    self._rtcout.RTC_DEBUG("Actual rate: %d [sec], %d [usec]",
                           (self._period.sec(), self._period.usec()))    

    self._comps = []
    self._profile = RTC.ExecutionContextProfile(RTC.PERIODIC, (1.0/self._period.toDouble()), None, [], [])
    self._ref = self._this()
    self._mutex_del = threading.RLock()
    return


  def __del__(self, Task=OpenRTM_aist.Task):
    self._rtcout.RTC_TRACE("~PeriodicExecutionContext()")
    self._worker._cond.acquire()
    self._worker._running = True
    self._worker._cond.notify()
    self._worker._cond.release()
    self._running = False
    #self.wait()

    self._profile.owner = None
    self._profile.paarticipants = []
    self._profile.properties = []
    guard = OpenRTM_aist.ScopedLock(self._mutex_del)
    Task.__del__(self)
    del guard

  
  def getObjRef(self):
    return self._ref


  
  def svc(self):
    self._rtcout.RTC_TRACE("svc()")
    flag = True
    count_ = 0
    
    guard = OpenRTM_aist.ScopedLock(self._mutex_del)
    while flag:
      self._worker._cond.acquire()
      while not self._worker._running:
        self._worker._cond.wait()

      t0_ = OpenRTM_aist.Time()

      if self._worker._running:
        for comp in self._comps:
          comp._sm.worker()

      self._worker._cond.release()

      t1_ = OpenRTM_aist.Time()

      if count_ > 1000:
        exctm_ = (t1_ - t0_).getTime().toDouble()
        slptm_ = self._period.toDouble() - exctm_
        self._rtcout.RTC_PARANOID("Period:    %f [s]", self._period.toDouble())
        self._rtcout.RTC_PARANOID("Execution: %f [s]", exctm_)
        self._rtcout.RTC_PARANOID("Sleep:     %f [s]", slptm_)

      t2_ = OpenRTM_aist.Time()

      if not self._nowait and self._period.toDouble() > ((t1_ - t0_).getTime().toDouble()):
        if count_ > 1000:
          self._rtcout.RTC_PARANOID("sleeping...")
        slptm_ = self._period.toDouble() - (t1_ - t0_).getTime().toDouble()
        time.sleep(slptm_)

      if count_ > 1000:
        t3_ = OpenRTM_aist.Time()
        self._rtcout.RTC_PARANOID("Slept:     %f [s]", (t3_ - t2_).getTime().toDouble())
        count_ = 0
      count_ += 1
      flag = self._running
    del guard
    return 0


  
  def close(self, flags):
    self._rtcout.RTC_TRACE("close()")
    return 0


  
  def is_running(self):
    self._rtcout.RTC_TRACE("is_running()")
    return self._running


  
  def start(self):
    self._rtcout.RTC_TRACE("start()")
    if self._running:
      return RTC.PRECONDITION_NOT_MET

    for comp in self._comps:
      comp._sm.on_startup()

    self._running = True

    self._worker._cond.acquire()
    self._worker._running = True
    self._worker._cond.notify()
    self._worker._cond.release()

    try:
      self.activate()
    except:
      self._running = False

      self._worker._cond.acquire()
      self._worker._running = False
      self._worker._cond.notify()
      self._worker._cond.release()
      self._rtcout.RTC_ERROR(OpenRTM_aist.Logger.print_exception())

    return RTC.RTC_OK


  
  def stop(self):
    self._rtcout.RTC_TRACE("stop()")
    if not self._running:
      return RTC.PRECONDITION_NOT_MET

    self._running = False
    self._worker._cond.acquire()
    self._worker._running = False
    self._worker._cond.release()

    for comp in self._comps:
      comp._sm.on_shutdown()

    #self.wait()
    return RTC.RTC_OK


  
  def get_rate(self):
    self._rtcout.RTC_TRACE("get_rate()")
    return self._profile.rate


  
  def set_rate(self, rate):
    self._rtcout.RTC_TRACE("set_rate(%f)", rate)
    if rate > 0.0:
      self._profile.rate = rate
      self._period.set_time(1.0/rate)
      if self._period.toDouble() == 0.0:
        self._nowait = True

      for comp in self._comps:
        comp._sm.on_rate_changed()
      return RTC.RTC_OK
    return RTC.BAD_PARAMETER


  
  def activate_component(self, comp):
    self._rtcout.RTC_TRACE("activate_component()")
    for compIn in self._comps:
      if compIn._ref._is_equivalent(comp):
        if not compIn._sm._sm.isIn(RTC.INACTIVE_STATE):
          return RTC.PRECONDITION_NOT_MET
        compIn._sm._sm.goTo(RTC.ACTIVE_STATE)
        return RTC.RTC_OK

    return RTC.BAD_PARAMETER


  
  def deactivate_component(self, comp):
    self._rtcout.RTC_TRACE("deactivate_component()")
    for compIn in self._comps:
      if compIn._ref._is_equivalent(comp):
        if not compIn._sm._sm.isIn(RTC.ACTIVE_STATE):
          return RTC.PRECONDITION_NOT_MET
        compIn._sm._sm.goTo(RTC.INACTIVE_STATE)
        count_ = 0
        usec_per_sec_ = 1.0e6
        sleeptime_ = 10.0 * usec_per_sec_ / float(self.get_rate())
        self._rtcout.RTC_PARANOID("Sleep time is %f [us]", sleeptime_)
        while compIn._sm._sm.isIn(RTC.ACTIVE_STATE):
          self._rtcout.RTC_TRACE("Waiting to be the INACTIVE state %d %f", (count_, float(time.time())))
          time.sleep(sleeptime_/usec_per_sec_)
          if count_ > 1000:
            self._rtcout.RTC_ERROR("The component is not responding.")
            break
          count_ += 1
        if compIn._sm._sm.isIn(RTC.INACTIVE_STATE):
          self._rtcout.RTC_TRACE("The component has been properly deactivated.")
          return RTC.RTC_OK
        self._rtcout.RTC_ERROR("The component could not be deactivated.")
        return RTC.RTC_ERROR

    return RTC.BAD_PARAMETER


  
  def reset_component(self, comp):
    self._rtcout.RTC_TRACE("reset_component()")
    for compIn in self._comps:
      if compIn._ref._is_equivalent(comp):
        if not compIn._sm._sm.isIn(RTC.ERROR_STATE):
          return RTC.PRECONDITION_NOT_MET
        compIn._sm._sm.goTo(RTC.INACTIVE_STATE)
        return RTC.RTC_OK

    return RTC.BAD_PARAMETER


  
  def get_component_state(self, comp):
    self._rtcout.RTC_TRACE("get_component_state()")
    for compIn in self._comps:
      if compIn._ref._is_equivalent(comp):
        return compIn._sm._sm.getState()

    return RTC.CREATED_STATE


  
  def get_kind(self):
    self._rtcout.RTC_TRACE("get_kind()")
    return self._profile.kind


  
  def add_component(self, comp):
    self._rtcout.RTC_TRACE("add_component()")
    if CORBA.is_nil(comp):
      return RTC.BAD_PARAMETER
    try:
      dfp_  = comp._narrow(OpenRTM.DataFlowComponent)
      rtc_  = comp._narrow(RTC.RTObject)
      if CORBA.is_nil(dfp_) or CORBA.is_nil(rtc_):
        return RTC.BAD_PARAMETER

      id_   = dfp_.attach_context(self._ref)
      comp_ = self.Comp(ref=comp, dfp=dfp_, id=id_)
      self._comps.append(comp_)
      self._profile.participants.append(rtc_)
      return RTC.RTC_OK
    except CORBA.Exception:
      self._rtcout.RTC_ERROR(OpenRTM_aist.Logger.print_exception())
      return RTC.BAD_PARAMETER

    return RTC.RTC_OK


  def bindComponent(self, rtc):
    self._rtcout.RTC_TRACE("bindComponent()")
    if rtc is None:
      return RTC.BAD_PARAMETER

    comp_ = rtc.getObjRef()
    dfp_  = comp_._narrow(OpenRTM.DataFlowComponent)
    id_   = rtc.bindContext(self._ref)
    if id_ < 0 or id_ > OpenRTM_aist.ECOTHER_OFFSET:
      self._rtcout.RTC_ERROR("bindContext returns invalid id: %d", id_)
      return RTC.RTC_ERROR

    self._rtcout.RTC_DEBUG("bindContext returns id = %d", id_)
    # rtc is owner of this EC
    self._comps.append(self.Comp(ref=comp_, dfp=dfp_, id=id_))
    self._profile.owner = dfp_
    return RTC.RTC_OK


  
  def remove_component(self, comp):
    self._rtcout.RTC_TRACE("remove_component()")
    len_ = len(self._comps)
    for i in range(len_):
      idx = (len_ - 1) - i
      if self._comps[idx]._ref._is_equivalent(comp):
        self._comps[idx]._ref.detach_context(self._comps[idx]._sm.ec_id)
        del self._comps[idx]
        rtcomp = comp._narrow(RTC.RTObject)
        if CORBA.is_nil(rtcomp):
          self._rtcout.RTC_ERROR("Invalid object reference.")
          return RTC.RTC_ERROR
        OpenRTM_aist.CORBA_SeqUtil.erase_if(self._profile.participants,
                                            self.find_participant(rtcomp))
        return RTC.RTC_OK

    return RTC.BAD_PARAMETER


  
  def get_profile(self):
    self._rtcout.RTC_TRACE("get_profile()")
    return self._profile


  class find_participant:
    def __init__(self, comp):
      self._comp = comp
      return

    def __call__(self, comp):
      return self._comp._is_equivalent(comp)

      
  
  class Comp:
    def __init__(self, ref=None, dfp=None, id=None, comp=None):
      if comp is None:
        self._ref = ref
        self._sm = PeriodicExecutionContext.DFP(dfp,id)
      else:
        self._ref = comp._ref
        self._sm  = PeriodicExecutionContext.DFP(comp._sm._obj,comp._sm.ec_id)

  
  class Worker:
    
    
    def __init__(self):
      self._mutex = threading.RLock()
      self._cond = threading.Condition(self._mutex)
      self._running = False


def PeriodicExecutionContextInit(manager):
  manager.registerECFactory("PeriodicExecutionContext",
                            OpenRTM_aist.PeriodicExecutionContext,
                            OpenRTM_aist.ECDelete)

---

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