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 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 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 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 test_rh_service(self):
try:
rh = rh_svc = None
rtm.nshost = 'localhost'
rtm.nsport = 2809
rtm.initCORBA()
rh = rtm.findRTC("RobotHardware0")
rh_svc = rtm.narrow(rh.service("service0"), "RobotHardwareService")
print "RTC(RobotHardware0)={0}, {1}".format(rh,rh_svc)
self.assertTrue(rh and rh_svc)
rh.start()
self.assertTrue(rh.isActive())
self.assertTrue(rh_svc.getStatus())
except Exception as e:
print "{0}, RTC(RobotHardware0)={1}, {2}".format(str(e),rh,rh_svc)
self.fail()
pass
def test_rh_service(self):
try:
rh = rh_svc = None
rtm.nshost = 'localhost'
rtm.nsport = 2809
rtm.initCORBA()
rh = rtm.findRTC("RobotHardware0")
rh_svc = rtm.narrow(rh.service("service0"), "RobotHardwareService")
print "RTC(RobotHardware0)={0}, {1}".format(rh, rh_svc)
self.assertTrue(rh and rh_svc)
rh.start()
self.assertTrue(rh.isActive())
self.assertTrue(rh_svc.getStatus())
except Exception as e:
print "{0}, RTC(RobotHardware0)={1}, {2}".format(
str(e), rh, rh_svc)
self.fail()
pass
mgr2 = mgr
else:
mgr2obj = rtm.orb.string_to_object('corbaloc:iiop:%s:2810/manager' % (nshost2))
mgr2 = rtm.RTCmanager(mgr2obj._narrow(RTM.Manager))
mgr2.load('SequencePlayer')
mgr2.load('StateHolder')
mgr2.load('ForwardKinematics')
mgr2.load('Joystick')
mgr2.load('Joystick2PanTiltAngles')
mgr2.load('MidJaxonController')
midjaxon = rtm.findRTC('MIDJAXON', nc2)
rh = rtm.findRTC('PDcontrollerMIDJAXON0', nc2)
seq = mgr2.create('SequencePlayer', 'seq')
seqsvc = rtm.narrow(seq.service('service0'), 'SequencePlayerService', 'hrpsys.OpenHRP')
sh = mgr2.create('StateHolder', 'sh')
shsvc = rtm.narrow(sh.service('service0'), 'StateHolderService', 'hrpsys.OpenHRP')
fk = mgr2.create('ForwardKinematics', 'fk')
rtm.rootnc.rebind([CosNaming.NameComponent('MIDJAXON', 'rtc')], midjaxon.ref)
rtm.rootnc.rebind([CosNaming.NameComponent('PDcontrollerMIDJAXON0', 'rtc')], rh.ref)
rtm.rootnc.rebind([CosNaming.NameComponent('seq', 'rtc')], seq.ref)
rtm.rootnc.rebind([CosNaming.NameComponent('sh', 'rtc')], sh.ref)
rtm.rootnc.rebind([CosNaming.NameComponent('fk', 'rtc')], fk.ref)
js = mgr.create('Joystick', 'js')
js.setProperty('device', '/dev/input/js0')
#js.setProperty('debugLevel', '1')
midc = mgr2.create('MidJaxonController', 'midc')
#midc.setProperty('debugLevel', '1')
