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
