예제 #1
0
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)
예제 #2
0
 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
예제 #3
0
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)
예제 #4
0
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"
예제 #5
0
    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
예제 #6
0
    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
예제 #7
0
 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
예제 #8
0
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"
예제 #9
0
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"
예제 #10
0
 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
예제 #11
0
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"))
예제 #12
0
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"))
예제 #13
0
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()