PortBase.py

Go to the documentation of this file.

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




import threading
import copy

import OpenRTM_aist
import RTC, RTC__POA




class PortBase(RTC__POA.PortService):
  """
  """



  
  def __init__(self, name=None):
    self._ownerInstanceName = "unknown"
    self._objref = self._this()

    self._profile = RTC.PortProfile("", [], RTC.PortService._nil, [], RTC.RTObject._nil,[])
    # Now Port name is <instance_name>.<port_name>. r1648
    if name is None:
      self._profile.name = "unknown.unknown"
    else:
      self._profile.name = self._ownerInstanceName+"."+name
      
    self._profile.port_ref = self._objref
    self._profile.owner = RTC.RTObject._nil
    self._profile_mutex = threading.RLock()
    self._connection_mutex = threading.RLock()
    self._rtcout = OpenRTM_aist.Manager.instance().getLogbuf(name)
    self._onPublishInterfaces = None
    self._onSubscribeInterfaces = None
    self._onConnected = None
    self._onUnsubscribeInterfaces = None
    self._onDisconnected = None
    self._onConnectionLost = None
    self._connectionLimit   = -1
    self._portconnListeners = None
    return

  
  
  def __del__(self):
    self._rtcout.RTC_TRACE("PortBase.__del__()")
    try:
      mgr = OpenRTM_aist.Manager.instance().getPOA()
      oid = mgr.servant_to_id(self)
      mgr.deactivate_object(oid)
    except:
      self._rtcout.RTC_ERROR(OpenRTM_aist.Logger.print_exception())
    

  
  def get_port_profile(self):
    self._rtcout.RTC_TRACE("get_port_profile()")

    self.updateConnectors()

    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)

    prof = RTC.PortProfile(self._profile.name,
                           self._profile.interfaces,
                           self._profile.port_ref,
                           self._profile.connector_profiles,
                           self._profile.owner,
                           self._profile.properties)

    return prof


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


  
  def get_connector_profiles(self):
    self._rtcout.RTC_TRACE("get_connector_profiles()")

    self.updateConnectors()

    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    return self._profile.connector_profiles


  
  def get_connector_profile(self, connector_id):
    self._rtcout.RTC_TRACE("get_connector_profile(%s)", connector_id)

    self.updateConnectors()

    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    index = OpenRTM_aist.CORBA_SeqUtil.find(self._profile.connector_profiles,
                                            self.find_conn_id(connector_id))
    if index < 0:
      conn_prof = RTC.ConnectorProfile("","",[],[])
      return conn_prof

    conn_prof = RTC.ConnectorProfile(self._profile.connector_profiles[index].name,
                                     self._profile.connector_profiles[index].connector_id,
                                     self._profile.connector_profiles[index].ports,
                                     self._profile.connector_profiles[index].properties)
    return conn_prof


  
  def connect(self, connector_profile):
    self._rtcout.RTC_TRACE("connect()")
    if self.isEmptyId(connector_profile):
      guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
      self.setUUID(connector_profile)
      assert(not self.isExistingConnId(connector_profile.connector_id))
      del guard
    else:
      guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
      if self.isExistingConnId(connector_profile.connector_id):
        self._rtcout.RTC_ERROR("Connection already exists.")
        return (RTC.PRECONDITION_NOT_MET,connector_profile)
      del guard

    try:
      retval,connector_profile = connector_profile.ports[0].notify_connect(connector_profile)
      if retval != RTC.RTC_OK:
        self._rtcout.RTC_ERROR("Connection failed. cleanup.")
        self.disconnect(connector_profile.connector_id)
    
      return (retval, connector_profile)
      #return connector_profile.ports[0].notify_connect(connector_profile)
    except:
      self._rtcout.RTC_ERROR(OpenRTM_aist.Logger.print_exception())
      return (RTC.BAD_PARAMETER, connector_profile)

    return (RTC.RTC_ERROR, connector_profile)


  
  def notify_connect(self, connector_profile):
    self._rtcout.RTC_TRACE("notify_connect()")

    guard_connection = OpenRTM_aist.ScopedLock(self._connection_mutex)

    # publish owned interface information to the ConnectorProfile
    retval = [RTC.RTC_OK for i in range(3)]

    self.onNotifyConnect(self.getName(),connector_profile)
    retval[0] = self.publishInterfaces(connector_profile)
    if retval[0] != RTC.RTC_OK:
      self._rtcout.RTC_ERROR("publishInterfaces() in notify_connect() failed.")

    self.onPublishInterfaces(self.getName(), connector_profile, retval[0])
    if self._onPublishInterfaces:
      self._onPublishInterfaces(connector_profile)

    # call notify_connect() of the next Port
    retval[1], connector_profile = self.connectNext(connector_profile)
    if retval[1] != RTC.RTC_OK:
      self._rtcout.RTC_ERROR("connectNext() in notify_connect() failed.")

    self.onConnectNextport(self.getName(), connector_profile, retval[1])
    # subscribe interface from the ConnectorProfile's information
    if self._onSubscribeInterfaces:
      self._onSubscribeInterfaces(connector_profile)

    retval[2] = self.subscribeInterfaces(connector_profile)
    if retval[2] != RTC.RTC_OK:
      self._rtcout.RTC_ERROR("subscribeInterfaces() in notify_connect() failed.")
      #self.notify_disconnect(connector_profile.connector_id)

    self.onSubscribeInterfaces(self.getName(), connector_profile, retval[2])

    self._rtcout.RTC_PARANOID("%d connectors are existing",
                              len(self._profile.connector_profiles))

    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    # update ConnectorProfile
    index = self.findConnProfileIndex(connector_profile.connector_id)
    if index < 0:
      OpenRTM_aist.CORBA_SeqUtil.push_back(self._profile.connector_profiles,
                                           connector_profile)
      self._rtcout.RTC_PARANOID("New connector_id. Push backed.")

    else:
      self._profile.connector_profiles[index] = connector_profile
      self._rtcout.RTC_PARANOID("Existing connector_id. Updated.")

    for ret in retval:
      if ret != RTC.RTC_OK:
        self.onConnected(self.getName(), connector_profile, ret)
        return (ret, connector_profile)

    # connection established without errors
    if self._onConnected:
      self._onConnected(connector_profile)
    self.onConnected(self.getName(), connector_profile, RTC.RTC_OK)
    return (RTC.RTC_OK, connector_profile)


  
  def disconnect(self, connector_id):
    self._rtcout.RTC_TRACE("disconnect(%s)", connector_id)

    index = self.findConnProfileIndex(connector_id)

    if index < 0:
      self._rtcout.RTC_ERROR("Invalid connector id: %s", connector_id)
      return RTC.BAD_PARAMETER

    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    if index < len(self._profile.connector_profiles):
      prof = self._profile.connector_profiles[index]
    del guard
    
    if len(prof.ports) < 1:
      self._rtcout.RTC_FATAL("ConnectorProfile has empty port list.")
      return RTC.PRECONDITION_NOT_MET

    for i in range(len(prof.ports)):
      p = prof.ports[i]
      try:
        return p.notify_disconnect(connector_id)
      except:
        self._rtcout.RTC_WARN(OpenRTM_aist.Logger.print_exception())
        continue

    self._rtcout.RTC_ERROR("notify_disconnect() for all ports failed.")
    return RTC.RTC_ERROR


  
  def notify_disconnect(self, connector_id):
    self._rtcout.RTC_TRACE("notify_disconnect(%s)", connector_id)

    guard_connection = OpenRTM_aist.ScopedLock(self._connection_mutex)
    # The Port of which the reference is stored in the beginning of
    # connectorProfile's PortServiceList is master Port.
    # The master Port has the responsibility of disconnecting all Ports.
    # The slave Ports have only responsibility of deleting its own
    # ConnectorProfile.

    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)

    index = self.findConnProfileIndex(connector_id)

    if index < 0:
      self._rtcout.RTC_ERROR("Invalid connector id: %s", connector_id)
      return RTC.BAD_PARAMETER

    prof = RTC.ConnectorProfile(self._profile.connector_profiles[index].name,
                                self._profile.connector_profiles[index].connector_id,
                                self._profile.connector_profiles[index].ports,
                                self._profile.connector_profiles[index].properties)
    self.onNotifyDisconnect(self.getName(), prof)

    retval = self.disconnectNext(prof)
    self.onDisconnectNextport(self.getName(), prof, retval)

    if self._onUnsubscribeInterfaces:
      self._onUnsubscribeInterfaces(prof)
    self.onUnsubscribeInterfaces(self.getName(), prof)
    self.unsubscribeInterfaces(prof)

    if self._onDisconnected:
      self._onDisconnected(prof)

    OpenRTM_aist.CORBA_SeqUtil.erase(self._profile.connector_profiles, index)
    
    self.onDisconnected(self.getName(), prof, retval)
    return retval


  
  def disconnect_all(self):
    self._rtcout.RTC_TRACE("disconnect_all()")
    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    plist = copy.deepcopy(self._profile.connector_profiles)
    del guard
    
    retcode = RTC.RTC_OK
    len_ = len(plist)
    self._rtcout.RTC_DEBUG("disconnecting %d connections.", len_)

    # disconnect all connections
    # Call disconnect() for each ConnectorProfile.
    for i in range(len_):
      tmpret = self.disconnect(plist[i].connector_id)
      if tmpret != RTC.RTC_OK:
        retcode = tmpret

    return retcode


  #============================================================
  # Local operations
  #============================================================

  
  def setName(self, name):
    self._rtcout.RTC_TRACE("setName(%s)", name)
    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    self._profile.name = name
    return

  
  def getName(self):
    self._rtcout.RTC_TRACE("getName() = %s", self._profile.name)
    return self._profile.name


  
  def getProfile(self):
    self._rtcout.RTC_TRACE("getProfile()")
    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    return self._profile


  
  def setPortRef(self, port_ref):
    self._rtcout.RTC_TRACE("setPortRef()")
    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    self._profile.port_ref = port_ref


  
  def getPortRef(self):
    self._rtcout.RTC_TRACE("getPortRef()")
    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    return self._profile.port_ref


  
  def setOwner(self, owner):
    prof = owner.get_component_profile()
    self._ownerInstanceName = prof.instance_name
    self._rtcout.RTC_TRACE("setOwner(%s)", self._ownerInstanceName)

    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    plist = self._profile.name.split(".")
    if not self._ownerInstanceName:
      self._rtcout.RTC_ERROR("Owner is not set.")
      self._rtcout.RTC_ERROR("addXXXPort() should be called in onInitialize().")
    portname = self._ownerInstanceName+"."+plist[-1]

    self._profile.owner = owner
    self._profile.name = portname


  #============================================================
  # callbacks
  #============================================================

  
  def setOnPublishInterfaces(self, on_publish):
    self._onPublishInterfaces = on_publish
    return


  
  def setOnSubscribeInterfaces(self, on_subscribe):
    self._onSubscribeInterfaces = on_subscribe
    return


  
  def setOnConnected(self, on_connected):
    self._onConnected = on_connected
    return


  
  def setOnUnsubscribeInterfaces(self, on_subscribe):
    self._onUnsubscribeInterfaces = on_subscribe
    return


  
  def setOnDisconnected(self, on_disconnected):
    self._onDisconnected = on_disconnected
    return

  # void setOnConnectionLost(ConnectionCallback* on_connection_lost);
  def setOnConnectionLost(self, on_connection_lost):
    self._onConnectionLost = on_connection_lost
    return


  
  def setPortConnectListenerHolder(self, portconnListeners):
    self._portconnListeners = portconnListeners
    return


  
  def publishInterfaces(self, connector_profile):
    pass


  
  def connectNext(self, connector_profile):
    index = OpenRTM_aist.CORBA_SeqUtil.find(connector_profile.ports,
                                            self.find_port_ref(self._profile.port_ref))
    if index < 0:
      return (RTC.BAD_PARAMETER, connector_profile)

    index += 1
    if index < len(connector_profile.ports):
      p = connector_profile.ports[index]
      return p.notify_connect(connector_profile)

    return (RTC.RTC_OK, connector_profile)


  
  def disconnectNext(self, connector_profile):
    index = OpenRTM_aist.CORBA_SeqUtil.find(connector_profile.ports,
                                            self.find_port_ref(self._profile.port_ref))
    if index < 0:
      return RTC.BAD_PARAMETER

    if index == (len(connector_profile.ports) - 1):
      return RTC.RTC_OK

    index += 1

    while index < len(connector_profile.ports):
      p = connector_profile.ports[index]
      index += 1
      try:
        return p.notify_disconnect(connector_profile.connector_id)
      except:
        self._rtcout.RTC_WARN(OpenRTM_aist.Logger.print_exception())
        continue

    return RTC.RTC_ERROR


  
  def subscribeInterfaces(self, connector_profile):
    pass


  
  def unsubscribeInterfaces(self, connector_profile):
    pass


  
  def setConnectionLimit(self, limit_value):
    self._connectionLimit = limit_value
    return
    

  
  def _publishInterfaces(self):
    if not (self._connectionLimit < 0) :
      if self._connectionLimit <= len(self._profile.connector_profiles):
        self._rtcout.RTC_PARANOID("Connected number has reached the limitation.")
        self._rtcout.RTC_PARANOID("Can connect the port up to %d ports.",
                                  self._connectionLimit)
        self._rtcout.RTC_PARANOID("%d connectors are existing",
                                  len(self._profile.connector_profiles))
        return RTC.RTC_ERROR

    return RTC.RTC_OK


  
  def isEmptyId(self, connector_profile):
    return connector_profile.connector_id == ""


  
  def getUUID(self):
    return str(OpenRTM_aist.uuid1())


  
  def setUUID(self, connector_profile):
    connector_profile.connector_id = self.getUUID()
    assert(connector_profile.connector_id != "")


  
  def isExistingConnId(self, id_):
    return OpenRTM_aist.CORBA_SeqUtil.find(self._profile.connector_profiles,
                                           self.find_conn_id(id_)) >= 0


  
  def findConnProfile(self, id_):
    index = OpenRTM_aist.CORBA_SeqUtil.find(self._profile.connector_profiles,
                                            self.find_conn_id(id_))
    if index < 0 or index >= len(self._profile.connector_profiles):
      return RTC.ConnectorProfile("","",[],[])

    return self._profile.connector_profiles[index]


  
  def findConnProfileIndex(self, id_):
    return OpenRTM_aist.CORBA_SeqUtil.find(self._profile.connector_profiles,
                                           self.find_conn_id(id_))


  
  def updateConnectorProfile(self, connector_profile):
    index = OpenRTM_aist.CORBA_SeqUtil.find(self._profile.connector_profiles,
                                            self.find_conn_id(connector_profile.connector_id))

    if index < 0:
      OpenRTM_aist.CORBA_SeqUtil.push_back(self._profile.connector_profiles,
                                           connector_profile)
    else:
      self._profile.connector_profiles[index] = connector_profile


  
  def eraseConnectorProfile(self, id_):
    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)

    index = OpenRTM_aist.CORBA_SeqUtil.find(self._profile.connector_profiles,
                                            self.find_conn_id(id_))

    if index < 0:
      return False

    OpenRTM_aist.CORBA_SeqUtil.erase(self._profile.connector_profiles, index)

    return True


  
  def appendInterface(self, instance_name, type_name, pol):
    index = OpenRTM_aist.CORBA_SeqUtil.find(self._profile.interfaces,
                                            self.find_interface(instance_name, pol))

    if index >= 0:
      return False

    # setup PortInterfaceProfile
    prof = RTC.PortInterfaceProfile(instance_name, type_name, pol)
    OpenRTM_aist.CORBA_SeqUtil.push_back(self._profile.interfaces, prof)

    return True


  
  def deleteInterface(self, name, pol):
    index = OpenRTM_aist.CORBA_SeqUtil.find(self._profile.interfaces,
                                            self.find_interface(name, pol))

    if index < 0:
      return False

    OpenRTM_aist.CORBA_SeqUtil.erase(self._profile.interfaces, index)
    return True


  
  def addProperty(self, key, value):
    OpenRTM_aist.CORBA_SeqUtil.push_back(self._profile.properties,
                                         OpenRTM_aist.NVUtil.newNV(key, value))

  
  def appendProperty(self, key, value):
    OpenRTM_aist.NVUtil.appendStringValue(self._profile.properties, key, value)



  
  def updateConnectors(self):
    guard = OpenRTM_aist.ScopedLock(self._profile_mutex)
    
    connector_ids = []
    clist = self._profile.connector_profiles

    for cprof in clist:
      if not self.checkPorts(cprof.ports):
        connector_ids.append(cprof.connector_id)
        self._rtcout.RTC_WARN("Dead connection: %s", cprof.connector_id)

    for cid in connector_ids:
      self.disconnect(cid)

    return


  
  def checkPorts(self, ports):
    for port in ports:
      try:
        if port._non_existent():
          self._rtcout.RTC_WARN("Dead Port reference detected.")
          return False
      except:
        self._rtcout.RTC_WARN(OpenRTM_aist.Logger.print_exception())
        return False

    return True


  #inline void onNotifyConnect(const char* portname,
  #                            RTC::ConnectorProfile& profile)
  def onNotifyConnect(self, portname, profile):
    if self._portconnListeners != None:
      type = OpenRTM_aist.PortConnectListenerType.ON_NOTIFY_CONNECT
      self._portconnListeners.portconnect_[type].notify(portname, profile)
    return


  #inline void onNotifyDisconnect(const char* portname,
  #                               RTC::ConnectorProfile& profile)
  def onNotifyDisconnect(self, portname, profile):
    if self._portconnListeners != None:
      type = OpenRTM_aist.PortConnectListenerType.ON_NOTIFY_DISCONNECT
      self._portconnListeners.portconnect_[type].notify(portname, profile)
    return


  #inline void onUnsubscribeInterfaces(const char* portname,
  #                                    RTC::ConnectorProfile& profile)
  def onUnsubscribeInterfaces(self, portname, profile):
    if self._portconnListeners != None:
      type = OpenRTM_aist.PortConnectListenerType.ON_UNSUBSCRIBE_INTERFACES
      self._portconnListeners.portconnect_[type].notify(portname, profile)
    return


  #inline void onPublishInterfaces(const char* portname,
  #                                RTC::ConnectorProfile& profile,
  #                                ReturnCode_t ret)
  def onPublishInterfaces(self, portname, profile, ret):
    if self._portconnListeners != None:
      type = OpenRTM_aist.PortConnectRetListenerType.ON_PUBLISH_INTERFACES
      self._portconnListeners.portconnret_[type].notify(portname, profile, ret)
    return


  #inline void onConnectNextport(const char* portname,
  #                              RTC::ConnectorProfile& profile,
  #                              ReturnCode_t ret)
  def onConnectNextport(self, portname, profile, ret):
    if self._portconnListeners != None:
      type = OpenRTM_aist.PortConnectRetListenerType.ON_CONNECT_NEXTPORT
      self._portconnListeners.portconnret_[type].notify(portname, profile, ret)
    return


  #inline void onSubscribeInterfaces(const char* portname,
  #                                  RTC::ConnectorProfile& profile,
  #                                  ReturnCode_t ret)
  def onSubscribeInterfaces(self, portname, profile, ret):
    if self._portconnListeners != None:
      type = OpenRTM_aist.PortConnectRetListenerType.ON_SUBSCRIBE_INTERFACES
      self._portconnListeners.portconnret_[type].notify(portname, profile, ret)
    return


  #inline void onConnected(const char* portname,
  #                        RTC::ConnectorProfile& profile,
  #                        ReturnCode_t ret)
  def onConnected(self, portname, profile, ret):
    if self._portconnListeners != None:
      type = OpenRTM_aist.PortConnectRetListenerType.ON_CONNECTED
      self._portconnListeners.portconnret_[type].notify(portname, profile, ret)
    return


  #inline void onDisconnectNextport(const char* portname,
  #                                 RTC::ConnectorProfile& profile,
  #                                 ReturnCode_t ret)
  def onDisconnectNextport(self, portname, profile, ret):
    if self._portconnListeners != None:
      type = OpenRTM_aist.PortConnectRetListenerType.ON_DISCONNECT_NEXT
      self._portconnListeners.portconnret_[type].notify(portname, profile, ret)
    return


  #inline void onDisconnected(const char* portname,
  #                           RTC::ConnectorProfile& profile,
  #                           ReturnCode_t ret)
  def onDisconnected(self, portname, profile, ret):
    if self._portconnListeners != None:
      type = OpenRTM_aist.PortConnectRetListenerType.ON_DISCONNECTED
      self._portconnListeners.portconnret_[type].notify(portname, profile, ret)
    return



  #============================================================
  # Functor
  #============================================================

  
  class if_name:
    def __init__(self, name):
      self._name = name

    def __call__(self, prof):
      return str(self._name) == str(prof.instance_name)
    

  
  class find_conn_id:
    def __init__(self, id_):
      """
       \param id_(string)
      """
      self._id = id_

    def __call__(self, cprof):
      """
       \param cprof(RTC.ConnectorProfile)
      """
      return str(self._id) == str(cprof.connector_id)

  
  class find_port_ref:
    def __init__(self, port_ref):
      """
       \param port_ref(RTC.PortService)
      """
      self._port_ref = port_ref

    def __call__(self, port_ref):
      """
       \param port_ref(RTC.PortService)
      """
      return self._port_ref._is_equivalent(port_ref)

  
  class connect_func:
    def __init__(self, p, prof):
      """
       \param p(RTC.PortService)
       \param prof(RTC.ConnectorProfile)
      """
      self._port_ref = p
      self._connector_profile = prof
      self.return_code = RTC.RTC_OK

    def __call__(self, p):
      """
       \param p(RTC.PortService)
      """
      if not self._port_ref._is_equivalent(p):
        retval = p.notify_connect(self._connector_profile)
        if retval != RTC.RTC_OK:
          self.return_code = retval

  
  class disconnect_func:
    def __init__(self, p, prof):
      """
       \param p(RTC.PortService)
       \param prof(RTC.ConnectorProfile)
      """
      self._port_ref = p
      self._connector_profile = prof
      self.return_code = RTC.RTC_OK
      
    def __call__(self, p):
      """
       \param p(RTC.PortService)
      """
      if not self._port_ref._is_equivalent(p):
        retval = p.disconnect(self._connector_profile.connector_id)
        if retval != RTC.RTC_OK:
          self.return_code = retval

  
  class disconnect_all_func:
    def __init__(self, p):
      """
       \param p(OpenRTM_aist.PortBase)
      """
      self.return_code = RTC.RTC_OK
      self._port = p

    def __call__(self, p):
      """
       \param p(RTC.ConnectorProfile)
      """
      retval = self._port.disconnect(p.connector_id)
      if retval != RTC.RTC_OK:
        self.return_code = retval

  
  class find_interface:
    def __init__(self, name, pol):
      """
       \param name(string)
       \param pol(RTC.PortInterfacePolarity)
      """
      self._name = name
      self._pol = pol

    def __call__(self, prof):
      """
       \param prof(RTC.PortInterfaceProfile)
      """
      name = prof.instance_name
      return (str(self._name) == str(name)) and (self._pol == prof.polarity)