00001 """ 00002 Mainly copied from "rtmros_common/hrpsys_ros_bridge/scripts/hrpsys-bridge-connect.py", due to the connection failue of rtshell and realtime loop problem 00003 00004 """ 00005 import os,time 00006 import rtm 00007 00008 from rtm import * 00009 from OpenHRP import * 00010 00011 def myinitCORBA(nshost): 00012 global rootnc 00013 args = string.split('-ORBInitRef NameService=corbaloc:iiop:'+ nshost +':2809/NameService') 00014 props = System.getProperties() 00015 orb = ORB.init(args, props) 00016 00017 nameserver = orb.resolve_initial_references("NameService") 00018 rootnc = NamingContextHelper.narrow(nameserver) 00019 00020 00021 if __name__ == '__main__': 00022 myinitCORBA(os.getenv("RTCTREE_NAMESERVERS","localhost")) 00023 00024 time.sleep(10) 00025 00026 #who wants to parse from launch file 00027 conn = [['Odometry0', 'ResultLocalization', 00028 'MobileRobotROSBridge0', 'in'], 00029 ['BeegoController0', 'TargetVelocity', 00030 'MobileRobotROSBridge0', 'out']] 00031 00032 for node1, port1, node2, port2 in conn: 00033 print [node1,port1,node2,port2] 00034 connectPorts(findRTC(node1,rootnc).port(port1), 00035 findRTC(node2,rootnc).port(port2), subscription='flush') 00036 00037 beego = findRTC("MobileRobotROSBridge0") 00038 beego.start()
