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