def init():
global ms, rh, rh_svc, ep_svc, simulation_mode, qRefPort
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if rh:
rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
ep_svc = narrow(rh.ec, "ExecutionProfileService")
qRefPort = rh.port("qRef")
else:
rh = rtm.findRTC("PA10Controller(Robot)0")
qRefPort = rtm.findRTC("HGcontroller0").port("qIn")
simulation_mode = 1
simulation_mode = 0
ms = rtm.findRTCmanager()
print "creating components"
createComps()
print "connecting components"
connectComps()
print "activating components"
activateComps()
print "initialized successfully"
def rtc_init():
global ms, co, co_svc, root_link_name, root_link_offset
initCORBA()
ms = rtm.findRTCmanager(rtm.nshost)
while ms == None:
time.sleep(1)
ms = rtm.findRTCmanager(rtm.nshost)
rospy.loginfo("[collision_state.py] wait for RTCmanager : ", ms)
co = rtm.findRTC(rospy.get_param('~comp_name', 'co'))
if co == None:
rospy.logerr("Could not found CollisionDetector, exiting...")
exit(1)
co_svc = narrow(co.service("service0"), "CollisionDetectorService")
if modelfile:
#import CosNaming
obj = rtm.rootnc.resolve([CosNaming.NameComponent('ModelLoader', '')])
mdlldr = obj._narrow(ModelLoader_idl._0_OpenHRP__POA.ModelLoader)
rospy.loginfo(" bodyinfo URL = file://" + modelfile)
body_info = mdlldr.getBodyInfo("file://" + modelfile)
root_link_name = body_info._get_links()[0].name
root_link_offset = inverse_matrix(
concatenate_matrices(
translation_matrix(body_info._get_links()[0].translation),
rotation_matrix(body_info._get_links()[0].rotation[3],
body_info._get_links()[0].rotation[0:3])))
else:
root_link_name = "WAIST"
root_link_offset = identity_matrix()
rospy.loginfo("ssetup collision_state with " + root_link_name + " " +
root_link_offset)
def init():
global ms, rh, rh_svc, ep_svc, simulation_mode, qRefPort
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if rh:
rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
ep_svc = narrow(rh.ec, "ExecutionProfileService")
qRefPort = rh.port("qRef")
else:
rh = rtm.findRTC("PA10Controller(Robot)0")
qRefPort = rtm.findRTC("HGcontroller0").port("qIn")
simulation_mode = 1
simulation_mode = 0
ms = rtm.findRTCmanager()
print "creating components"
createComps()
print "connecting components"
connectComps()
print "activating components"
activateComps()
print "initialized successfully"
def rtc_init () :
global ms, rh, eps
initCORBA()
ms = rtm.findRTCmanager(rtm.nshost)
while ms == None :
time.sleep(1);
ms = rtm.findRTCmanager(rtm.nshost)
print "[hrpsys_profile.py] wait for RTCmanager : ",ms
def rtc_init():
global ms, rh, eps
initCORBA()
ms = rtm.findRTCmanager(rtm.nshost)
while ms == None:
time.sleep(1)
ms = rtm.findRTCmanager(rtm.nshost)
print "[hrpsys_profile.py] wait for RTCmanager : ", ms
def init(hostname=socket.gethostname()):
global ms, simulation_mode, kinematics_mode
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if rh is not None:
rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
servo = rh
ep_svc = narrow(rh.ec, "ExecutionProfileService")
simulation_mode = 0
else:
rh = rtm.findRTC("JVRC1Controller(Robot)0")
servo = rtm.findRTC("PDcontroller0")
simulation_mode = 1
if rh.port("baseTformIn"):
kinematics_mode = 1
else:
kinematics_mode = 0
print("creating components")
createComps()
print("connecting components")
connectComps()
print("activating components")
activateComps()
print("initialized successfully")
def createComps():
global ms, rh, rh_svc, sh, sh_svc, tk_svc, st, kf, log, log_svc, servo
global ep_svc, mc, mc_ctrl
rh = rtm.findRTC("pandaController(Robot)0")
servo = rtm.findRTC("PDcontroller0")
ms = rtm.findRTCmanager()
ms.load("KalmanFilter")
kf = ms.create("KalmanFilter")
ms.load("StateHolder")
sh = ms.create("StateHolder")
sh_svc = narrow(sh.service("service0"), "StateHolderService")
tk_svc = narrow(sh.service("service1"), "TimeKeeperService")
ms.load("DataLogger")
log = ms.create("DataLogger")
log_svc = narrow(log.service("service0"), "DataLoggerService")
if use_udp:
ms.load("MCUDPSensors")
mc = ms.create("MCUDPSensors")
ms.load("MCUDPControl")
mc_ctrl = ms.create("MCUDPControl")
else:
ms.load("MCControl")
mc = ms.create("MCControl")
def createComps():
global ms, rh, rh_svc, sh, sh_svc, tk_svc, st, kf, log, log_svc, servo
global ep_svc, mc, mc_svc
rh = rtm.findRTC("RobotHardware0")
if rh is not None:
rh_svc = narrow(rh.service("service0"), "RobotHardwareService")
servo = rh
ep_svc = narrow(rh.ec, "ExecutionProfileService")
else:
rh = rtm.findRTC("JVRC1Controller(Robot)0")
servo = rtm.findRTC("PDcontroller0")
ms = rtm.findRTCmanager()
ms.load("KalmanFilter")
kf = ms.create("KalmanFilter")
ms.load("StateHolder")
sh = ms.create("StateHolder")
sh_svc = narrow(sh.service("service0"), "StateHolderService")
tk_svc = narrow(sh.service("service1"), "TimeKeeperService")
ms.load("DataLogger")
log = ms.create("DataLogger")
log_svc = narrow(log.service("service0"), "DataLoggerService")
ms.load("MCControl")
mc = ms.create("MCControl")
def createComps(hostname=socket.gethostname()):
global ms, adm_svc, rh, rh_svc, seq, seq_svc, sh, sh_svc, servo, kpg, kpg_svc, st, kf, log, simulation_mode
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("RobotHardware0")
if rh != None:
simulation_mode = 0
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
servo = rh
adm = rtm.findRTC("SystemAdmin0")
if adm != None:
adm.start()
adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))
else:
simulation_mode = 1
rh = rtm.findRTC(bodyinfo.modelName+"Controller(Robot)0")
servo = rtm.findRTC("PDservo0")
seq = initRTC("SequencePlayer", "seq")
seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
sh = initRTC("StateHolder", "StateHolder0")
sh_svc = OpenHRP.StateHolderServiceHelper.narrow(sh.service("service0"))
kf = initRTC("KalmanFilter", "kf")
kpg = initRTC("WalkGenerator", "kpg")
kpg_svc = OpenHRP.WalkGeneratorServiceHelper.narrow(kpg.service("service0"))
st = initRTC("Stabilizer", "st")
st.setConfiguration(bodyinfo.hstConfig)
log = initRTC("DataLogger", "log")
return [rh, seq, kf, kpg, sh, st, log]
def total():
global vc, rg2, ri, je, jd, civ
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("RGB2Gray")
mgr.load("ResizeImage")
mgr.load("JpegEncoder")
mgr.load("JpegDecoder")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
r2g = mgr.create("RGB2Gray")
ri = mgr.create("ResizeImage")
je = mgr.create("JpegEncoder")
jd = mgr.create("JpegDecoder")
civ = mgr.create("CameraImageViewer")
ri.setProperty("scale", "0.5")
rtm.connectPorts(vc.port("CameraImage"), r2g.port("rgb"))
rtm.connectPorts(r2g.port("gray"), ri.port("original"))
rtm.connectPorts(ri.port("resized"), je.port("decoded"))
rtm.connectPorts(je.port("encoded"), jd.port("encoded"))
rtm.connectPorts(jd.port("decoded"), civ.port("imageIn"))
rtm.serializeComponents([vc, r2g, ri, je, jd, civ])
vc.start()
r2g.start()
ri.start()
je.start()
jd.start()
civ.start()
def total():
global vc, rg2, ri, je, jd, civ
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("RGB2Gray")
mgr.load("ResizeImage")
mgr.load("JpegEncoder")
mgr.load("JpegDecoder")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
r2g = mgr.create("RGB2Gray")
ri = mgr.create("ResizeImage")
je = mgr.create("JpegEncoder")
jd = mgr.create("JpegDecoder")
civ = mgr.create("CameraImageViewer")
ri.setProperty("scale", "0.5")
rtm.connectPorts(vc.port("CameraImage"), r2g.port("rgb"))
rtm.connectPorts(r2g.port("gray"), ri.port("original"))
rtm.connectPorts(ri.port("resized"), je.port("decoded"))
rtm.connectPorts(je.port("encoded"), jd.port("encoded"))
rtm.connectPorts(jd.port("decoded"), civ.port("imageIn"))
rtm.serializeComponents([vc, r2g, ri, je, jd, civ])
vc.start()
r2g.start()
ri.start()
je.start()
jd.start()
civ.start()
def waitForRTCManager(self, managerhost=nshost):
self.ms = None
while self.ms == None :
time.sleep(1);
if managerhost == "localhost":
managerhost = socket.gethostname()
self.ms = rtm.findRTCmanager(managerhost)
print self.configurator_name, "wait for RTCmanager : ", managerhost
def rtc_init () :
global ms, co, co_svc, root_link_name, root_link_offset
rtm.initCORBA()
ms = rtm.findRTCmanager(rtm.nshost)
while ms == None :
time.sleep(1);
ms = rtm.findRTCmanager(rtm.nshost)
rospy.loginfo("[collision_state.py] wait for RTCmanager : ",ms)
co = None
count = 0
while co == None and count < 10:
co_name = rospy.get_param('~comp_name', 'co')
co = rtm.findRTC(co_name)
if co and co.isActive():
break
if co and co.isActive() is False:
rospy.logwarn("CollisionDetector(%s) is not activated, waiting..." % co_name)
else:
rospy.logwarn("Could not found CollisionDetector(%s), waiting..." % co_name)
time.sleep(1)
count += 1
co = None
if co == None:
rospy.logerr("Could not found CollisionDetector, exiting...")
exit(0)
co_svc = rtm.narrow(co.service("service0"), "CollisionDetectorService")
if modelfile:
import CosNaming
obj = rtm.rootnc.resolve([CosNaming.NameComponent('ModelLoader', '')])
mdlldr = obj._narrow(OpenHRP.ModelLoader_idl._0_OpenHRP__POA.ModelLoader)
rospy.loginfo(" bodyinfo URL = file://"+modelfile)
body_info = mdlldr.getBodyInfo("file://"+modelfile)
root_link_name = body_info._get_links()[0].segments[0].name
root_link_offset = inverse_matrix(concatenate_matrices(translation_matrix(body_info._get_links()[0].translation),
rotation_matrix(body_info._get_links()[0].rotation[3],
body_info._get_links()[0].rotation[0:3])))
else:
root_link_name = "WAIST"
root_link_offset = identity_matrix()
rospy.loginfo("ssetup collision_state with " + root_link_name + " " + root_link_offset)
def waitForRTCManager(self, managerhost=nshost):
self.ms = None
while self.ms == None :
time.sleep(1);
if managerhost == "localhost":
managerhost = socket.gethostname()
self.ms = rtm.findRTCmanager(managerhost)
print self.configurator_name, "wait for RTCmanager : ", managerhost
def check_initCORBA(self, nshost, nsport=2809):
try:
ms = rh = None
rtm.nshost = nshost
rtm.nsport = nsport
rtm.initCORBA()
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
self.assertTrue(ms and rh)
except Exception as e:
print "{0}, RTCmanager={1}, RTC(RobotHardware0)={2}".format(str(e),ms,rh)
self.fail()
pass
def createComps(hostname=socket.gethostname()):
global ms, user, user_svc, log, rh, servo, joystick, kf, st, st_svc
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("HRP2A")
if ms==None:
print "no ms"
else:
user = initRTC("sony", "wpg")
joystick= initRTC("GamepadRTC","joystick")
kf= initRTC("KalmanFilter","kf")
st= initRTC("Stabilizer","st")
#log = initRTC("DataLogger", "log")
servo= rtm.findRTC("JojoPDservo0")
#user = rtm.findRTC("sony0")
#joystick= rtm.findRTC("GamepadRTC0")
#kf=rtm.findRTC("KalmanFilter0")
#st=rtm.findRTC("Stabilizer0")
if servo==None:
print "no servo component"
return
if rh==None:
print "no robot component"
return
if user==None:
print "no user component"
return
user_svc = OpenHRP.sonyServiceHelper.narrow(user.service("service0"))
if user_svc==None:
print "no svc"
st_svc = OpenHRP.StabilizerServiceHelper.narrow(st.service("service0"))
if st_svc==None:
print "no st svc"
if joystick==None:
print "no joystick component"
rtcs=[rh, joystick, kf, user, st]
#user.start()
#joystick.start()
#return [rh, user]
return rtcs
def createComps():
global rh, ms, tp, log, log_svc
rh = rtm.findRTC("SimpleFoot0")
ms = rtm.findRTCmanager()
ms.load("TactileInfoProcessor")
tp = ms.create("TactileInfoProcessor")
ms.load("DataLogger")
log = ms.create("DataLogger", "log")
log_svc = narrow(log.service("service0"), "DataLoggerService")
def init():
global ms
ms = rtm.findRTCmanager()
print "creating components"
createComps()
print "connecting components"
connectComps()
print "activating components"
activateComps()
print "initialized successfully"
def init():
global ms
ms = rtm.findRTCmanager()
print "creating components"
createComps()
print "connecting components"
connectComps()
print "activating components"
activateComps()
print "initialized successfully"
def capture():
global vc, civ, ccs
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
civ = mgr.create("CameraImageViewer")
ccs = rtm.narrow(vc.service("service0"), "CameraCaptureService", "Img")
rtm.connectPorts(vc.port("CameraImage"), civ.port("imageIn"))
rtm.serializeComponents([vc, civ])
vc.start()
civ.start()
def capture():
global vc, civ, ccs
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
civ = mgr.create("CameraImageViewer")
ccs = rtm.narrow(vc.service("service0"), "CameraCaptureService", "Img")
rtm.connectPorts(vc.port("CameraImage"), civ.port("imageIn"))
rtm.serializeComponents([vc, civ])
vc.start()
civ.start()
def createComps():
global rh, ms, ted, log, log_svc
rh = rtm.findRTC("SimpleFoot0")
ms = rtm.findRTCmanager()
ms.load("TactileInfoEdgeDetector")
ted = ms.create("TactileInfoEdgeDetector")
ted.setProperty("foot", "left")
ted.setProperty("Wdin", "100000")
ms.load("DataLogger")
log = ms.create("DataLogger", "log")
log_svc = narrow(log.service("service0"), "DataLoggerService")
def rgb2gray():
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("RGB2Gray")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
r2g = mgr.create("RGB2Gray")
civ = mgr.create("CameraImageViewer")
rtm.connectPorts(vc.port("CameraImage"), r2g.port("rgb"))
rtm.connectPorts(r2g.port("gray"), civ.port("imageIn"))
rtm.serializeComponents([vc, r2g, civ])
vc.start()
r2g.start()
civ.start()
def rgb2gray():
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("RGB2Gray")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
r2g = mgr.create("RGB2Gray")
civ = mgr.create("CameraImageViewer")
rtm.connectPorts(vc.port("CameraImage"), r2g.port("rgb"))
rtm.connectPorts(r2g.port("gray"), civ.port("imageIn"))
rtm.serializeComponents([vc, r2g, civ])
vc.start()
r2g.start()
civ.start()
def createComps(hostname=socket.gethostname()):
global ms, rh, rh_svc, seq, seq_svc, sh, sh_svc, servo
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("RobotHardware0")
rh_svc = None
if rh != None:
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
servo = rh
else:
rh = rtm.findRTC(bodyinfo.modelName+"Controller(Robot)0")
servo = rtm.findRTC("PDservo0")
seq = initRTC("SequencePlayer", "seq")
sh = initRTC("StateHolder", "StateHolder0")
seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
sh_svc = OpenHRP.StateHolderServiceHelper.narrow(sh.service("service0"))
return [rh, seq, sh]
def resize():
global vc, ri, civ
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("ResizeImage")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
ri = mgr.create("ResizeImage")
civ = mgr.create("CameraImageViewer")
ri.setProperty("scale", "0.5")
rtm.connectPorts(vc.port("CameraImage"), ri.port("original"))
rtm.connectPorts(ri.port("resized"), civ.port("imageIn"))
rtm.serializeComponents([vc, ri, civ])
vc.start()
ri.start()
civ.start()
def createComps(hostname=socket.gethostname()):
global ms, adm_svc, rh, rh_svc, seq, seq_svc, sh, sh_svc, servo, kpg, kpg_svc, st, kf, log, simulation_mode, sot
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("RobotHardware0")
if rh != None:
simulation_mode = 0
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(
rh.service("service0"))
servo = rh
adm = rtm.findRTC("SystemAdmin0")
if adm != None:
adm.start()
adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(
adm.service("service0"))
else:
simulation_mode = 1
rh = rtm.findRTC(bodyinfo.modelName + "Controller(Robot)0")
servo = rtm.findRTC("PDservo0")
seq = initRTC("SequencePlayer", "seq")
seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(
seq.service("service0"))
sh = initRTC("StateHolder", "StateHolder0")
sh_svc = OpenHRP.StateHolderServiceHelper.narrow(sh.service("service0"))
if useStabilizerComp:
kf = initRTC("KalmanFilter", "kf")
kpg = initRTC("WalkGenerator", "kpg")
kpg_svc = OpenHRP.WalkGeneratorServiceHelper.narrow(
kpg.service("service0"))
if useStabilizerComp:
st = initRTC("Stabilizer", "st")
st.setConfiguration(bodyinfo.hstConfig)
log = initRTC("DataLogger", "log")
print "Creating SoT"
sot = initRTC("RtcStackOfTasks", "sot")
print "Set configuration of SoT"
sot.setConfiguration(sotinfo.sotConfig)
if useStabilizerComp:
return [rh, seq, kf, kpg, sh, st, log, sot]
else:
return [rh, seq, kpg, sh, log, sot]
def resize():
global vc, ri, civ
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("ResizeImage")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
ri = mgr.create("ResizeImage")
civ = mgr.create("CameraImageViewer")
ri.setProperty("scale", "0.5")
rtm.connectPorts(vc.port("CameraImage"), ri.port("original"))
rtm.connectPorts(ri.port("resized"), civ.port("imageIn"))
rtm.serializeComponents([vc, ri, civ])
vc.start()
ri.start()
civ.start()
def check_initCORBA(self, nshost, nsport=2809):
try:
ms = rh = None
rtm.nshost = nshost
rtm.nsport = nsport
rtm.initCORBA()
count = 0
while ( not (ms and rh) ) and count < 10:
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if ms and rh :
break
time.sleep(1)
print >>sys.stderr, "wait for RTCmanager=%r, RTC(RobotHardware0)=%r"%(ms,rh)
count += 1
self.assertTrue(ms and rh)
except Exception as e:
self.fail("%r, nshost=%r, nsport=%r RTCmanager=%r, RTC(RobotHardware0)=%r"%(str(e),nshost,nsport,ms,rh))
pass
def createCompsConsole(nshC, rncC):
global msC, rhC, servoC, rtcListC
msC = rtm.findRTCmanager(nshC, rncC)
rhC = rtm.findRTC("JVRC-State", rncC)
servoC = rtm.findRTC("creekRobotState0", rncC)
if rhC == None:
rtcListC = rtcListR
servoC = initRTC("creekRobotState", "creekRobotState0", msC)
else:
rtcListC = [rhC]
#rtcListC = rtcListR
if servoC == None:
return
global camera, camera_svc
camera = initRTC("creekCameraViewer", "camera", msC)
if camera != None:
camera_svc = OpenHRP.creekCameraViewerServiceHelper.narrow(camera.service("service0"))
rtcListC.append(camera)
def createComps(hostname=socket.gethostname()):
global ms, rh, servo, rtcList
global rh_s, servo_s
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC(bodyinfo.modelName)
servo = rtm.findRTC("creekPdServo0")
rh_s = rtm.findRTC("JVRC-State")
servo_s = rtm.findRTC("creekRobotState0")
if rh==None or servo==None:
print "couldn't find ROBOT or SERVO"
return
rtcList = [rh]
if servo_s == None:
servo_s = initRTC("creekRobotState", "creekRobotState0")
global kf, seq, seq_svc, sh, camera, camera_svc
kf = initRTC("creekStateEstimator", "kf")
if kf != None:
rtcList.append(kf)
seq = initRTC("creekSequencePlayer", "seq")
if seq != None:
seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
rtcList.append(seq)
sh = initRTC("creekReferenceHolder", "holder")
if sh != None:
rtcList.append(sh)
camera = initRTC("creekCameraViewer", "camera")
if camera != None:
camera_svc = OpenHRP.creekCameraViewerServiceHelper.narrow(camera.service("service0"))
rtcList.append(camera)
return rtcList
def createCompsRobot(nshR, rncR):
global ms, rh, servo, rtcListR
ms = rtm.findRTCmanager(nshR, rncR)
rh = rtm.findRTC(bodyinfo.modelName, rncR)
servo = rtm.findRTC("creekPdServo0", rncR)
if rh==None or servo==None:
print "couldn't find ROBOT or SERVO"
return
rtcListR = [rh]
global kf, seq, seq_svc, sh, wpg, wpg_svc, st, st_svc, arm, arm_svc
kf = initRTC("creekStateEstimator", "kf", ms)
if kf != None:
rtcListR.append(kf)
seq = initRTC("creekSequencePlayer", "seq", ms)
if seq != None:
seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
rtcListR.append(seq)
wpg = initRTC("sony", "wpg", ms)
if wpg != None:
wpg_svc = OpenHRP.sonyServiceHelper.narrow(wpg.service("service0"))
rtcListR.append(wpg)
arm = initRTC("creekArmControlCartesian", "arm", ms)
if arm != None:
arm_svc = OpenHRP.creekArmControlCartesianServiceHelper.narrow(arm.service("service0"))
rtcListR.append(arm)
sh = initRTC("creekReferenceHolder", "holder", ms)
if sh != None:
rtcListR.append(sh)
st= initRTC("Stabilizer","st", ms)
if st != None:
st_svc = OpenHRP.StabilizerServiceHelper.narrow(st.service("service0"))
rtcListR.append(st)
def createComps(hostname=socket.gethostname()):
global ms, adm_svc, rh, rh_svc, seq, seq_svc, armR, armR_svc,\
armL, armL_svc, grsp, grsp_svc, sa, tk_svc, log, log_svc, gf, ca, use_ros
ms = rtm.findRTCmanager(hostname)
adm = rtm.findRTC("SystemAdmin0")
adm_svc = None
if adm != None:
adm.start()
adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))
rh = rtm.findRTC("RobotHardware0")
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
seq = initRTC("SequencePlayer", "seq")
seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
if dir(seq_svc).count("loadTrajectory") == 0:
seq_svc = SequencePlayerRosService(seq_svc, rh_svc)
armR = initRTC("ArmControl", "armR")
armR_svc = OpenHRP.ArmControlServiceHelper.narrow(armR.service("service0"))
armL = initRTC("ArmControl", "armL")
armL_svc = OpenHRP.ArmControlServiceHelper.narrow(armL.service("service0"))
grsp = initRTC("Grasper", "grsp")
grsp_svc = OpenHRP.GrasperServiceHelper.narrow(grsp.service("service0"))
sa = initRTC("StateArbitrator", "sa")
tk_svc = OpenHRP.TimeKeeperServiceHelper.narrow(sa.service("service1"))
log = initRTC("DataLogger", "log")
log_svc = OpenHRP.DataLoggerServiceHelper.narrow(log.service("service0"))
gf = initRTC("GazeFixer", "gf")
ca = initRTC("CollisionAvoider", "ca")
return [rh, seq, armR, armL, sa, grsp, gf, ca, log]
def check_initCORBA(self, nshost, nsport=2809):
try:
ms = rh = None
rtm.nshost = nshost
rtm.nsport = nsport
rtm.initCORBA()
count = 0
while (not (ms and rh)) and count < 10:
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
if ms and rh:
break
time.sleep(1)
print >> sys.stderr, "wait for RTCmanager=%r, RTC(RobotHardware0)=%r" % (
ms, rh)
count += 1
self.assertTrue(ms and rh)
except Exception as e:
self.fail(
"%r, nshost=%r, nsport=%r RTCmanager=%r, RTC(RobotHardware0)=%r"
% (str(e), nshost, nsport, ms, rh))
pass
def init(hostname=socket.gethostname()):
global ms, simulation_mode, kinematics_mode
ms = rtm.findRTCmanager()
rh = rtm.findRTC("pandaController(Robot)0")
servo = rtm.findRTC("PDcontroller0")
simulation_mode = 1
if rh.port("baseTformIn"):
kinematics_mode = 1
else:
kinematics_mode = 0
print("creating components")
createComps()
print("connecting components")
connectComps()
print("activating components")
activateComps()
print("initialized successfully")
def jpeg():
global vc, je, jd, civ
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("JpegEncoder")
mgr.load("JpegDecoder")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
je = mgr.create("JpegEncoder")
jd = mgr.create("JpegDecoder")
civ = mgr.create("CameraImageViewer")
rtm.connectPorts(vc.port("CameraImage"), je.port("decoded"))
rtm.connectPorts(je.port("encoded"), jd.port("encoded"))
rtm.connectPorts(jd.port("decoded"), civ.port("imageIn"))
rtm.serializeComponents([vc, je, jd, civ])
vc.start()
je.start()
jd.start()
civ.start()
time.sleep(3)
print "jpeg quality 95 -> 30"
je.setProperty("quality", "30")
def createComps(hostname=socket.gethostname()):
global ms, adm_svc, rh, rh_svc, servo, seq, seq_svc, kf, sh, simulation_mode
ms = rtm.findRTCmanager(hostname)
rh = rtm.findRTC("RobotHardware0")
if rh != None:
simulation_mode = 0
rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
servo = rh
adm = rtm.findRTC("SystemAdmin0")
if adm != None:
adm.start()
adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))
else:
simulation_mode = 1
rh = rtm.findRTC("JVRC")
servo = rtm.findRTC("creekPdServo0")
seq = initRTC("creekSequencePlayer", "seq")
seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
kf = initRTC("creekStateEstimator", "kf")
sh = initRTC("creekReferenceHolder", "holder")
return [rh, seq, kf, sh]
def createCompsConsole(nshC, rncC):
global msC, rhC, servoC, servoC_svc, rtcListC
msC = rtm.findRTCmanager(nshC, rncC)
rhC = rtm.findRTC("JVRC-State", rncC)
servoC = rtm.findRTC("creekRobotState0", rncC)
if rhC == None:
rtcListC = rtcListR
servoC = initRTC("creekRobotState", "creekRobotState0", msC)
rtcListC.append(servoC)
else:
rtcListC = [rhC]
if servoC == None:
return
servoC_svc = OpenHRP.creekRobotStateServiceHelper.narrow(servoC.service("service0"))
global camera, camera_svc, stick, pcl, pcl_svc
camera = None
stick = None
pcl = None
camera = initRTC("creekCameraViewer", "camera", msC)
if camera != None:
camera_svc = OpenHRP.creekCameraViewerServiceHelper.narrow(camera.service("service0"))
rtcListC.append(camera)
stick = initRTC("GamepadRTC", "stick", msC)
if stick != None:
rtcListC.append(stick)
pcl = initRTC("creekPointCloudViewer", "pcl", msC)
if pcl != None:
pcl_svc = OpenHRP.creekPointCloudViewerServiceHelper.narrow(pcl.service("service0"))
rtcListC.append(pcl)
def createCompsRobot(nshR, rncR):
global ms, rh, servo, rtcListR
ms = rtm.findRTCmanager(nshR, rncR)
rh = rtm.findRTC(bodyinfo.modelName, rncR)
servo = rtm.findRTC("creekPdServo0", rncR)
if rh==None or servo==None:
print "couldn't find ROBOT or SERVO"
return
rtcListR = [rh]
global kf, seq, seq_svc, sh
kf = initRTC("creekStateEstimator", "kf", ms)
if kf != None:
rtcListR.append(kf)
seq = initRTC("creekSequencePlayer", "seq", ms)
if seq != None:
seq_svc = OpenHRP.creekSequencePlayerServiceHelper.narrow(seq.service("service0"))
rtcListR.append(seq)
sh = initRTC("creekReferenceHolder", "holder", ms)
if sh != None:
rtcListR.append(sh)
def jpeg():
global vc, je, jd, civ
mgr = rtm.findRTCmanager()
mgr.load("VideoCapture")
mgr.load("JpegEncoder")
mgr.load("JpegDecoder")
mgr.load("CameraImageViewer")
vc = mgr.create("VideoCapture")
je = mgr.create("JpegEncoder")
jd = mgr.create("JpegDecoder")
civ = mgr.create("CameraImageViewer")
rtm.connectPorts(vc.port("CameraImage"), je.port("decoded"))
rtm.connectPorts(je.port("encoded"), jd.port("encoded"))
rtm.connectPorts(jd.port("decoded"), civ.port("imageIn"))
rtm.serializeComponents([vc, je, jd, civ])
vc.start()
je.start()
jd.start()
civ.start()
time.sleep(3)
print "jpeg quality 95 -> 30"
je.setProperty("quality", "30")
import sys, os, datetime, rtm
execfile("rtm-utils.py")
try:
rtm.initCORBA()
setupRTM() # Set NameServer to this host.
viewer = findRTC("ImageViewer0") # Find for existing viewer.
cpanel = findRTC("ControlPanel0") # Find for existing control panel.
vision_pc = setupNameserver() # Set NameServer to control PC.
robohw = findRTC("RobotHardware0") # Find for existing robot hardware.
mgr = rtm.findRTCmanager(vision_pc) # Find RTC manager on vision PC.
if mgr == None:
raise Exception("Failed to find manager on {}.".format(vision_pc))
camera = createRTC(mgr, "V4L2CameraRTC", "v4l2")
syncer = createRTC(mgr, "VideoSynchronizerRTC", "sync")
syncer.setProperty("verbose", "1")
rtm.connectPorts(camera.port("TimedCameraImage"), syncer.port("primary"))
rtm.connectPorts(robohw.port("q"), syncer.port("secondary"))
rtm.connectPorts(cpanel.port("Command"), camera.port("Command"))
rtm.connectPorts(syncer.port("primaryOut"), viewer.port("images"))
cpanel.start()
viewer.start()
robohw.start()
import rtm
import commands
mgr = rtm.findRTCmanager()
mgr.load("VirtualCamera")
mgr.load("OccupancyGridMap3D")
mgr.load("OGMap3DViewer")
vc = mgr.create("VirtualCamera")
ogmap = mgr.create("OccupancyGridMap3D")
viewer = mgr.create("OGMap3DViewer")
openhrp_dir = commands.getoutput("pkg-config --variable=prefix openhrp3.1")
project = "file://" + openhrp_dir + "/share/OpenHRP-3.1/sample/project/SampleRobot_inHouse.xml"
vc.setProperty("project", project)
vc.setProperty("camera", "Robot:VISION_SENSOR1")
vc.setProperty("generateRange", "0")
vc.setProperty("generatePointCloud", "1")
vc.setProperty("generatePointCloudStep", "5")
ogmap.setProperty("resolution", "0.05")
rtm.connectPorts(vc.port("cloud"), ogmap.port("cloud"))
rtm.connectPorts(vc.port("poseSensor"), ogmap.port("pose"))
rtm.connectPorts(ogmap.port("OGMap3DService"), viewer.port("OGMap3DService"))
rtm.serializeComponents([vc, ogmap])
vc.start()
ogmap.start()
viewer.start()
