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 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')