Example #1
0
def openExtraChannel():

    status = 0
    ic = None
    Extra2Tx = ExtraI()
    try:
        ic = EasyIce.initialize(sys.argv)
        adapter = ic.createObjectAdapterWithEndpoints("ExtraAdapter", "default -p 9002")
        object = Extra2Tx
        adapter.add(object, ic.stringToIdentity("Extra"))
        adapter.activate()
        ic.waitForShutdown()
    except:
        traceback.print_exc()
        status = 1

    if ic:
        # Clean up
        try:
            ic.destroy()
        except:
            traceback.print_exc()
            status = 1

    sys.exit(status)
Example #2
0
def openCMDVelChannel(CMDVel):
    status = 0
    ic = None
    CMDVel2Rx = CMDVel #CMDVel.getCMDVelData()
    try:
        ic = EasyIce.initialize(sys.argv)
        adapter = ic.createObjectAdapterWithEndpoints("CMDVelAdapter", "default -p 9999")
        object = CMDVel2Rx
        print (CMDVel2Rx)
        adapter.add(object, ic.stringToIdentity("CMDVel"))
        adapter.activate()
        ic.waitForShutdown()
    except:
        traceback.print_exc()
        status = 1

    if ic:
        # Clean up
        try:
            ic.destroy()
        except:
            traceback.print_exc()
            status = 1

    sys.exit(status)
    def __init__(self):
        self.lock = threading.Lock()
        self.playButton=False

        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()

            self.robot = properties.getProperty("TeleTaxi.robot")
            print "Robot:", self.robot

            motorsBase = ic.propertyToProxy("TeleTaxi.motors.Proxy")
            self.motorsProxy = jderobot.MotorsPrx.checkedCast(motorsBase)
            if self.motorsProxy:
                print 'Motors connected'
            else:
                print 'Motors not connected'

            basepose3D = ic.propertyToProxy("TeleTaxi.Pose3D.Proxy")
            self.pose3DProxy=jderobot.Pose3DPrx.checkedCast(basepose3D)
            if self.pose3DProxy:
                self.pose = jderobot.Pose3DData()
                self.angle = self.quat2Angle(self.pose.q0, self.pose.q1, self.pose.q2, self.pose.q3)
                print "Pose3D connected"
            else:
                print 'Pose3D not connected'

            self.maxSpeedV = 50
            self.maxSpeedW = 20   

        except:
            traceback.print_exc()
            exit()
            status = 1
Example #4
0
def openPose3DChannel(Pose3D):

    status = 0
    ic = None
    Pose2Tx = Pose3D  # Pose3D.getPose3DData()
    try:
        ic = EasyIce.initialize(sys.argv)
        adapter = ic.createObjectAdapterWithEndpoints("Pose3DAdapter", "default -p 9998")
        object = Pose2Tx
        # print (object.getPose3DData())
        adapter.add(object, ic.stringToIdentity("Pose3D"))
        adapter.activate()
        ic.waitForShutdown()
    except:
        traceback.print_exc()
        status = 1

    if ic:
        # Clean up
        try:
            ic.destroy()
        except:
            traceback.print_exc()
            status = 1

    sys.exit(status)
Example #5
0
    def __init__(self, argv=sys.argv):
        self.lock = threading.Lock()
        self.dist = 0
        self.ic = None
        try:
            self.ic = EasyIce.initialize(sys.argv)
            self.properties = self.ic.getProperties()

            self.basePoseAr = self.ic.propertyToProxy(
                "Referee.Cat.Pose3D.Proxy")
            self.poseProxy = jderobot.Pose3DPrx.checkedCast(self.basePoseAr)
            print self.poseProxy
            if not self.basePoseAr:
                raise Runtime("Cat Pose3D -> Invalid proxy")

            self.baseRedPoseAr = self.ic.propertyToProxy(
                "Referee.Mouse.Pose3D.Proxy")
            self.poseRedProxy = jderobot.Pose3DPrx.checkedCast(
                self.baseRedPoseAr)
            print self.poseRedProxy
            if not self.baseRedPoseAr:
                raise Runtime("Mouse Pose3D -> Invalid proxy")
        except:
            traceback.print_exc()
            status = 1
Example #6
0
def openCMDVelChannel(CMDVel):
    status = 0
    ic = None
    CMDVel2Rx = CMDVel  # CMDVel.getCMDVelData()
    try:
        ic = EasyIce.initialize(sys.argv)
        adapter = ic.createObjectAdapterWithEndpoints("CMDVelAdapter", "default -p 9999")
        object = CMDVel2Rx
        print(CMDVel2Rx)
        adapter.add(object, ic.stringToIdentity("CMDVel"))
        adapter.activate()
        ic.waitForShutdown()
    except:
        traceback.print_exc()
        status = 1

    if ic:
        # Clean up
        try:
            ic.destroy()
        except:
            traceback.print_exc()
            status = 1

    sys.exit(status)
    def __init__(self):
        self.lock = threading.Lock()
        self.playButton=False

        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()

            self.robot = properties.getProperty("TeleTaxi.robot")
            print "Robot:", self.robot

            motorsBase = ic.propertyToProxy("TeleTaxi.motors.Proxy")
            self.motorsProxy = jderobot.MotorsPrx.checkedCast(motorsBase)
            if self.motorsProxy:
                print 'Motors connected'
            else:
                print 'Motors not connected'

            basepose3D = ic.propertyToProxy("TeleTaxi.Pose3D.Proxy")
            self.pose3DProxy=jderobot.Pose3DPrx.checkedCast(basepose3D)
            if self.pose3DProxy:
                self.pose = jderobot.Pose3DData()
                self.angle = self.quat2Angle(self.pose.q0, self.pose.q1, self.pose.q2, self.pose.q3)
                print "Pose3D connected"
            else:
                print 'Pose3D not connected'

            self.maxSpeedV = 30
            self.maxSpeedW = 3    

        except:
            traceback.print_exc()
            exit()
            status = 1
Example #8
0
    def __init__ (self):
        ''' Camera class gets images from live video and transform them
        in order to predict the digit in the image.
        '''
	print "\nLoading Keras model..."
        self.model = load_model("/home/dpascualhe/workspace/" + 
                                "2016-tfg-david-pascual/Net/Nets/0-1_tuned/" +
                                "net_4conv_patience5.h5")
        print "loaded\n"
        status = 0
        ic = None
        
        # Initializing the Ice run-time.
        ic = EasyIce.initialize(sys.argv)

        properties = ic.getProperties()
        self.lock = threading.Lock()
    
        try:
            # We obtain a proxy for the camera.
            obj = ic.propertyToProxy("Digitclassifier.Camera.Proxy")
            # We get the first image and print its description.
            self.cam = CameraPrx.checkedCast(obj)
            if self.cam:
                self.im = self.cam.getImageData("RGB8")
                self.im_height = self.im.description.height
                self.im_width = self.im.description.width
                print(self.im.description)
            else: 
                print("Interface camera not connected")
                    
        except:
            traceback.print_exc()
            exit()
            status = 1
Example #9
0
    def __init__(self):
        self.lock = threading.Lock()

        try:
            ic = EasyIce.initialize(sys.argv)
            basecamera = ic.propertyToProxy("basic_component.Camera1.Proxy")
            self.cameraProxy = jderobot.CameraPrx.checkedCast(basecamera)

            if self.cameraProxy:
                self.image = self.cameraProxy.getImageData("RGB8")
                self.height = self.image.description.height
                self.width = self.image.description.width

                self.trackImage = np.zeros((self.height, self.width, 3), np.uint8)
                self.trackImage.shape = self.height, self.width, 3

                self.thresoldImage = np.zeros((self.height, self.width, 1), np.uint8)
                self.thresoldImage.shape = self.height, self.width,

            else:
                print ('Interface camera not connected')

        except:
            traceback.print_exc()
            exit()
Example #10
0
def openPose3DChannel(Pose3D):

    status = 0
    ic = None
    Pose2Tx = Pose3D #Pose3D.getPose3DData()
    try:
        ic = EasyIce.initialize(sys.argv)
        adapter = ic.createObjectAdapterWithEndpoints("Pose3DAdapter", "default -p 9998")
        object = Pose2Tx
        #print (object.getPose3DData())
        adapter.add(object, ic.stringToIdentity("Pose3D"))
        adapter.activate()
        ic.waitForShutdown()
    except:
        traceback.print_exc()
        status = 1

    if ic:
        # Clean up
        try:
            ic.destroy()
        except:
            traceback.print_exc()
            status = 1

    sys.exit(status)
Example #11
0
def openExtraChannel():

    status = 0
    ic = None
    Extra2Tx = ExtraI()
    try:
        ic = EasyIce.initialize(sys.argv)
        adapter = ic.createObjectAdapterWithEndpoints("ExtraAdapter", "default -p 9002")
        object = Extra2Tx
        adapter.add(object, ic.stringToIdentity("Extra"))
        adapter.activate()
        ic.waitForShutdown()
    except:
        traceback.print_exc()
        status = 1

    if ic:
        # Clean up
        try:
            ic.destroy()
        except:
            traceback.print_exc()
            status = 1

    sys.exit(status)
    def __init__(self):
        self.lock = threading.Lock()
        self.playButton = False

        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()
            basecameraL = ic.propertyToProxy(
                "ObstacleAvoidance.CameraLeft.Proxy")
            self.cameraProxyL = jderobot.CameraPrx.checkedCast(basecameraL)

            if self.cameraProxyL:
                self.imageLeft = self.cameraProxyL.getImageData("RGB8")
                self.imageLeft_h = self.imageLeft.description.height
                self.imageLeft_w = self.imageLeft.description.width
            else:
                print 'Interface for left camera not connected'

            basecameraR = ic.propertyToProxy(
                "ObstacleAvoidance.CameraRight.Proxy")
            self.cameraProxyR = jderobot.CameraPrx.checkedCast(basecameraR)

            if self.cameraProxyR:
                self.imageRight = self.cameraProxyR.getImageData("RGB8")
                self.imageRight_h = self.imageRight.description.height
                self.imageRight_w = self.imageRight.description.width
            else:
                print 'Interface for right camera not connected'

            motorsBase = ic.propertyToProxy("ObstacleAvoidance.motors.Proxy")
            self.motorsProxy = jderobot.MotorsPrx.checkedCast(motorsBase)
            if self.motorsProxy:
                print 'Interface for motors connected'
            else:
                print 'Interface for motors not connected'

            encodersBase = ic.propertyToProxy(
                "ObstacleAvoidance.encoders.Proxy")
            #self.encodersProxy = jderobot.EncodersPrx.checkedCast(encodersBase)
            self.encodersProxy = jderobot.Pose3DPrx.checkedCast(encodersBase)
            if self.encodersProxy:
                print 'Interface for encoders connected'
            else:
                print 'Interface for encoders not connected'

            laserBase = ic.propertyToProxy("ObstacleAvoidance.laser.Proxy")
            self.laserProxy = jderobot.LaserPrx.checkedCast(laserBase)
            if self.laserProxy:
                print 'Interface for laser connected'
            else:
                print 'Interface for laser not connected'

            self.maxSpeedV = 2.0
            self.maxSpeedW = -1.0

        except:
            traceback.print_exc()
            exit()
            status = 1
Example #13
0
 def connectProxies(self):
     self.ic = EasyIce.initialize(sys.argv)
     self.myMotors = self.ic.propertyToProxy("automata.myMotors.Proxy")
     if not self.myMotors:
         raise Exception("could not create proxy with myMotors")
     self.myMotors = MotorsPrx.checkedCast(self.myMotors)
     if not self.myMotors:
         raise Exception("invalid proxy automata.myMotors.Proxy")
Example #14
0
 def connectProxies(self):
     self.ic = EasyIce.initialize(sys.argv)
     self.myMotors = self.ic.propertyToProxy("automata.myMotors.Proxy")
     if not self.myMotors:
         raise Exception("could not create proxy with myMotors")
     self.myMotors = MotorsPrx.checkedCast(self.myMotors)
     if not self.myMotors:
         raise Exception("invalid proxy automata.myMotors.Proxy")
Example #15
0
    def connectToProxys(self):
        self.ic = EasyIce.initialize(sys.argv)
        self.ic, self.node = comm.init(self.ic)

        # Contact to my_motors
        self.my_motors = comm.getMotorsClient(self.ic, 'automata.my_motors')
        if (not self.my_motors):
            raise Exception('could not create client with my_motors')
        print('my_motors connected')
Example #16
0
	def connectToProxys(self):
		self.ic = EasyIce.initialize(sys.argv)
		self.ic,self.node = comm.init(self.ic)

		# Contact to my_motors
		self.my_motors = comm.getMotorsClient(self.ic, 'automata.my_motors')
		if(not self.my_motors):
			raise Exception('could not create client with my_motors')
		print('my_motors connected')
    def __init__(self):
        self.lock = threading.Lock()
        self.playButton=False

        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()
            basecameraL = ic.propertyToProxy("ObstacleAvoidance.CameraLeft.Proxy")
            self.cameraProxyL = jderobot.CameraPrx.checkedCast(basecameraL)

            if self.cameraProxyL:
                self.imageLeft = self.cameraProxyL.getImageData("RGB8")
                self.imageLeft_h= self.imageLeft.description.height
                self.imageLeft_w = self.imageLeft.description.width
            else:
                print 'Interface for left camera not connected'

            basecameraR = ic.propertyToProxy("ObstacleAvoidance.CameraRight.Proxy")
            self.cameraProxyR = jderobot.CameraPrx.checkedCast(basecameraR)

            if self.cameraProxyR:
                self.imageRight = self.cameraProxyR.getImageData("RGB8")
                self.imageRight_h= self.imageRight.description.height
                self.imageRight_w = self.imageRight.description.width
            else:
                print 'Interface for right camera not connected'

            motorsBase = ic.propertyToProxy("ObstacleAvoidance.motors.Proxy")
            self.motorsProxy = jderobot.MotorsPrx.checkedCast(motorsBase)
            if self.motorsProxy:
                print 'Interface for motors connected'
            else:
                print 'Interface for motors not connected'

            encodersBase = ic.propertyToProxy("ObstacleAvoidance.encoders.Proxy")
            #self.encodersProxy = jderobot.EncodersPrx.checkedCast(encodersBase)
            self.encodersProxy = jderobot.Pose3DPrx.checkedCast(encodersBase)
            if self.encodersProxy:
                print 'Interface for encoders connected'
            else:
                print 'Interface for encoders not connected'

            laserBase = ic.propertyToProxy("ObstacleAvoidance.laser.Proxy")
            self.laserProxy = jderobot.LaserPrx.checkedCast(laserBase)
            if self.laserProxy:
                print 'Interface for laser connected'
            else:
                print 'Interface for laser not connected'

            self.maxSpeedV=2.0
            self.maxSpeedW=-1.0

        except:
            traceback.print_exc()
            exit()
            status = 1
Example #18
0
	def connectToProxys(self):
		self.ic = EasyIce.initialize(sys.argv)

		# Contact to myMotors
		myMotors = self.ic.propertyToProxy('automata.myMotors.Proxy')
		if(not myMotors):
			raise Exception('could not create proxy with myMotors')
		self.myMotors = MotorsPrx.checkedCast(myMotors)
		if(not self.myMotors):
			raise Exception('invalid proxy automata.myMotors.Proxy')
		print('myMotors connected')
Example #19
0
    def __init__(self):
        self.lock = threading.Lock()
        self.v = self.w = 0
        try:
            ic = EasyIce.initialize(sys.argv)
            base = ic.propertyToProxy("basic_component.Motors.Proxy")
            self.proxy = jderobot.MotorsPrx.checkedCast(base)

            if not self.proxy:
                print ('Interface Motors not connected')

        except:
            traceback.print_exc()
            exit()
    def __init__(self):
        self.lock = threading.Lock()
        self.playButton=False

        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()
            basecameraL = ic.propertyToProxy("FollowLine.CameraLeft.Proxy")
            self.cameraProxyL = jderobot.CameraPrx.checkedCast(basecameraL)

            self.robot = properties.getProperty("FolowLine.robot")

            if self.cameraProxyL:
                self.imageLeft = self.cameraProxyL.getImageData("RGB8")
                self.imageLeft_h= self.imageLeft.description.height
                self.imageLeft_w = self.imageLeft.description.width
            else:
                print 'Interface for left camera not connected'

            basecameraR = ic.propertyToProxy("FollowLine.CameraRight.Proxy")
            self.cameraProxyR = jderobot.CameraPrx.checkedCast(basecameraR)

            if self.cameraProxyR:
                self.imageRight = self.cameraProxyR.getImageData("RGB8")
                self.imageRight_h= self.imageRight.description.height
                self.imageRight_w = self.imageRight.description.width
            else:
                print 'Interface for right camera not connected'


            motorsBase = ic.propertyToProxy("FolowLine.motors.Proxy")
            self.motorsProxy = jderobot.MotorsPrx.checkedCast(motorsBase)
            if self.motorsProxy:
                print 'Interface for motors connected'
            else:
                print 'Interface for motors not connected'


            if self.robot == 'F1':
                self.maxSpeedV=30
                self.maxSpeedW=3
            else:
                self.maxSpeedV=100
                self.maxSpeedW=100


        except:
            traceback.print_exc()
            exit()
            status = 1
Example #21
0
    def __init__(self):
        self.lock = threading.Lock()
        self.v = self.w = 0
        try:
            ic = EasyIce.initialize(sys.argv)
            base = ic.propertyToProxy("basic_component.Motors.Proxy")
            self.proxy = jderobot.MotorsPrx.checkedCast(base)

            if not self.proxy:
                print('Interface Motors not connected')

        except:
            traceback.print_exc()
            exit()
Example #22
0
	def connectToProxys(self):
		self.ic = EasyIce.initialize(sys.argv)
		self.ic,self.node = comm.init(self.ic)

		# Contact to KobukiMotors
		self.KobukiMotors = comm.getMotorsClient(self.ic, 'automata.KobukiMotors')
		if(not self.KobukiMotors):
			raise Exception('could not create client with KobukiMotors')
		print('KobukiMotors connected')

		# Contact to KobukiLaser
		self.KobukiLaser = comm.getLaserClient(self.ic, 'automata.KobukiLaser')
		if(not self.KobukiLaser):
			raise Exception('could not create client with KobukiLaser')
		print('KobukiLaser connected')
Example #23
0
    def __init__(self, opt):
        self.lock = threading.Lock()
        self.lock2 = threading.Lock()
        self.opt = opt
        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()

            #Connection to ICE interfaces

            #------- POSE3D AUTOLOC ---------


            basepose3D = ic.propertyToProxy("CamAutoloc.Pose3D.Proxy")
            print "Autoloc:",basepose3D

            self.pose3DProxy=jderobot.Pose3DPrx.checkedCast(basepose3D)
            if self.pose3DProxy:
                self.pose=jderobot.Pose3DData()
            else:
                print 'Interface pose3D not connected'





            #------- POSE3D SIM ---------
            simpose3D = ic.propertyToProxy("CamAutoloc.Pose3Dsim.Proxy")
            print "GroundTruth: ",simpose3D
            self.simpose3DProxy=jderobot.Pose3DPrx.checkedCast(simpose3D)
            if self.simpose3DProxy:
                self.simpose=jderobot.Pose3DData()
            else:
                print 'Interface pose3D not connected'







        except:
            traceback.print_exc()
            exit()
            status = 1

        self.pause = True
        self.patherror = 0.0
    def __init__(self):
        self.lock = threading.Lock()
        self.playButton = False

        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()
            basecameraL = ic.propertyToProxy("FollowLine.CameraLeft.Proxy")
            self.cameraProxyL = jderobot.CameraPrx.checkedCast(basecameraL)

            self.robot = properties.getProperty("FolowLine.robot")

            if self.cameraProxyL:
                self.imageLeft = self.cameraProxyL.getImageData("RGB8")
                self.imageLeft_h = self.imageLeft.description.height
                self.imageLeft_w = self.imageLeft.description.width
            else:
                print 'Interface for left camera not connected'

            basecameraR = ic.propertyToProxy("FollowLine.CameraRight.Proxy")
            self.cameraProxyR = jderobot.CameraPrx.checkedCast(basecameraR)

            if self.cameraProxyR:
                self.imageRight = self.cameraProxyR.getImageData("RGB8")
                self.imageRight_h = self.imageRight.description.height
                self.imageRight_w = self.imageRight.description.width
            else:
                print 'Interface for right camera not connected'

            motorsBase = ic.propertyToProxy("FolowLine.motors.Proxy")
            self.motorsProxy = jderobot.MotorsPrx.checkedCast(motorsBase)
            if self.motorsProxy:
                print 'Interface for motors connected'
            else:
                print 'Interface for motors not connected'

            if self.robot == 'F1':
                self.maxSpeedV = 30
                self.maxSpeedW = 3
            else:
                self.maxSpeedV = 100
                self.maxSpeedW = 100

        except:
            traceback.print_exc()
            exit()
            status = 1
	def __init__(self,argv=sys.argv):
		self.lock = threading.Lock()
		self.dist=0
		self.ic = None
		try:
			self.ic = EasyIce.initialize(sys.argv)
			self.properties = self.ic.getProperties()
	
			self.basePoseAr = self.ic.propertyToProxy("Referee.Cat.Pose3D.Proxy")
			self.poseProxy = jderobot.Pose3DPrx.checkedCast(self.basePoseAr)
			print self.poseProxy
			if not self.basePoseAr:
				raise Runtime("Cat Pose3D -> Invalid proxy")
	
			self.baseRedPoseAr = self.ic.propertyToProxy("Referee.Mouse.Pose3D.Proxy")
			self.poseRedProxy = jderobot.Pose3DPrx.checkedCast(self.baseRedPoseAr)
			print self.poseRedProxy
			if not self.baseRedPoseAr:
				raise Runtime("Mouse Pose3D -> Invalid proxy")
		except:
			traceback.print_exc()
			status = 1
Example #26
0
 def __init__(self):
     try:
         ic = EasyIce.initialize(sys.argv)
         proxy = ic.getPropertyWithDefault("emSensorView.emSensor.Proxy","emSensor:default -h localhost -p 9090")
         try:
                 base = ic.stringToProxy(proxy)
         except:
             print(("Failed to use defined 'emSensorView.emSensor.Proxy',"
                    "using 'emSensor:default -h localhost -p 9090' as fallback"),
                     file=sys.stderr)
             base = ic.stringToProxy("emSensor:default -h localhost -p 9090")
             
         self.emSensorProxy = jderobot.EMSensorPrx.checkedCast(base)
         if not self.emSensorProxy:
             raise RuntimeError("Invalid proxy")
     
         self.sensorData = self.emSensorProxy.getEMSensorData();
             
         self.lock = threading.Lock()  
         self.running = True
     except:
         traceback.print_exc()
         self.running = False
Example #27
0
import jderobotComm as comm
import os
import scratch
import time

from drone import Drone

# get current working directory
path = os.getcwd()
open_path = path[:path.rfind('src')] + 'cfg/'
filename = 'drone_mouse.cfg'


if __name__ == '__main__':
    # loading the ICE and ROS parameters
    ic = EasyIce.initialize(['main_drone.py', open_path + filename])
    ic, node = comm.init(ic)

    # creating the object
    robot = Drone(ic)

    try:
        # executing the mouse behavior
        robot.take_off()
        time.sleep(1)

        while True:
            robot.stop()

        #~ robot.move("left")
        #~ time.sleep(3)
Example #28
0
    def __init__(self):
        self.lock = threading.Lock()

        self.playButton = False

        self.cmd = jderobot.CMDVelData()
        self.cmd.linearX = self.cmd.linearY = self.cmd.linearZ = 0
        self.cmd.angularZ = 0
        ''' With values distinct to 0 in the next fields, the ardrone not enter in hover mode'''
        self.cmd.angularX = 0.0
        self.cmd.angularY = 0.0

        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()
            basecamera = ic.propertyToProxy("Introrob.Camera.Proxy")
            self.cameraProxy = jderobot.CameraPrx.checkedCast(basecamera)

            if self.cameraProxy:
                self.image = self.cameraProxy.getImageData("RGB8")
                self.height = self.image.description.height
                self.width = self.image.description.width

                self.trackImage = np.zeros((self.height, self.width, 3),
                                           np.uint8)
                self.trackImage.shape = self.height, self.width, 3

                self.thresoldImage = np.zeros((self.height, self.width, 1),
                                              np.uint8)
                self.thresoldImage.shape = self.height, self.width,

            else:
                print 'Interface camera not connected'

            basecmdVel = ic.propertyToProxy("Introrob.CMDVel.Proxy")
            self.cmdVelProxy = jderobot.CMDVelPrx.checkedCast(basecmdVel)
            if not self.cmdVelProxy:
                print 'Interface cmdVel not connected'

            basenavdata = ic.propertyToProxy("Introrob.Navdata.Proxy")
            self.navdataProxy = jderobot.NavdataPrx.checkedCast(basenavdata)

            if self.navdataProxy:
                self.navdata = self.navdataProxy.getNavdata()
                if self.navdata.vehicle == self.ARDRONE_SIMULATED:
                    self.virtualDrone = True
                else:
                    self.virtualDrone = False
            else:
                print 'Interface navdata not connected'
                self.virtualDrone = True

            baseextra = ic.propertyToProxy("Introrob.Extra.Proxy")
            self.extraProxy = jderobot.ArDroneExtraPrx.checkedCast(baseextra)
            if not self.extraProxy:
                print 'Interface ardroneExtra not connected'

            basepose3D = ic.propertyToProxy("Introrob.Pose3D.Proxy")
            self.pose3DProxy = jderobot.Pose3DPrx.checkedCast(basepose3D)
            if self.pose3DProxy:
                self.pose = jderobot.Pose3DData()
            else:
                print 'Interface pose3D not connected'

        except:
            traceback.print_exc()
            exit()
            status = 1
Example #29
0
#

import sys

import jderobotComm as comm
from gui.threadGUI import ThreadGUI
from gui.GUI import MainWindow
from PyQt5.QtWidgets import QApplication
import easyiceconfig as EasyIce

import signal

signal.signal(signal.SIGINT, signal.SIG_DFL)

if __name__ == '__main__':
    ic = EasyIce.initialize(sys.argv)

    #starting comm
    ic, node = comm.init(ic)


    camera = comm.getCameraClient(ic,"basic_component.Camera")
    motors = comm.getMotorsClient(ic,"basic_component.Motors")

    app = QApplication(sys.argv)
    frame = MainWindow()
    frame.setMotors(motors)
    frame.setCamera(camera)
    frame.show()

    t2 = ThreadGUI(frame)
    machine.setStateName(0, 'Take off')
    machine.setStateName(1, 'Search')
    machine.setStateName(2, 'Possibly \n beacon')
    machine.setStateName(3, 'Centering \n beacon')
    machine.setStateName(4, 'Landing')
    machine.setStateName(5, 'Landed')

    machine.addTransition(0, 1,'')
    machine.addTransition(1, 2,'')
    machine.addTransition(2, 3,'')
    machine.addTransition(3, 4,'')
    machine.addTransition(4, 5,'')
    machine.addTransition(5, 0,'')


    ic = EasyIce.initialize(sys.argv) #Establece conexiones con los servidores de los conectores y actuadores
    cameraCli = CameraClient(ic, "Introrob.Camera", True)
    camera = CameraFilter(cameraCli)
    navdata = NavDataClient(ic, "Introrob.Navdata", True)
    pose = Pose3DClient(ic, "Introrob.Pose3D", True)
    cmdvel = CMDVel(ic, "Introrob.CMDVel")
    extra = Extra(ic, "Introrob.Extra")

    algorithm=MyAlgorithm(camera, navdata, pose, cmdvel, machine, extra)


    app = QApplication(sys.argv)
    frame = MainWindow()
    frame.setCamera(camera)
    frame.setNavData(navdata)
    frame.setPose3D(pose)
Example #31
0
    def __init__(self):
        self.lock = threading.Lock()
        self.playButton=False

        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()
            basecameraL = ic.propertyToProxy("FollowLine.CameraLeft.Proxy")
            self.cameraProxyL = jderobot.CameraPrx.checkedCast(basecameraL)

            if self.cameraProxyL:
                self.imageLeft = self.cameraProxyL.getImageData("RGB8")
                self.imageLeft_h= self.imageLeft.description.height
                self.imageLeft_w = self.imageLeft.description.width
            else:
                print 'Interface for left camera not connected'

            basecameraR = ic.propertyToProxy("FollowLine.CameraRight.Proxy")
            self.cameraProxyR = jderobot.CameraPrx.checkedCast(basecameraR)

            if self.cameraProxyR:
                self.imageRight = self.cameraProxyR.getImageData("RGB8")
                self.imageRight_h= self.imageRight.description.height
                self.imageRight_w = self.imageRight.description.width
            else:
                print 'Interface for right camera not connected'


            motorsBase = ic.propertyToProxy("FolowLine.motors.Proxy")
            self.motorsProxy = jderobot.MotorsPrx.checkedCast(motorsBase)
            if self.motorsProxy:
                print 'Interface for motors connected'
            else:
                print 'Interface for motors not connected'


            self.maxSpeedV=1
            self.maxSpeedW=1


            #visualization
            baseViewer = ic.propertyToProxy("FollowLine.Viewer.Proxy")
            self.viewerProxy = jderobot.VisualizationPrx.checkedCast(baseViewer)

            #draw floor:
            self.MAXWORLD=30
            colorJDE= jderobot.Color()
            colorJDE.r=0
            colorJDE.g=0
            colorJDE.b=0
            for i in range(0,self.MAXWORLD+1):
                pointJde1= jderobot.Point()
                pointJde2= jderobot.Point()
                pointJde3= jderobot.Point()
                pointJde4= jderobot.Point()
                pointJde1.x=-self.MAXWORLD * 10 / 2 + i * 10
                pointJde1.y= -self.MAXWORLD * 10 / 2
                pointJde1.z=0
                pointJde2.x=-self.MAXWORLD * 10 / 2 + i * 10
                pointJde2.y= self.MAXWORLD * 10 / 2
                pointJde2.z=0
                pointJde3.x=-self.MAXWORLD * 10 / 2
                pointJde3.y=-self.MAXWORLD * 10 / 2. + i * 10
                pointJde3.z=0
                pointJde4.x=self.MAXWORLD * 10 / 2
                pointJde4.y=-self.MAXWORLD * 10 / 2. + i * 10
                pointJde4.z=0
                seg1= jderobot.Segment()
                seg1.fromPoint=pointJde1
                seg1.toPoint=pointJde2
                seg2= jderobot.Segment()
                seg2.fromPoint=pointJde3
                seg2.toPoint=pointJde4
                self.viewerProxy.drawSegment(seg1,colorJDE)
                self.viewerProxy.drawSegment(seg2,colorJDE)




        except:
            traceback.print_exc()
            exit()
            status = 1
Example #32
0
__version__ = "0.0.0"
__maintainer__ = "Raul Perula-Martinez"
__email__ = "*****@*****.**"
__status__ = "Development"

import easyiceconfig as EasyIce
import jderobotComm as comm
import os
import scratch

from robot import Robot

# get current working directory
path = os.getcwd()
open_path = path[:path.rfind('src')] + 'cfg/'
filename = 'robot.cfg'

if __name__ == '__main__':
    # loading the ICE and ROS parameters
    ic = EasyIce.initialize(['main_robot.py', open_path + filename])
    ic, node = comm.init(ic)

    # creating the object
    robot = Robot(ic, node)

    # executing the scratch program
    scratch.execute(robot)

    # destroying the common communications
    comm.destroy(ic, node)
Example #33
0
        f.close()              
    
    def checkDestination(self):
        found = False
        xRobot = self.pose3d.getX()
        yRobot = self.pose3d.getY()
        xdest = self.dist.destiny[0]
        ydest = self.dist.destiny[1]
        if (abs(xRobot)<(abs(xdest)+5) and abs(xRobot)>(abs(xdest)-5)) and (abs(yRobot)<(abs(ydest)+5) and abs(yRobot)>(abs(ydest)-5)):
            found = True
        return found
        

    def updateG(self):
        self.update()
        
        
        
if __name__ == "__main__":
    
    app = QApplication(sys.argv)
    ic = EasyIce.initialize(sys.argv)
    pose3d = Pose3DClient(ic, "TeleTaxi.Pose3D", True)

    myGUI = MainWindowReferee(pose3d)
    myGUI.show()
    t2 = ThreadGUI(myGUI)
    t2.daemon=True
    t2.start()
    sys.exit(app.exec_())
    def __init__(self):
        self.lock = threading.Lock()

        self.playButton=False

        self.cmd=jderobot.CMDVelData()
        self.cmd.linearX=self.cmd.linearY=self.cmd.linearZ=0
        self.cmd.angularZ=0
        ''' With values distinct to 0 in the next fields, the ardrone not enter in hover mode'''
        self.cmd.angularX=0.0
        self.cmd.angularY=0.0

        try:
            ic = EasyIce.initialize(sys.argv)
            properties = ic.getProperties()
            basecamera = ic.propertyToProxy("Introrob.Camera.Proxy")
            self.cameraProxy = jderobot.CameraPrx.checkedCast(basecamera)

            if self.cameraProxy:
                self.image = self.cameraProxy.getImageData("RGB8")
                self.height= self.image.description.height
                self.width = self.image.description.width

                self.trackImage = np.zeros((self.height, self.width,3), np.uint8)
                self.trackImage.shape = self.height, self.width, 3

                self.thresoldImage = np.zeros((self.height, self.width,1), np.uint8)
                self.thresoldImage.shape = self.height, self.width,

            else:
                print('Interface camera not connected')

            basecmdVel = ic.propertyToProxy("Introrob.CMDVel.Proxy")
            self.cmdVelProxy=jderobot.CMDVelPrx.checkedCast(basecmdVel)
            if not self.cmdVelProxy:
                print('Interface cmdVel not connected')

            basenavdata = ic.propertyToProxy("Introrob.Navdata.Proxy")
            self.navdataProxy = jderobot.NavdataPrx.checkedCast(basenavdata)

            if self.navdataProxy:
                self.navdata=self.navdataProxy.getNavdata()
                if self.navdata.vehicle == self.ARDRONE_SIMULATED :
                    self.virtualDrone = True
                else:
                    self.virtualDrone = False
            else:
                print('Interface navdata not connected')
                self.virtualDrone = True

            baseextra = ic.propertyToProxy("Introrob.Extra.Proxy")
            self.extraProxy=jderobot.ArDroneExtraPrx.checkedCast(baseextra)
            if not self.extraProxy:
                print('Interface ardroneExtra not connected')

            basepose3D = ic.propertyToProxy("Introrob.Pose3D.Proxy")
            self.pose3DProxy=jderobot.Pose3DPrx.checkedCast(basepose3D)
            if self.pose3DProxy:
                self.pose=jderobot.Pose3DData()
            else:
                print('Interface pose3D not connected')

        except:
            traceback.print_exc()
            exit()
            status = 1
Example #35
0
__version__ = "0.0.0"
__maintainer__ = "Raul Perula-Martinez"
__email__ = "*****@*****.**"
__status__ = "Development"

import easyiceconfig as EasyIce
import jderobotComm as comm
import os
import scratch

from drone import Drone

# get current working directory
path = os.getcwd()
open_path = path[:path.rfind('src')] + 'cfg/'
filename = 'drone.cfg'

if __name__ == '__main__':
    # loading the ICE and ROS parameters
    ic = EasyIce.initialize(['main_drone.py', open_path + filename])
    ic, node = comm.init(ic)

    # creating the object
    robot = Drone(ic)

    # executing the scratch program
    scratch.execute(robot)

    # destroying the common communications
    comm.destroy(ic, node)
	def __init__(self):
		self.lock = threading.Lock()

		self.playButton=False

		self.cmd=jderobot.CMDVelData()
		self.cmd.linearX=self.cmd.linearY=self.cmd.linearZ=0
		self.cmd.angularZ=0
		''' With values distinct to 0 in the next fields, the ardrone not enter in hover mode'''
		self.cmd.angularX=0.0
		self.cmd.angularY=0.0
	
		try:
			ic = EasyIce.initialize(sys.argv)
			properties = ic.getProperties()
			basecamera = ic.propertyToProxy("CamTry.Camera.Proxy")
			self.cameraProxy = jderobot.CameraPrx.checkedCast(basecamera)

			if self.cameraProxy:
				self.image = self.cameraProxy.getImageData("RGB8")
				self.height= self.image.description.height
				self.width = self.image.description.width

				self.trackImage = np.zeros((self.height, self.width,3), np.uint8)
				self.trackImage.shape = self.height, self.width, 3
	
				self.thresoldImage = np.zeros((self.height, self.width,1), np.uint8)
				self.thresoldImage.shape = self.height, self.width,

			else:
				print 'Interface camera not connected'

			basecmdVel = ic.propertyToProxy("Introrob.CMDVel.Proxy")
			self.cmdVelProxy=jderobot.CMDVelPrx.checkedCast(basecmdVel)
			if not self.cmdVelProxy:
				print 'Interface cmdVel not connected'

			basenavdata = ic.propertyToProxy("Introrob.Navdata.Proxy")
			self.navdataProxy = jderobot.NavdataPrx.checkedCast(basenavdata)

			if self.navdataProxy:
				self.navdata=self.navdataProxy.getNavdata()
				if self.navdata.vehicle == self.ARDRONE_SIMULATED :
					self.virtualDrone = True
				else:
					self.virtualDrone = False
			else:
				print 'Interface navdata not connected'
				self.virtualDrone = True

			baseextra = ic.propertyToProxy("Introrob.Extra.Proxy")
			self.extraProxy=jderobot.ArDroneExtraPrx.checkedCast(baseextra)
			if not self.extraProxy:
				print 'Interface ardroneExtra not connected'

			basepose3D = ic.propertyToProxy("Introrob.Pose3D.Proxy")
			self.pose3DProxy=jderobot.Pose3DPrx.checkedCast(basepose3D)
			if self.pose3DProxy:
				self.pose=jderobot.Pose3DData()
			else:
				print 'Interface pose3D not connected'
	
		except:
			traceback.print_exc()
			exit()
			status = 1

            
		def update(self):
			self.lock.acquire()
			self.updateCamera()
			self.updateNavdata()
			self.updatePose()
			self.lock.release()

		def updateCamera(self):
			if self.cameraProxy:
				self.image = self.cameraProxy.getImageData("RGB8")
				self.height= self.image.description.height
				self.width = self.image.description.width

		def updateNavdata(self):
			if self.navdataProxy:
				self.navdata=self.navdataProxy.getNavdata()
	
		#def updatePose(self):
		#	if self.pose3DProxy:
		#	self.pose=self.pose3DProxy.getPose3DData()

		def getNavdata(self):
			if self.navdataProxy:
				self.lock.acquire()
				tmp=self.navdata
				self.lock.release()
				return tmp

			return None
    
		def getImage(self):
			if self.cameraProxy:
				self.lock.acquire()
				img = np.zeros((self.height, self.width, 3), np.uint8)
				img = np.frombuffer(self.image.pixelData, dtype=np.uint8)
				img.shape = self.height, self.width, 3
				self.lock.release()
				return img;

			return None

		def takeoff(self):
			if self.extraProxy:
				self.sendCMDVel(0,0,0,0,0,0)
				self.lock.acquire()
				self.extraProxy.takeoff()
				self.lock.release()
        
		def land(self):
			if self.extraProxy:
				self.sendCMDVel(0,0,0,0,0,0)
				self.lock.acquire()
				self.extraProxy.land()
				self.lock.release()
       
		def toggleCam(self):
			if self.extraProxy:
				self.lock.acquire()
				self.extraProxy.toggleCam()
				self.lock.release()
              
		def reset(self):
			if self.extraProxy:
				self.lock.acquire()
				self.extraProxy.reset()
				self.lock.release()
       
		def record(self,record):
			if self.extraProxy and self.navdataProxy:
				if self.navdata.vehicle == self.ARDRONE2:
					self.extraProxy.recordOnUsb(record)

		def setVX(self,vx):
			self.cmd.linearX=vx

		def setVY(self,vy):
			self.cmd.linearY=vy

		def setVZ(self,vz):
			self.cmd.linearZ=vz

		def setYaw(self,yaw):
			self.cmd.angularZ=yaw

		def setRoll(self,roll):
			self.cmd.angularX=roll
        
		def setPitch(self,pitch):
			self.cmd.angularY=pitch
	
		def sendVelocities(self):
			if self.cmdVelProxy:
				if self.isVirtual():
					self.sendCMDVel(self.cmd.linearY,self.cmd.linearX,self.cmd.linearZ,self.cmd.angularZ,self.cmd.angularY,self.cmd.angularX)
				else:
					self.sendCMDVel(self.cmd.linearX,self.cmd.linearY,self.cmd.linearZ,self.cmd.angularZ,self.cmd.angularY,self.cmd.angularX)

		def sendCMDVel(self,vx,vy,vz,yaw,roll,pitch):
			cmd=jderobot.CMDVelData()

			if self.isVirtual() == True:

				if vx > self.MAX_LINX_SIM:
					vx = self.MAX_LINX_SIM
				elif vx < -self.MAX_LINX_SIM:
					vx = -self.MAX_LINX_SIM
	
				if vy > self.MAX_LINY_SIM:
					vy = self.MAX_LINY_SIM
				elif vy < -self.MAX_LINY_SIM:
					vy = -self.MAX_LINY_SIM

				if vz > self.MAX_LINZ_SIM:
					vz = self.MAX_LINZ_SIM
				elif vz < -self.MAX_LINZ_SIM:
					vz = -self.MAX_LINZ_SIM

				if yaw > self.MAX_ANGZ_SIM:
					yaw = self.MAX_ANGZ_SIM
				elif yaw < -self.MAX_ANGZ_SIM:
					yaw = -self.MAX_ANGZ_SIM

				cmd.linearX=vy
				cmd.linearY=vx
				cmd.linearZ=vz
				cmd.angularZ=yaw
			else:
				if abs(vx) > self.MAX_LINX:
					if vx > 0:
						cmd.linearX = self.MAX_LINX
					else:
						cmd.linearX = -self.MAX_LINX
				else:
					cmd.linearX = vx

				if abs(vy) > self.MAX_LINY:
					if vy > 0:
						cmd.linearY = self.MAX_LINY
					else:
						cmd.linearY = -self.MAX_LINY
				else:
					cmd.linearY = vy
	
				if abs(vz) > self.MAX_LINZ:
					if vz > 0:
						cmd.linearZ = self.MAX_LINZ
					else:
						cmd.linearZ = -self.MAX_LINZ
				else:
					cmd.linearZ = vz
	
				if abs(yaw) > self.MAX_ANGZ:
					if yaw > 0:
						cmd.angularZ = self.MAX_ANGZ
					else:
						cmd.angularZ = -self.MAX_ANGZ
				else:
					cmd.angularZ = yaw

			cmd.angularX = roll
			cmd.angularY = pitch

			if self.cmdVelProxy:
				self.lock.acquire();
				self.cmdVelProxy.setCMDVelData(cmd)
				self.lock.release();
    
		def getPose3D(self):
			if self.pose3DProxy:
				self.lock.acquire()
				tmp=self.pose
				self.lock.release()
				return tmp

			return None

		def isPlayButton(self):
			return self.playButton
    
		def setPlayButton(self,value):
			self.playButton=value

		def isVirtual(self):
			return self.virtualDrone