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 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 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 init(robotHost=None):
if robotHost != None:
print 'robot host = '+robotHost
java.lang.System.setProperty('NS_OPT',
'-ORBInitRef NameService=corbaloc:iiop:'+robotHost+':2809/NameService')
rtm.initCORBA()
print "creating components"
rtcList = createComps(robotHost)
print "connecting components"
connectComps()
print "activating components"
activateComps(rtcList)
print "initialized successfully"
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
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(robotHost=None):
if robotHost != None:
print "robot host = " + robotHost
java.lang.System.setProperty(
"NS_OPT", "-ORBInitRef NameService=corbaloc:iiop:" + robotHost + ":2809/NameService"
)
rtm.initCORBA()
print "creating components"
rtcList = createComps(robotHost)
print "connecting components"
connectComps()
print "activating components"
activateComps(rtcList)
print "initialize bodyinfo"
bodyinfo.init(rtm.rootnc)
print "initialized successfully"
def init(robotHost=None):
if robotHost != None:
print 'robot host = ' + robotHost
java.lang.System.setProperty(
'NS_OPT', '-ORBInitRef NameService=corbaloc:iiop:' + robotHost +
':2809/NameService')
rtm.initCORBA()
print "creating components"
rtcList = createComps(robotHost)
print "connecting components"
connectComps()
print "activating components"
activateComps(rtcList)
print "initialize bodyinfo"
bodyinfo.init(rtm.rootnc)
print "initialized successfully"
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(host='localhost', refSuffix=""):
global vs, vs_svc, cvp, cvp_svc
if robotHost != None:
print 'robot host = '+robotHost
java.lang.System.setProperty('NS_OPT',
'-ORBInitRef NameService=corbaloc:iiop:'+robotHost+':2809/NameService')
rtm.initCORBA()
if refSuffix == None:
refSuffix = ''
elif refSuffix != '':
refSuffix = '_'+refSuffix
print 'suffix = '+refSuffix
vs = rtm.findRTC("VideoStream0"+refSuffix)
vs_svc = Img.CameraCaptureServiceHelper.narrow(vs.service('service0'))
vs.start()
cvp = rtm.findRTC("CvProcessor0"+refSuffix)
cvp_svc = OpenHRP.CvProcessorServiceHelper.narrow(cvp.service('service0'))
cvp.start()
rtm.connectPorts(vs.port("MultiCameraImages"), cvp.port("MultiCameraImage"))
def setNameserver(nshost=None, nsport=2809):
if nshost == None:
rtm.nshost = socket.gethostname()
else:
rtm.nshost = nshost
rtm.nsport = nsport
nsloc = "corbaloc:iiop:%s:%d/NameService" % (rtm.nshost, rtm.nsport)
rtm.rootnc = rtm.orb.string_to_object(nsloc)._narrow(
CosNaming.NamingContext)
try:
import cnoid.Corba
rtm.orb = cnoid.Corba.getORB()
except:
rtm.initCORBA()
try:
setNameserver()
camera = rtm.findRTC("IIDCCamera0")
if camera == None:
raise Exception("Failed to find IIDCCamera0")
viewer = rtm.findRTC("ImageViewer0")
if viewer == None:
raise Exception("Failed to find ImageViewer0")
cpanel = rtm.findRTC("ControlPanel0")
if cpanel == None:
raise Exception("Failed to find ControlPanel0")
connectPorts(cpanel.port("Command"), camera.port("Command"))
import rtm
rtm.nshost="localhost"
rtm.nsport=2809
rtm.initCORBA()
mgr = rtm.findRTCmanager()
mgr.load("CameraImageLoader")
loader = mgr.create("CameraImageLoader")
mgr.load("ColorExtractor")
extractor = mgr.create("ColorExtractor")
extractor.setProperty("minPixels","10")
extractor.setProperty("rgbRegion","100,0,0,255,100,100")
mgr.load("CameraImageViewer")
viewer = mgr.create("CameraImageViewer")
#rtm.serializeComponents([loader, extractor, viewer])
rtm.connectPorts(loader.port("image"), extractor.port("original"))
rtm.connectPorts(extractor.port("result"), viewer.port("imageIn"))
viewer.start()
extractor.start()
loader.start()
