python/rtm.py

Go to the documentation of this file.

from omniORB import CORBA, any, cdrUnmarshal, cdrMarshal
import CosNaming

import OpenRTM_aist
import RTC, OpenRTM, SDOPackage, RTM
from OpenRTM import CdrData, OutPortCdr, InPortCdr
from RTC import *

import sys
import string, math, socket
import os
import time
import re


rootnc = None
nshost = None
nsport = None
mgrhost = None
mgrport = None


class RTcomponent:
    
    def __init__(self, ref):
        self.ref = ref
        self.owned_ecs = ref.get_owned_contexts()
        self.ec = self.owned_ecs[0]
        self.ports = {}
        ports = self.ref.get_ports()
        for p in ports:
            prof = p.get_port_profile()
            name = prof.name.split('.')[1]
            self.ports[name] = p

    
    def port(self, name):
        try:
            p = self.ports[unicode(name)]
        except KeyError:
            p = findPort(self.ref, name)
            self.ports[unicode(name)] = p
        return p

    
    def service(self, instance_name, type_name="", port_name=""):
        return findService(self, port_name, type_name, instance_name)

    
    def setConfiguration(self, nvlist):
        return setConfiguration(self.ref, nvlist)

    
    def setProperty(self, name, value):
        return self.setConfiguration([[name, value]])

    
    def getProperties(self):
        return getConfiguration(self.ref)

    
    def getProperty(self, name):
        cfg = self.ref.get_configuration()
        cfgsets = cfg.get_configuration_sets()
        if len(cfgsets) == 0:
            print("configuration set is not found")
            return None
        cfgset = cfgsets[0]
        for d in cfgset.configuration_data:
            if d.name == name:
                return any.from_any(d.value)
        return None

    
    def start(self, ec=None, timeout=3.0):
        if ec == None:
            ec = self.ec
        if ec != None:
            if self.isActive(ec):
                return True
            ret = ec.activate_component(self.ref)
            if  ret != RTC.RTC_OK:
                print ('[rtm.py] \033[31m   Failed to start %s(%s)\033[0m' % \
                       (self.name(), ret))
                return False
            tm = 0 
            while tm < timeout:
                if self.isActive(ec):
                    return True
                time.sleep(0.01)
                tm += 0.01
        print ('[rtm.py] \033[31m   Failed to start %s(timeout)\033[0m' % \
               self.name())
        return False

    
    def stop(self, ec=None, timeout=3.0):
        if ec == None:
            ec = self.ec
        if ec != None:
            if self.isInactive(ec):
                return True
            ret = ec.deactivate_component(self.ref)
            if  ret != RTC.RTC_OK:
                print ('[rtm.py] \033[31m   Failed to stop %s(%s)\033[0m' % \
                       (self.name(), ret))
                return False
            tm = 0
            while tm < timeout:
                if self.isInactive(ec):
                    return True
                time.sleep(0.01)
                tm += 0.01
        print ('[rtm.py] \033[31m   Failed to stop %s(timeout)\033[0m' % \
               self.name())
        return False

    
    def reset(self, ec=None, timeout=3.0):
        if self.getLifeCycleState(ec) != RTC.ERROR_STATE:
            return True
        if ec == None:
            ec = self.ec
        return ec.reset_component(self.ref) == RTC.RTC_OK
    
    def getLifeCycleState(self, ec=None):
        if ec == None:
            ec = self.ec
        if ec != None:
            return ec.get_component_state(self.ref)
        else:
            return None

    
    def isActive(self, ec=None):
        return RTC.ACTIVE_STATE == self.getLifeCycleState(ec)

    
    def isInactive(self, ec=None):
        return RTC.INACTIVE_STATE == self.getLifeCycleState(ec)

    
    def name(self):
        cprof = self.ref.get_component_profile()
        return cprof.instance_name


class RTCmanager:
    
    def __init__(self, ref):
        self.ref = ref
        uname = os.uname()[0]
        if uname == "Darwin":
            self.soext = ".dylib"
        else:
            self.soext = ".so"

    
    def load(self, basename, initfunc=""):
        path = basename + self.soext
        if initfunc == "":
            basename + "Init"
        try:
            self.ref.load_module(path, initfunc)
        except:
            print("failed to load", path)

    
    def create(self, module, name=None):
        if name != None:
            rtc = findRTC(name)
            if rtc != None:
                print('RTC named "' + name + '" already exists.')
                return rtc
        args = module
        if name != None:
            args += '?instance_name=' + name
        ref = self.ref.create_component(args)
        if ref == None:
            return None
        else:
            return RTcomponent(ref)

    
    def delete(self, name):
        # ref = self.ref.delete_component(name)
        ref = findRTC(name).ref.exit() # delte_component did not actually kill component, so use rtc.exit https://github.com/fkanehiro/hrpsys-base/pull/512#issuecomment-80430387
        if ref == RTC_OK:
            return True
        else:
            return False

    
    def get_factory_names(self):
        fs = []
        fps = self.ref.get_factory_profiles()
        for afp in fps:
            for p in afp.properties:
                if p.name == "implementation_id":
                    fs.append(any.from_any(p.value))
        return fs

    
    def get_components(self):
        cs = []
        crefs = self.ref.get_components()
        for cref in crefs:
            c = RTcomponent(cref)
            cs.append(c)
        return cs

    
    def restart(self):
        self.ref.shutdown()
        time.sleep(1)


def unbindObject(name, kind):
    nc = NameComponent(name, kind)
    path = [nc]
    rootnc.unbind(path)
    return None


def initCORBA():
    global rootnc, orb, nshost, nsport, mgrhost, mgrport

    # from omniorb's document
    # When CORBA::ORB_init() is called, the value for each configuration
    # parameter is searched for in the following order:
    #  Command line arguments
    #  Environment variables
    # so init_CORBA will follow this order
    # first check command line argument

    rtm_argv = [sys.argv[0]]    # extract rtm related args only
    for i in range(len(sys.argv)):
        if sys.argv[i] == '-o':
            rtm_argv.extend(['-o', sys.argv[i+1]])

    mc = OpenRTM_aist.ManagerConfig();
    mc.parseArgs(rtm_argv)

    if nshost != None: # these values can be set via other script like "import rtm; rtm.nshost=XXX"
        print("\033[34m[rtm.py] nshost already set as " + str(nshost) + "\033[0m")
    else:
        try:
            nshost = mc._argprop.getProperty("corba.nameservers").split(":")[0]
            if not nshost:
                raise
        except:
            nshost = socket.gethostname() # default
            print("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nshost) + " as nshost \033[0m")

    if nsport != None:
        print("\033[34m[rtm.py] nsport already set as " + str(nsport) + "\033[0m")
    else:
        try:
            nsport = int(mc._argprop.getProperty("corba.nameservers").split(":")[1])
            if not nsport:
                raise
        except:
            nsport = 15005  # default
            print("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nsport) + " as nsport \033[0m")

    if mgrhost != None: 
        print("\033[34m[rtm.py] mgrhost already set as " + str(mgrhost) + "\033[0m")
    else:
        mgrhost = nshost

    if mgrport != None: 
        print("\033[34m[rtm.py] mgrport already set as " + str(mgrport) + "\033[0m")
    else:
        try:
            mgrport = int(mc._argprop.getProperty("corba.master_manager").split(":")[1])
            if not mgrport:
                raise
        except:
            mgrport = 2810  # default
            print("\033[34m[rtm.py] Failed to parse corba.master_manager, use " + str(mgrport) + "\033[0m")

    print("\033[34m[rtm.py] configuration ORB with %s:%s\033[0m"%(nshost, nsport))
    print("\033[34m[rtm.py] configuration RTCManager with %s:%s\033[0m"%(mgrhost, mgrport))
    os.environ['ORBInitRef'] = 'NameService=corbaloc:iiop:%s:%s/NameService' % \
                               (nshost, nsport)

    try:
        orb = CORBA.ORB_init(sys.argv, CORBA.ORB_ID)
        nameserver = orb.resolve_initial_references("NameService")
        rootnc = nameserver._narrow(CosNaming.NamingContext)
    except omniORB.CORBA.ORB.InvalidName:
        _, e, _ = sys.exc_info()
        sys.exit('[ERROR] Invalide Name (hostname=%s).\n' % (nshost) +
                 'Make sure the hostname is correct.\n' + str(e))
    except omniORB.CORBA.TRANSIENT:
        try:
            nameserver = orb.string_to_object('corbaloc:iiop:%s:%s/NameService'%(nshost, nsport))
            rootnc = nameserver._narrow(CosNaming.NamingContext)
        except:
            _, e, _ = sys.exc_info()
            sys.exit('[ERROR] Connection Failed with the Nameserver (hostname=%s port=%s).\n' % (nshost, nsport) +
                     'Make sure the hostname is correct and the Nameserver is running.\n' + str(e))
    except Exception:
        _, e, _ = sys.exc_info()
        print(str(e))

    return None


def getRootNamingContext(corbaloc):
    props = System.getProperties()

    args = ["-ORBInitRef", corbaloc]
    orb = ORB.init(args, props)

    nameserver = orb.resolve_initial_references("NameService")
    return NamingContextHelper.narrow(nameserver)


def findObject(name, kind="", rnc=None):
    nc = CosNaming.NameComponent(name, kind)
    path = [nc]
    if not rnc:
        rnc = rootnc
        if not rnc:
            print("[ERROR] findObject(%r,kind=%r,rnc=%r) rootnc is not found" % (name, kind, rnc))
    return rnc.resolve(path)


def findRTCmanager(hostname=None, rnc=None):
    if not rootnc:
        print("[ERROR] findRTCmanager(hostname=%r,rnc=%r) rootnc is not defined, need to call initCORBA()" % \
              (hostname, rnc))
    if not hostname:
        hostname = nshost
        cxt = None
    if not hostname:
        hostname = socket.gethostname()
    try:
        socket.gethostbyaddr(hostname)
    except Exception:
        _, e, _ = sys.exc_info()
        sys.exit('[ERROR] %s\n' % (str(e)) + '[ERROR] Could not get hostname for %s\n' % (hostname) +
                 '[ERROR] Make sure that you set the target hostname and address in DNS or /etc/hosts in linux/unix.')


    def getManagerFromNS(hostname, mgr=None):
        try:
            obj = findObject("manager", "mgr", findObject(hostname, "host_cxt", rnc))
            mgr = RTCmanager(obj._narrow(RTM.Manager))
        except:
            mgr = None
        return mgr

    def getManagerDirectly(hostname, mgr=None):
        global orb, mgrport
        corbaloc = "corbaloc:iiop:" + hostname + ":" + str(mgrport) + "/manager"
        print("\033[34m[rtm.py] trying to findRTCManager on port" + str(mgrport) + "\033[0m")
        try:
            obj = orb.string_to_object(corbaloc)
            mgr = RTCmanager(obj._narrow(RTM.Manager))
        except:
            mgr = None
        return mgr

    try:
        import CORBA
    except:
        print('import CORBA failed in findRTCmanager and neglect it for old python environment.')
    # fqdn
    mgr = None
    hostnames = [hostname, hostname.split(".")[0],
                 socket.gethostbyaddr(hostname)[0],
                 socket.gethostbyaddr(hostname)[0].split(".")[0]]
    for h in hostnames:
        mgr = getManagerDirectly(h) or getManagerFromNS(h)
        if mgr:
            return mgr
    print("Manager not found")
    return None



def findRTC(name, rnc=None):
    try:
        obj = findObject(name, "rtc", rnc)
        try:
            rtc = RTcomponent(obj._narrow(RTC.RTObject))
        except TypeError:
            rtc = RTcomponent(obj._narrow(RTC.DataFlowComponent))
        cxts = rtc.ref.get_participating_contexts()
        if len(cxts) > 0:
            rtc.ec = cxts[0]
        return rtc
    except:
        return None


def findPort(rtc, name):
    ports = rtc.get_ports()
    cprof = rtc.get_component_profile()
    portname = cprof.instance_name + "." + name
    for p in ports:
        prof = p.get_port_profile()
        if prof.name == portname:
            return p
    return None


def serializeComponents(rtcs, stopEC=True):
    if len(rtcs) < 2:
        return
    ec = rtcs[0].ec
    for rtc in rtcs[1:]:
        try:
            if not ec._is_equivalent(rtc.ec):
                if stopEC:
                    rtc.ec.stop()
                if ec.add_component(rtc.ref) == RTC.RTC_OK:
                    rtc.ec = ec
                else:
                    print('error in add_component()')
            else:
                print(rtc.name() + 'is already serialized')
        except Exception:
            _, e, _ = sys.exc_info()
            print("[rtm.py] \033[31m   error in serialize %s of %s %s\033[0m" % (rtc.name(),  [[r, r.name()] for r in rtcs], str(e)))
            raise e


def isConnected(outP, inP):
    op = outP.get_port_profile()
    for con_prof in op.connector_profiles:
        ports = con_prof.ports
        if len(ports) == 2 and outP._is_equivalent(ports[0]) and \
           inP._is_equivalent(ports[1]):
            return True
    return False


def disconnectPorts(outP, inP):
    op = outP.get_port_profile()
    iname = inP.get_port_profile().name
    for con_prof in op.connector_profiles:
        ports = con_prof.ports
        if len(ports) == 2:
            pname = ports[1].get_port_profile().name
            if pname == iname:
                print('[rtm.py]    Disconnect %s - %s' %(op.name, iname))
                outP.disconnect(con_prof.connector_id)
                return True
    return False


def dataTypeOfPort(port):
    prof = port.get_port_profile()
    prop = prof.properties
    for p in prop:
        if p.name == "dataport.data_type":
            return any.from_any(p.value)
    return None


def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength=1, rate=1000, pushpolicy="new", interfaceType="corba_cdr"):
    if not isinstance(inPs, list):
        inPs = [inPs]
    if not outP:
        print('[rtm.py] \033[31m   Failed to connect %s to %s(%s)\033[0m' % \
              (outP, [inP.get_port_profile().name if inP else inP for inP in inPs], inPs))
        return
    for inP in inPs:
        if not inP:
            print('[rtm.py] \033[31m   Failed to connect %s to %s(%s)\033[0m' % \
                  (outP.get_port_profile().name, inP, inPs))
            continue
        if isConnected(outP, inP) == True:
            print('[rtm.py]      %s and %s are already connected' % \
                  (outP.get_port_profile().name, inP.get_port_profile().name))
            continue
        if dataTypeOfPort(outP) != dataTypeOfPort(inP):
            print('[rtm.py] \033[31m     %s and %s have different data types\033[0m' % \
                  (outP.get_port_profile().name, inP.get_port_profile().name))
            continue
        nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any(interfaceType))
        nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any(dataflow))
        nv3 = SDOPackage.NameValue("dataport.subscription_type", any.to_any(subscription))
        nv4 = SDOPackage.NameValue("dataport.buffer.length", any.to_any(str(bufferlength)))
        nv5 = SDOPackage.NameValue("dataport.publisher.push_rate", any.to_any(str(rate)))
        nv6 = SDOPackage.NameValue("dataport.publisher.push_policy", any.to_any(pushpolicy))
        nv7 = SDOPackage.NameValue("dataport.data_type", any.to_any(dataTypeOfPort(outP)))
        con_prof = RTC.ConnectorProfile("connector0", "", [outP, inP],
                                        [nv1, nv2, nv3, nv4, nv5, nv6, nv7])
        print('[rtm.py]    Connect ' + outP.get_port_profile().name + ' - ' + \
              inP.get_port_profile().name+' (dataflow_type='+dataflow+', subscription_type='+ subscription+', bufferlength='+str(bufferlength)+', push_rate='+str(rate)+', push_policy='+pushpolicy+')')
        ret, prof = inP.connect(con_prof)
        if ret != RTC.RTC_OK:
            print("failed to connect")
            continue
        # confirm connection
        if isConnected(outP, inP) == False:
            print("connet() returned RTC_OK, but not connected")


def data2cdr(data):
    return cdrMarshal(any.to_any(data).typecode(), data, True)


def classFromString(fullname):
    component_path = fullname.split('.')
    package_name = component_path[0]
    component_path = component_path[1:]
    attr = None
    while component_path:
        class_name = component_path[0]
        component_path = component_path[1:]
        if attr:
            attr = getattr(attr, class_name)
        else:
            __import__(str(package_name))
            attr = getattr(sys.modules[package_name], class_name)
    return attr


def cdr2data(cdr, classname):
    return cdrUnmarshal(any.to_any(classFromString(classname)).typecode(), cdr, True)



connector_list = []


def writeDataPort(port, data, tm=1.0, disconnect=True):
    global connector_list, orb
    
    connector_name = "writeDataPort"
    
    
    
    prof = None
    
    for p in connector_list:
        if p["port"]._is_equivalent(port):
            if port.get_connector_profile(p["prof"].connector_id).name == connector_name:
                prof = p["prof"]
            else:
                connector_list.remove(p)
                
            


    if prof is None:      
        nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any("corba_cdr"))
        nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any("Push"))
        nv3 = SDOPackage.NameValue("dataport.subscription_type", any.to_any("flush"))
        con_prof = RTC.ConnectorProfile(connector_name, "", [port], [nv1, nv2, nv3])
        
        ret, prof = port.connect(con_prof)
        
        if ret != RTC.RTC_OK:
            print("failed to connect")
            return None
        connector_list.append({"port":port,"prof":prof})
        
        
    for p in prof.properties:
        if p.name == 'dataport.corba_cdr.inport_ior':
            ior = any.from_any(p.value)
            obj = orb.string_to_object(ior)
            inport = obj._narrow(InPortCdr)
            cdr = data2cdr(data)
            if inport.put(cdr) != OpenRTM.PORT_OK:
                print("failed to put")
            if disconnect:
                time.sleep(tm)
                port.disconnect(prof.connector_id)
                for p in connector_list:
                    if prof.connector_id == p["prof"].connector_id:
                        connector_list.remove(p)
            else:
                return prof.connector_id
    return None




def readDataPort(port, timeout=1.0, disconnect=True):
    global connector_list, orb

    
    connector_name = "readDataPort"
    prof = None
    for p in connector_list:
        if p["port"]._is_equivalent(port):
            if port.get_connector_profile(p["prof"].connector_id).name == connector_name:
                prof = p["prof"]
            else:
                connector_list.remove(p)
                
            

    
    

    pprof = port.get_port_profile()
    for prop in pprof.properties:
        if prop.name == "dataport.data_type":
            classname = any.from_any(prop.value)
    
    if prof is None:     
        
        nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any("corba_cdr"))
        nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any("Pull"))
        nv3 = SDOPackage.NameValue("dataport.subscription_type", any.to_any("flush"))
        con_prof = RTC.ConnectorProfile(connector_name, "", [port], [nv1, nv2, nv3])
        
        ret, prof = port.connect(con_prof)
        
        if ret != RTC.RTC_OK:
            print("failed to connect")
            return None
        
        connector_list.append({"port":port,"prof":prof})
    
    for p in prof.properties:
        if p.name == 'dataport.corba_cdr.outport_ior':
            ior = any.from_any(p.value)
            obj = orb.string_to_object(ior)
            outport = obj._narrow(OutPortCdr)
            tm = 0
            while tm < timeout:
                try:
                    ret, data = outport.get()
                    if ret == OpenRTM.PORT_OK:
                        if disconnect:
                            port.disconnect(prof.connector_id)
                            for p in connector_list:
                                if prof.connector_id == p["prof"].connector_id:
                                    connector_list.remove(p)
                        
                        tokens = classname.split(':')
                        if len(tokens) == 3:  # for 1.1?
                            classname = tokens[1].replace('/', '.')
                        return cdr2data(data, classname)
                except:
                    pass
                time.sleep(0.1)
                tm = tm + 0.1
                

        
    return None


def deleteAllConnector():
    global connector_list
    for port in connector_list:
        port["port"].disconnect(port["prof"].connector_id)
    del connector_list[:]


def findService(rtc, port_name, type_name, instance_name):
    if port_name == "":
        prof = rtc.ref.get_component_profile()
        # print("RTC name:",prof.instance_name)
        port_prof = prof.port_profiles
    else:
        p = rtc.port(port_name)
        if p == None:
            print("can't find a port named" + port_name)
            return None
        else:
            port_prof = [p.get_port_profile()]
    port = None
    for pp in port_prof:
        # print("name:", pp.name)
        ifs = pp.interfaces
        for aif in ifs:
            #print("IF name:", aif.instance_name)
            #print("IF type:", aif.type_name)
            if aif.instance_name == instance_name and \
               (type_name == "" or aif.type_name == type_name) and \
               aif.polarity == PROVIDED:
                port = pp.port_ref
    if port == None:
        print("can't find a service named", instance_name)
        return None
    con_prof = RTC.ConnectorProfile("noname", "", [port], [])
    ret, con_prof = port.connect(con_prof)
    ior = any.from_any(con_prof.properties[0].value)
    return orb.string_to_object(ior)


def setConfiguration(rtc, nvlist):
    ret = True
    cfg = rtc.get_configuration()
    cfgsets = cfg.get_configuration_sets()
    if len(cfgsets) == 0:
        print("configuration set is not found")
        return
    cfgset = cfgsets[0]
    for nv in nvlist:
        name = nv[0]
        value = nv[1]
        found = False
        for d in cfgset.configuration_data:
            if d.name == name:
                d.value = any.to_any(value)
                cfg.set_configuration_set_values(cfgset)
                found = True
                break
        if not found:
            ret = False
    cfg.activate_configuration_set('default')
    return ret


def getConfiguration(rtc):
    cfg = rtc.get_configuration()
    cfgsets = cfg.get_configuration_sets()
    if len(cfgsets) == 0:
        print("configuration set is not found")
        return None
    ret = {}
    for nv in cfgsets[0].configuration_data:
        ret[nv.name] = any.from_any(nv.value)
    return ret
    


def narrow(ior, klass, package="OpenHRP"):
    return ior._narrow(getattr(sys.modules[package], klass))


def isJython():
    return sys.version.count("GCC") == 0


if __name__ == '__main__':
    initCORBA()