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 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
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)
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
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
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
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()
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)
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
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")
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")
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 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
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')
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
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 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')
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
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
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)
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
#
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)
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
__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)
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
__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