jython/rtm.py

Go to the documentation of this file.

from org.omg.CORBA import *
from org.omg.CosNaming import *
from org.omg.CosNaming.NamingContextPackage import *
from com.sun.corba.se.impl.encoding import EncapsOutputStream

from java.lang import System, Class

from RTC import *
from RTM import *
from OpenRTM import *
from _SDOPackage import *

import string, math, socket, time, sys


rootnc = None


nshost = 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):
                setConfiguration(self.ref, nvlist)

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

        
        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 d.value.extract_string()
                return None             

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

        
        def start(self, ec=None):
                if ec == None:
                        ec = self.ec
                if ec != None:
                        ec.activate_component(self.ref)
                        while self.isInactive(ec):
                                time.sleep(0.01)

        
        def stop(self, ec=None):
                if ec == None:
                        ec = self.ec
                if ec != None:
                        ec.deactivate_component(self.ref)
                        while self.isActive(ec):
                                time.sleep(0.01)

        
        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 LifeCycleState.ACTIVE_STATE == self.getLifeCycleState(ec)

        
        def isInactive(self, ec=None):
                return LifeCycleState.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
                osname = System.getProperty("os.name")
                # assuming jython and rtcd are running on the same OS
                if osname == "Mac OS X":
                        self.soext = ".dylib"
                else: 
                        self.soext = ".so"
        
        
        def load(self, basename):
                path = basename+self.soext
                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 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(p.value.extract_string())
                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, nshost, orb
        props = System.getProperties()
        
        args = string.split(System.getProperty("NS_OPT"))
        nshost = System.getProperty("NS_OPT").split(':')[2]
        if nshost == "localhost" or nshost == "127.0.0.1":
                nshost = socket.gethostname()
        print 'nshost =',nshost
        orb = ORB.init(args, props)

        nameserver = orb.resolve_initial_references("NameService");
        rootnc = NamingContextHelper.narrow(nameserver);
        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 = NameComponent(name,kind)
        path = [nc]
        if not rnc:
                rnc = rootnc
        return rnc.resolve(path)


def findRTCmanager(hostname=None, rnc=None):
        if not hostname:
                hostname = nshost
        try:
                try:
                        cxt = findObject(hostname, "host_cxt", rnc)
                except:
                        hostname = socket.gethostbyaddr(hostname)[0]
                        cxt = findObject(hostname, "host_cxt", rnc)
                obj = findObject("manager","mgr",cxt)
                return RTCmanager(ManagerHelper.narrow(obj))
        except:
                print "exception in findRTCmanager("+hostname+")"


def findRTC(name, rnc=None):
        try:
                obj = findObject(name, "rtc", rnc)
                rtc = RTcomponent(RTObjectHelper.narrow(obj))
                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:]:
                if ec != rtc.ec: 
                        if stopEC:
                                rtc.ec.stop()
                        if ec.add_component(rtc.ref) == ReturnCode_t.RTC_OK:
                                rtc.ec = ec
                        else:
                                print 'error in add_component()'
                else:
                        print 'already serialized'


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 == ports[0] and inP == ports[1]:
                        return True
        return False


def disconnectPorts(outP, inP):
        op = outP.get_port_profile()
        for con_prof in op.connector_profiles:
                ports = con_prof.ports
                if len(ports) == 2 and outP == ports[0] and inP == ports[1]:
                        outP.disconnect(con_prof.connector_id)
        return


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


def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength=1, rate=1000):
        if not isinstance(inPs, list):
                inPs = [inPs]
        for inP in inPs: 
                if isConnected(outP, inP) == True:
                        print outP.get_port_profile().name,'and',inP.get_port_profile().name,'are already connected'
                        continue
                if dataTypeOfPort(outP) != dataTypeOfPort(inP):
                        print outP.get_port_profile().name,'and',inP.get_port_profile().name,'have different data types'
                        continue
                        
                con_prof = ConnectorProfile()
                con_prof.connector_id = ""
                con_prof.name = "connector0"
                con_prof.ports = [outP, inP]
                #
                nv1 = NameValue()
                nv1.name = "dataport.interface_type";
                a1 = orb.create_any()
                a1.insert_string("corba_cdr")
                nv1.value = a1
                #
                nv2 = NameValue()
                nv2.name = "dataport.dataflow_type"
                a2 = orb.create_any()
                a2.insert_string(dataflow)
                nv2.value = a2
                #
                nv3 = NameValue()
                nv3.name = "dataport.subscription_type"
                a3 = orb.create_any()
                a3.insert_string(subscription)
                nv3.value = a3
                #
                nv4 = NameValue()
                nv4.name = "dataport.buffer.length"
                a4 = orb.create_any()
                a4.insert_string(str(bufferlength))
                nv4.value = a4
                #
                nv5 = NameValue()
                nv5.name = "dataport.publisher.push_rate"
                a5 = orb.create_any()
                a5.insert_string(str(rate))
                nv5.value = a5
                #
                con_prof.properties = [nv1, nv2, nv3, nv4, nv5]
                con_prof_holder = ConnectorProfileHolder()
                con_prof_holder.value = con_prof
                if inP.connect(con_prof_holder) != ReturnCode_t.RTC_OK:
                        print "failed to connect(",outP.get_port_profile().name,'<->',inP.get_port_profile().name,")"
                        continue
                # confirm connection
                if isConnected(outP, inP) == False:
                        print "connet() returned RTC_OK, but not connected"


def data2cdr(data):
        holder = Class.forName(data.getClass().getCanonicalName()+"Holder",
                               True, data.getClass().getClassLoader())
        streamable = holder.newInstance()
        #field = holder.getField("value")
        #field.set(streamable, data)
        streamable.value = data;
        strm = EncapsOutputStream(orb, True)
        streamable._write(strm)
        return strm.toByteArray()


def cdr2data(cdr, classname):
        ostrm = EncapsOutputStream(orb, True)
        ostrm.write_octet_array(cdr, 0, len(cdr))
        istrm = ostrm.create_input_stream()
        holder = Class.forName("RTC."+classname+"Holder", True, Manager.getClassLoader())
        streamable = holder.newInstance()
        streamable._read(istrm)
        return streamable.value


def writeDataPort(port, data, tm=1.0):
        con_prof = ConnectorProfile()
        con_prof.connector_id = ""
        con_prof.name = "connector0"
        con_prof.ports = [port]
        #
        nv1 = NameValue()
        nv1.name = "dataport.interface_type";
        a1 = orb.create_any()
        a1.insert_string("corba_cdr")
        nv1.value = a1
        #
        nv2 = NameValue()
        nv2.name = "dataport.dataflow_type"
        a2 = orb.create_any()
        a2.insert_string("Push")
        nv2.value = a2
        #
        nv3 = NameValue()
        nv3.name = "dataport.subscription_type"
        a3 = orb.create_any()
        a3.insert_string("flush")
        nv3.value = a3
        #
        con_prof.properties = [nv1, nv2, nv3]
        con_prof_holder = ConnectorProfileHolder()
        con_prof_holder.value = con_prof
        if port.connect(con_prof_holder) != ReturnCode_t.RTC_OK:
                print "failed to connect"
                return None
        for p in con_prof_holder.value.properties:
                if p.name == 'dataport.corba_cdr.inport_ior':
                        ior = p.value.extract_string()
                        obj = orb.string_to_object(ior)
                        inport = InPortCdrHelper.narrow(obj)
                        cdr = data2cdr(data)
                        if inport.put(cdr) != PortStatus.PORT_OK:
                                print "failed to put"
                        time.sleep(tm)
                        port.disconnect(con_prof_holder.value.connector_id)
        return None             
                        
                

def readDataPort(port, timeout = 1.0):
        pprof = port.get_port_profile()
        for prop in pprof.properties:
                if prop.name == "dataport.data_type":
                        classname = prop.value.extract_string()
                        break;
        con_prof = ConnectorProfile()
        con_prof.connector_id = ""
        con_prof.name = "connector0"
        con_prof.ports = [port]
        #
        nv1 = NameValue()
        nv1.name = "dataport.interface_type";
        a1 = orb.create_any()
        a1.insert_string("corba_cdr")
        nv1.value = a1
        #
        nv2 = NameValue()
        nv2.name = "dataport.dataflow_type"
        a2 = orb.create_any()
        a2.insert_string("Pull")
        nv2.value = a2
        #
        nv3 = NameValue()
        nv3.name = "dataport.subscription_type"
        a3 = orb.create_any()
        a3.insert_string("flush")
        nv3.value = a3
        #
        con_prof.properties = [nv1, nv2, nv3]
        con_prof_holder = ConnectorProfileHolder()
        con_prof_holder.value = con_prof
        if port.connect(con_prof_holder) != ReturnCode_t.RTC_OK:
                print "failed to connect"
                return None
        for p in con_prof_holder.value.properties:
                #print p.name
                if p.name == 'dataport.corba_cdr.outport_ior':
                        ior = p.value.extract_string()
                        obj = orb.string_to_object(ior)
                        outport = OutPortCdrHelper.narrow(obj)
                        cdr = CdrDataHolder()
                        tm = 0
                        while tm < timeout:
                                try:
                                        outport.get(cdr)
                                        if len(cdr.value) > 0:
                                                port.disconnect(con_prof_holder.value.connector_id)
                                                return cdr2data(cdr.value, classname)
                                except:
                                        pass
                                time.sleep(0.1)
                                tm = tm + 0.1

        port.disconnect(con_prof_holder.value.connector_id)
        return None             
                        
                


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):
                                port = pp.port_ref
        if port == None:
                print "can't find a service named",instance_name
                return None
        #print port
        con_prof = ConnectorProfile()   
        con_prof.name = "noname"
        con_prof.connector_id = ""
        con_prof.ports = [port]
        con_prof.properties = []
        con_prof_holder = ConnectorProfileHolder()
        con_prof_holder.value = con_prof
        port.connect(con_prof_holder)
        ior = con_prof_holder.value.properties[0].value.extract_string()
        port.disconnect(con_prof_holder.value.connector_id)
        return orb.string_to_object(ior)


def setConfiguration(rtc, nvlist):
        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.insert_string(value)
                                cfg.set_configuration_set_values(cfgset)
                                found = True
                                break;
                if not found:
                        print "no such property(",name,")"
        cfg.activate_configuration_set('default')


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


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

initCORBA()