def main(): orb = CORBA.ORB_init(sys.argv) poa = orb.resolve_initial_references("RootPOA") poa._get_the_POAManager().activate() naming = OpenRTM_aist.CorbaNaming(orb, "localhost") servant = ComponentObserver_i() oid = poa.servant_to_id(servant) provider = poa.id_to_reference(oid) rtc = naming.resolve("COCTestRTC0.rtc")._narrow(RTC.RTObject) config = rtc.get_configuration() properties = [ OpenRTM_aist.NVUtil.newNV("heartbeat.enable", "YES"), OpenRTM_aist.NVUtil.newNV("heartbeat.interval", "10"), OpenRTM_aist.NVUtil.newNV("observed_status", "ALL") ] id = OpenRTM_aist.toTypename(servant) sprof = SDOPackage.ServiceProfile("test_id", id, properties, provider) ret = config.add_service_profile(sprof) flag = True print("If you exit program, please input 'q'.") sys.stdin.readline() ret = config.remove_service_profile("test_id") print("test program end. ret : ", ret) return
def main(): # subscription type subs_type = "Flush" # initialization of ORB orb = CORBA.ORB_init(sys.argv) # get NamingService naming = OpenRTM_aist.CorbaNaming(orb, "localhost") sl = OpenRTM_aist.CorbaConsumer() tkm = OpenRTM_aist.CorbaConsumer() # find TkMotorComp0 component tkm.setObject(naming.resolve("TkMotorComp0.rtc")) # get ports inobj = tkm.getObject()._narrow(RTC.RTObject) pin = inobj.get_ports() pin[0].disconnect_all() # find SliderComp0 component sl.setObject(naming.resolve("SliderComp0.rtc")) # get ports outobj = sl.getObject()._narrow(RTC.RTObject) pout = outobj.get_ports() pout[0].disconnect_all() # connect ports conprof = RTC.ConnectorProfile("connector0", "", [pin[0],pout[0]], []) OpenRTM_aist.CORBA_SeqUtil.push_back(conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.interface_type", "corba_cdr")) OpenRTM_aist.CORBA_SeqUtil.push_back(conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.dataflow_type", "push")) OpenRTM_aist.CORBA_SeqUtil.push_back(conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.subscription_type", subs_type)) ret = pin[0].connect(conprof) # activate TkMotorComp0 eclistin = inobj.get_owned_contexts() eclistin[0].activate_component(inobj) # activate SliderComp0 eclistout = outobj.get_owned_contexts() eclistout[0].activate_component(outobj)
def main(): # initialization of ORB orb = CORBA.ORB_init(sys.argv) # get NamingService naming = OpenRTM_aist.CorbaNaming(orb, "localhost") consumer = OpenRTM_aist.CorbaConsumer() provider = OpenRTM_aist.CorbaConsumer() for _ in range(100): try: # find MyServiceConsumer0 component consumer.setObject(naming.resolve("MyServiceConsumer0.rtc")) # get ports consobj = consumer.getObject()._narrow(RTC.RTObject) pcons = consobj.get_ports() break except: time.sleep(0.1) pcons[0].disconnect_all() for _ in range(100): try: # find MyServiceProvider0 component provider.setObject(naming.resolve("MyServiceProvider0.rtc")) # get ports provobj = provider.getObject()._narrow(RTC.RTObject) prov = provobj.get_ports() break except: time.sleep(0.1) prov[0].disconnect_all() # connect ports conprof = RTC.ConnectorProfile("connector0", "", [pcons[0], prov[0]], []) ret = pcons[0].connect(conprof) # activate ConsoleIn0 eclistin = consobj.get_owned_contexts() eclistin[0].activate_component(consobj) # activate ConsoleOut0 eclistout = provobj.get_owned_contexts() eclistout[0].activate_component(provobj)
def test_port(self): inport = OpenRTM_aist.get_port_by_name(self.comp2, "TestComp20.in") outport = OpenRTM_aist.get_port_by_name(self.comp1, "TestComp10.out") self.manager.connectDataPorts(inport, [outport]) ans = OpenRTM_aist.already_connected(inport, outport) self.assertTrue(ans) service_required = OpenRTM_aist.get_port_by_name( self.comp2, "TestComp20.service") service_provided = OpenRTM_aist.get_port_by_name( self.comp1, "TestComp10.service") self.manager.connectServicePorts(service_required, [service_provided]) ans = OpenRTM_aist.already_connected(service_required, service_provided) self.assertTrue(ans) ports = self.manager.getPortsOnNameServers( "dataports.port_cxt/test.topic_cxt", "inport") name = ports[0].get_port_profile().name self.assertEqual(name, "TestComp20.topic_in") orb = self.manager.getORB() names = "localhost" cns = OpenRTM_aist.CorbaNaming(orb, names) bl = cns.listByKind("dataports.port_cxt/test.topic_cxt", "inport") name = bl[0].binding_name[0].id self.assertEqual(name, "TestComp20.topic_in") self._d_out = RTC.TimedOctetSeq(RTC.Time(0, 0), []) self._outOut = OpenRTM_aist.OutPort("out", self._d_out) prop = OpenRTM_aist.Properties() self._outOut.init(prop) naming = self.manager.getNaming() naming.bindPortObject("test.port", self._outOut) port = cns.resolveStr("test.port") self.assertTrue(port is not None) naming = OpenRTM_aist.NamingOnCorba(orb, "localhost") naming.bindPortObject("test2.port", self._outOut) port = cns.resolveStr("test2.port") self.assertTrue(port is not None)
def string_to_component(self, name): rtc_list = [] tmp = name.split("://") if len(tmp) > 1: if tmp[0] == "rtcname": #tag = tmp[0] url = tmp[1] r = url.split("/") if len(r) > 1: host = r[0] rtc_name = url[len(host) + 1:] try: cns = None if host == "*": cns = self._cosnaming else: orb = OpenRTM_aist.Manager.instance().getORB() cns = OpenRTM_aist.CorbaNaming(orb, host) names = rtc_name.split("/") if len(names) == 2 and names[0] == "*": root_cxt = cns.getRootContext() self.getComponentByName(root_cxt, names[1], rtc_list) return rtc_list else: rtc_name += ".rtc" obj = cns.resolveStr(rtc_name) if CORBA.is_nil(obj): return [] if obj._non_existent(): return [] rtc_list.append(obj) return rtc_list except BaseException: self._rtcout.RTC_ERROR( OpenRTM_aist.Logger.print_exception()) return [] return rtc_list
def __init__(self, orb, names): self._rtcout = OpenRTM_aist.Manager.instance().getLogbuf('manager.namingoncorba') self._cosnaming = OpenRTM_aist.CorbaNaming(orb,names) self._endpoint = "" self._replaceEndpoint = False
def main(): # initialization of ORB orb = CORBA.ORB_init(sys.argv) # get NamingService naming = OpenRTM_aist.CorbaNaming(orb, "localhost") conin = OpenRTM_aist.CorbaConsumer() conout = OpenRTM_aist.CorbaConsumer() # find ConsoleIn0 component conin.setObject(naming.resolve("ConsoleIn0.rtc")) # get ports inobj = conin.getObject()._narrow(RTC.RTObject) pin = inobj.get_ports() pin[0].disconnect_all() # find ConsoleOut0 component conout.setObject(naming.resolve("ConsoleOut0.rtc")) # get ports outobj = conout.getObject()._narrow(RTC.RTObject) pout = outobj.get_ports() pout[0].disconnect_all() # subscription type subs_type = "flush" period = "1.0" push_policy = "new" skip_count = "0" for arg in sys.argv[1:]: if arg == "--flush": subs_type = "flush" break elif arg == "--new": subs_type = "new" if len(sys.argv) > 2: push_policy = OpenRTM_aist.normalize([sys.argv[3]]) if push_policy == "skip": skip_count = sys.argv[5] break elif arg == "--periodic": subs_type = "periodic" period = sys.argv[2] if len(sys.argv) > 3: push_policy = OpenRTM_aist.normalize([sys.argv[4]]) if push_policy == "skip": skip_count = sys.argv[6] break # elif sbus_type == "periodic" and type(arg) == float: # period = srt(arg) # break else: usage() print("Subscription Type: ", subs_type) print("Period: ", period, " [Hz]") print("push policy: ", push_policy) print("skip count: ", skip_count) # connect ports conprof = RTC.ConnectorProfile("connector0", "", [pin[0], pout[0]], []) OpenRTM_aist.CORBA_SeqUtil.push_back( conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.interface_type", "corba_cdr")) OpenRTM_aist.CORBA_SeqUtil.push_back( conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.dataflow_type", "push")) OpenRTM_aist.CORBA_SeqUtil.push_back( conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.subscription_type", subs_type)) if subs_type == "periodic": OpenRTM_aist.CORBA_SeqUtil.push_back( conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.publisher.push_rate", period)) OpenRTM_aist.CORBA_SeqUtil.push_back( conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.publisher.push_policy", push_policy)) if push_policy == "skip": OpenRTM_aist.CORBA_SeqUtil.push_back( conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.publisher.skip_count", skip_count)) ret, conprof = pin[0].connect(conprof) # activate ConsoleIn0 eclistin = inobj.get_owned_contexts() eclistin[0].activate_component(inobj) # activate ConsoleOut0 eclistout = outobj.get_owned_contexts() eclistout[0].activate_component(outobj)
def main(): # subscription type subs_type = "Flush" # initialization of ORB manager = OpenRTM_aist.Manager.init(sys.argv + [ "-o", "manager.shutdown_auto:NO", "-o", "manager.shutdown_on_nortcs:NO" ]) manager.activateManager() manager.runManager(True) orb = CORBA.ORB_init(sys.argv) # get NamingService naming = OpenRTM_aist.CorbaNaming(orb, "localhost") conin = OpenRTM_aist.CorbaConsumer() conout = OpenRTM_aist.CorbaConsumer() ec0 = OpenRTM_aist.CorbaConsumer( interfaceType=OpenRTM.ExtTrigExecutionContextService) ec1 = OpenRTM_aist.CorbaConsumer( interfaceType=OpenRTM.ExtTrigExecutionContextService) # find ConsoleIn0 component conin.setObject(naming.resolve("ConsoleIn0.rtc")) # get ports inobj = conin.getObject()._narrow(RTC.RTObject) pin = inobj.get_ports() pin[0].disconnect_all() # activate ConsoleIn0 eclisti = inobj.get_owned_contexts() eclisti[0].activate_component(inobj) ec0.setObject(eclisti[0]) # find ConsoleOut0 component conout.setObject(naming.resolve("ConsoleOut0.rtc")) # get ports outobj = conout.getObject()._narrow(RTC.RTObject) pout = outobj.get_ports() pout[0].disconnect_all() # activate ConsoleOut0 eclisto = outobj.get_owned_contexts() eclisto[0].activate_component(outobj) ec1.setObject(eclisto[0]) # connect ports conprof = RTC.ConnectorProfile("connector0", "", [pin[0], pout[0]], []) OpenRTM_aist.CORBA_SeqUtil.push_back( conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.interface_type", "corba_cdr")) OpenRTM_aist.CORBA_SeqUtil.push_back( conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.dataflow_type", "push")) OpenRTM_aist.CORBA_SeqUtil.push_back( conprof.properties, OpenRTM_aist.NVUtil.newNV("dataport.subscription_type", subs_type)) ret = pin[0].connect(conprof) while 1: try: print("\n\n") print("0: tick ConsoleIn component") print("1: tick ConsoleOut component") print("2: tick both components") print("q: exit") print("cmd? >") cmd = str(sys.stdin.readline()) if cmd == "0\n": ec0._ptr().tick() elif cmd == "1\n": ec1._ptr().tick() elif cmd == "2\n": ec0._ptr().tick() ec1._ptr().tick() elif cmd == "q\n": print("exit") break except: print("Exception.") pass manager.shutdown()
aistsim.setTimeRangeMode(bp.TimeRangeMode.UNLIMITED) world.addChildItem(aistsim) cont = rtm.BodyRTCItem() cont.setName("TankControllerRTC") tank.addChildItem(cont) cont.setModuleName(top + "/lib/choreonoid-1.5/rtc/TankJoystickControllerRTC") joy = rtm.RTCItem() joy.setName("JoystickControllerRTC") world.addChildItem(joy) joy.setModuleName(top + "/lib/choreonoid-1.5/rtc/JoystickRTC") orb = corba.getORB() naming = OpenRTM_aist.CorbaNaming(orb, "localhost") controller = OpenRTM_aist.CorbaConsumer() robot = OpenRTM_aist.CorbaConsumer() joystick = OpenRTM_aist.CorbaConsumer() controller.setObject(naming.resolve("TankJoystickControllerRTC0.rtc")) inobj = controller.getObject()._narrow(RTC.RTObject) robot.setObject(naming.resolve("Tank.rtc")) outobj = robot.getObject()._narrow(RTC.RTObject) joystick.setObject(naming.resolve("JoystickRTC0.rtc")) joyobj = joystick.getObject()._narrow(RTC.RTObject) subs_type = "flush"
def __init__(self, orb): threading.Thread.__init__(self) if hasattr(sys, "frozen"): self._basedir = os.path.dirname(unicode(sys.executable, sys.getfilesystemencoding())) else: self._basedir = os.path.dirname(__file__) self._basedir = os.path.abspath(self._basedir) self._basedir = self._basedir.replace('\\', '/') if sys.platform == 'win32': bindir = glob.glob('/Program Files*/OpenHRPSDK')[0] self._datadir = os.path.join(bindir, 'share', 'OpenHRP-3.1', 'sample') else: self._datadir = '' # TODO: unix path detector self._datadir = os.path.abspath(self._datadir) self._datadir = self._datadir.replace('\\', '/') # define constants self.ROBOT_URL = "file://"+self._datadir+"/model/PA10/pa10.main.wrl" self.FLOOR_URL = "file://"+self._datadir+"/model/floor.wrl" self.BOX1_URL = "file://"+self._basedir+"/data/box.wrl" self.BOX2_URL = "file://"+self._basedir+"/data/box2.wrl" self.BOX3_URL = "file://"+self._basedir+"/data/box3.wrl" self.time = 0.0 self.dt = 0.005 self.prevjoint = None self.commands = [] # find CORBA name server self.ns = OpenRTM_aist.CorbaNaming(orb, 'localhost:2809') # find OpenHRP components from the name server simfactoryobj = self.ns.resolve([CosNaming.NameComponent("DynamicsSimulatorFactory","")]) self.simfactory = simfactoryobj._narrow(OpenHRP.DynamicsSimulatorFactory) modelloaderobj = self.ns.resolve([CosNaming.NameComponent("ModelLoader","")]) self.modelloader = modelloaderobj._narrow(OpenHRP.ModelLoader) viewerobj = self.ns.resolve([CosNaming.NameComponent("OnlineViewer","")]) self.viewer = viewerobj._narrow(OpenHRP.OnlineViewer) # load models from the file print self.ROBOT_URL self.robot = self.modelloader.loadBodyInfo(str(self.ROBOT_URL)) print self.FLOOR_URL self.floor = self.modelloader.loadBodyInfo(str(self.FLOOR_URL)) print self.BOX1_URL self.box1 = self.modelloader.loadBodyInfo(str(self.BOX1_URL)) print self.BOX2_URL self.box2 = self.modelloader.loadBodyInfo(str(self.BOX2_URL)) print self.BOX3_URL self.box3 = self.modelloader.loadBodyInfo(str(self.BOX3_URL)) # initialize real world dynamics simulator self.sim = self.simfactory.create() self.sim.registerCharacter("robot", self.robot) self.sim.registerCharacter("floor", self.floor) self.sim.registerCharacter("box1", self.box1) self.sim.registerCharacter("box2", self.box2) self.sim.registerCharacter("box3", self.box3) # initialize virtual world dynamics simulator self.planner = self.simfactory.create() self.planner.registerCharacter("robot", self.robot) # set simulation algorithms self.sim.init(self.dt, OpenHRP.DynamicsSimulator.RUNGE_KUTTA, OpenHRP.DynamicsSimulator.DISABLE_SENSOR) self.planner.init(self.dt, OpenHRP.DynamicsSimulator.RUNGE_KUTTA, OpenHRP.DynamicsSimulator.DISABLE_SENSOR) # set gravity self.sim.setGVector([0, 0, 9.8]) self.planner.setGVector([0, 0, 9.8]) # set initial joint angles self.sim.setCharacterAllJointModes("robot", OpenHRP.DynamicsSimulator.TORQUE_MODE) self.planner.setCharacterAllJointModes("robot", OpenHRP.DynamicsSimulator.HIGH_GAIN_MODE) for s in [self.sim, self.planner]: s.setCharacterLinkData("robot", "J2", OpenHRP.DynamicsSimulator.JOINT_VALUE, [math.pi/3]) s.setCharacterLinkData("robot", "J4", OpenHRP.DynamicsSimulator.JOINT_VALUE, [math.pi/3]) s.setCharacterLinkData("robot", "J6", OpenHRP.DynamicsSimulator.JOINT_VALUE, [math.pi/3]) s.setCharacterLinkData("robot", "HAND_L", OpenHRP.DynamicsSimulator.JOINT_VALUE, [-0.04]) s.setCharacterLinkData("robot", "HAND_R", OpenHRP.DynamicsSimulator.JOINT_VALUE, [0.04]) s.setCharacterLinkData("box1", "WAIST", OpenHRP.DynamicsSimulator.ABS_TRANSFORM, [0.82, 0.0, 0.135, 1,0,0,0,1,0,0,0,1]) s.setCharacterLinkData("box2", "WAIST", OpenHRP.DynamicsSimulator.ABS_TRANSFORM, [0.62, 0.1, 0.135, 1,0,0,0,1,0,0,0,1]) s.setCharacterLinkData("box3", "WAIST", OpenHRP.DynamicsSimulator.ABS_TRANSFORM, [0.82, 0.2, 0.135, 1,0,0,0,1,0,0,0,1]) s.calcWorldForwardKinematics() for o in ["box1", "box2", "box3"]: self.sim.registerCollisionCheckPair("floor", "", o, "", 0.5, 0.5, [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], 0.01) for p in ["HAND_L", "HAND_R"]: self.sim.registerCollisionCheckPair("robot", p, o, "", 0.5, 0.5, [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], 0.01) for o in ["HAND_L", "HAND_R", "J7"]: self.sim.registerCollisionCheckPair("floor", "", "robot", o, 0.5, 0.5, [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], 0.01) self.sim.registerCollisionCheckPair("robot", "HAND_L", "robot", "HAND_R", 0.5, 0.5, [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], 0.01) self.sim.initSimulation() self.planner.initSimulation() # init online viewer self.viewer.load("robot", str(self.ROBOT_URL)) self.viewer.load("floor", str(self.FLOOR_URL)) self.viewer.load("box1", str(self.BOX1_URL)) self.viewer.load("box2", str(self.BOX2_URL)) self.viewer.load("box3", str(self.BOX3_URL)) self.viewer.clearLog() self.prevtime = time.clock()
def main(): term = myfunc.get_terminal_path() if term == None: sys.exit(1) # OpenRTMの初期化 orb = CORBA.ORB_init(sys.argv) naming = OpenRTM_aist.CorbaNaming(orb, rtchost) # コンポーネントを起動する # 例) os.system("%s -e python ConsoleIn.py &" % term) for p in procs: if myfunc.is_running_rtc(naming, p[0]) == False: os.system("%s -e 'cd %s && %s' &" % (term, p[1], p[2])) # ちょっと待つ time.sleep(0.5) raw_input("[press enter] to check running status...") # コンポーネントの起動確認 for p in procs: if myfunc.is_running_rtc(naming, p[0]) == False: print "running? (%s)" % p[0] raw_input("[press enter] to connect components...") # 対戦RTC以外のコンポーネントをつなぐ connect_components(naming, connects[0]) # 最低限必要なもの connect_components(naming, connects[1]) # ロガー connect_components(naming, connects[2]) # ビューア time.sleep(0.5) activate_components(naming, activate_list[0]) print "connect - COMPLETE!!!" # アルゴリズムの切り替えループ algo = -1 while True: print "<コマンド一覧>" print "\t", "q: プログラムの終了" print "\t", "0-9: アルゴリズムの切り替え" print "\t", "s: 対戦開始" print "\t", "e: 対戦終了" print "---" print "<アルゴリズム一覧>" print "\t", "0 : 単純な打ち返し(AHAttack)" print "\t", "1 : 止め打ち(LSQ_NUM, AHstop)" print "\t", "2 : 学習済みアルゴリズム(カウンター打ち, Imitation_Counter)" print "\t", "3 : 学習済みアルゴリズム(カット打ち, Imitation_Cut)" cmd = raw_input("input command > ") if cmd == 'q': if algo != -1: deactivate_components(naming, activate_list[1]) deactivate_components(naming, atkactivate_list[algo]) time.sleep(0.5) disconnect_components(naming, atkcon[algo]) break elif cmd == 's': if algo != -1: activate_components(naming, atkactivate_list[algo]) activate_components(naming, activate_list[1]) elif cmd == 'e': if algo != -1: deactivate_components(naming, activate_list[1]) deactivate_components(naming, atkactivate_list[algo]) elif cmd.isdigit(): if (int(cmd) >= 0) and (int(cmd) <= 3): deactivate_components(naming, activate_list[1]) deactivate_components(naming, activate_list[0]) if algo != -1: deactivate_components(naming, atkactivate_list[algo]) time.sleep(0.3) disconnect_components(naming, atkcon[algo]) else: time.sleep(0.3) algo = int(cmd) connect_components(naming, connects[0]) # 最低限必要なもの connect_components(naming, connects[1]) # ロガー connect_components(naming, connects[2]) # ビューア connect_components(naming, atkcon[algo]) time.sleep(0.3) activate_components(naming, activate_list[0]) # 対戦RTC以外のコンポーネントの接続を解除する print "disconnect start!" deactivate_components(naming, activate_list[1]) deactivate_components(naming, activate_list[0]) time.sleep(0.5) disconnect_components(naming, connects[0]) # 最低限必要なもの disconnect_components(naming, connects[1]) # ロガー disconnect_components(naming, connects[2]) # ビューア print "disconnect - COMPLETE!!!" return