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 getObj3D(self, id, current=None):
if len(obj_list) > self.cont:
obj3D = jderobot.object3d()
model = obj_list[self.cont].split(":")
if model[0] == "https":
obj3D.obj = obj_list[self.cont]
model = obj_list[self.cont].split("/")
name, form = model[len(model) - 1].split(".")
print "Sending model by url: " + name + "." + form
else:
obj = open(obj_list[self.cont], "r").read()
name, form = obj_list[self.cont].split(".")
obj3D.obj = obj
print "Sending model by file: " + name + "." + form
pose3d = jderobot.Pose3DData()
pose3d.x = float(init_pos[self.cont][0] / 100.)
pose3d.y = float(init_pos[self.cont][1] / 100.)
pose3d.z = float(init_pos[self.cont][2] / 100.)
pose3d.h = 1
pose3d.q0 = init_quat[self.cont][0]
pose3d.q1 = init_quat[self.cont][1]
pose3d.q2 = init_quat[self.cont][2]
pose3d.q3 = init_quat[self.cont][3]
id_list.append(id)
obj3D.id = id
obj3D.format = form
obj3D.pos = pose3d
obj3D.scale = scales[self.cont]
self.cont = self.cont + 1
return obj3D
def getPose3DData(self, current=None):
while not ac.isArmed():
time.sleep(0.5)
pose = jderobot.Pose3DData()
att = ac.getAttitude()
gps = ac.getLocation()
print("attitude" + str(att))
print("gps" + str(gps))
yaw = att["yaw"]
pitch = att["pitch"]
roll = att["roll"]
pose.x = gps["lat"]
pose.y = gps["lon"]
pose.z = ac.getAltitude()
q = quaternion.Quaternion([roll, pitch, yaw])
pose.q0 = q.__getitem__(0)
pose.q1 = q.__getitem__(1)
pose.q2 = q.__getitem__(2)
pose.q3 = q.__getitem__(3)
print(q)
return pose
def getObj3D(self, id, current=None):
if len(obj_list) > self.cont:
obj3D = jderobot.object3d()
model = obj_list[self.cont].split(":")
if model[0] == "https":
obj3D.obj = obj_list[self.cont]
model = obj_list[self.cont].split("/")
name, form = model[len(model) - 1].split(".")
print "Sending model by url: " + name + "." + form
else:
obj = open(obj_list[self.cont], "r").read()
name, form = obj_list[self.cont].split(".")
obj3D.obj = obj
print "Sending model by file: " + name + "." + form
pose3d = jderobot.Pose3DData()
pose3d.x = randrange(0, 20)
pose3d.y = randrange(0, 20)
pose3d.z = randrange(0, 20)
pose3d.h = uniform(0, 10)
pose3d.q0 = uniform(0, 1)
pose3d.q1 = uniform(0, 1 - pose3d.q0)
pose3d.q2 = uniform(0, 1 - pose3d.q0 - pose3d.q1)
pose3d.q3 = uniform(0, 1 - pose3d.q0 - pose3d.q1 - pose3d.q2)
id_list.append(id)
obj3D.id = id
obj3D.format = form
obj3D.pos = pose3d
obj3D.scale = uniform(0, 10)
self.cont = self.cont + 1
return obj3D
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 getTargets(self):
targets = []
for t in self.data["targets"]:
targets.append(
Target(t["name"],
jderobot.Pose3DData(t["x"], t["y"], 0, 0, 0, 0, 0, 0),
False, False))
return targets
def drawPose3d(self, obj_id, point, quaternion, h):
pose3d = jderobot.Pose3DData()
pose3d.x = float(point[0] / 100.0)
pose3d.y = float(point[2] / 100.0)
pose3d.z = float(point[1] / 100.0)
pose3d.h = h
pose3d.q0 = quaternion[0]
pose3d.q1 = quaternion[1]
pose3d.q2 = quaternion[2]
pose3d.q3 = quaternion[3]
getbufferPose3d(obj_id, pose3d)
def loadmarkers(self):
a = []
for line in open(self.filename, 'r').readlines():
line = line.rstrip('\n')
linelist = line.split()
if len(linelist) > 1:
pose = jderobot.Pose3DData()
pose.x = float(linelist[1])
pose.y = float(linelist[2])
pose.z = float(linelist[3])
a.append(pose)
self.map = list(a)
def getPoseObj3DData(self, current=None):
for i in id_list[:]:
pose3d = jderobot.Pose3DData()
pose3d.x = randrange(0, 20)
pose3d.y = randrange(0, 20)
pose3d.z = randrange(0, 20)
pose3d.h = randrange(-1, 1)
pose3d.q0 = uniform(0, 1)
pose3d.q1 = uniform(0, 1 - pose3d.q0)
pose3d.q2 = uniform(0, 1 - pose3d.q0 - pose3d.q1)
pose3d.q3 = uniform(0, 1 - pose3d.q0 - pose3d.q1 - pose3d.q2)
objpose3d = jderobot.PoseObj3D()
objpose3d.id = id_list[id_list.index(i)]
objpose3d.pos = pose3d
bufferpose3D.append(objpose3d)
return bufferpose3D
def getError(self):
if self.simpose3DProxy and self.pose3DProxy:
self.lock.acquire()
error = jderobot.Pose3DData()
error.x = self.simpose.x - self.pose.x
error.y = self.simpose.y - self.pose.y
error.z = self.simpose.z - self.pose.z
error.q0 = self.simpose.q0 - self.pose.q0
error.q1 = self.simpose.q1 - self.pose.q1
error.q2 = self.simpose.q2 - self.pose.q2
error.q3 = self.simpose.q3 - self.pose.q3
self.lock.release()
return error
else:
return None
def getPose3DData(self, current=None):
time.sleep(0.05) # 20Hz (50ms) rate to tx Pose3D
lock.acquire()
data = jderobot.Pose3DData()
data.x = self.x
data.y = self.y
data.z = self.z
data.h = self.h
data.q0 = self.q0
data.q1 = self.q1
data.q2 = self.q2
data.q3 = self.q3
lock.release()
return data
def reset(self):
''' Resets the environment state.
Sets a new goal point and a
new spawn point for the agent.'''
self.prevVW = {'v': 0, 'w': 0}
self.halt()
resetPose = jderobot.Pose3DData()
resetPose.x = -3.5
resetPose.y = -0.5
resetPose.z = 0
resetPose.yaw = 0
resetPose.roll = 0
resetPose.pitch = 0
self.pose3d.pose3d.proxy.setPose3DData(resetPose)
self.goal = goalstates.getRandomGoal(56)
print "GOAL: {}".format(self.goal)
self.state = self.getCurrentState()
# #print self.goal, self.state
return np.array(self.state)
def getPose3DData(self, current=None):
time.sleep(0.05) #50 ms rate to tx Pose3D
lock.acquire()
#print ('adquired G')
data = jderobot.Pose3DData()
data.x = self.x
data.y = self.y
data.z = self.z
data.h = self.h
data.q0 = self.q0
data.q1 = self.q1
data.q2 = self.q2
data.q3 = self.q3
#print ('released G')
lock.release()
return data
def __init__(self, port, baudrate):
'''control variables to identify if is real the measure'''
self.attitudeStatus = 0
self.altitudeStatus = 0
self.gpsStatus = 0
self.lastSentHeartbeat = 0
self.pose3D = jderobot.Pose3DData()
'''Conectar al Ardupilot'''
self.master = mavutil.mavlink_connection(port, baudrate, autoreconnect=True)
print('Connection established to device...')
self.master.wait_heartbeat()
print("Heartbeat Recieved")
T = threading.Thread(target=self.mavMsgHandler, args=(self.master))
print('Initiating server')
T.start()
PoseTheading = threading.Thread(target=self.openPose3DChannel(), name='Pose_Theading')
PoseTheading.daemon = True
PoseTheading.start()
def initBeacons(self):
self.beacons.append(
Beacon('baliza1', jderobot.Pose3DData(0, 5, 0, 0, 0, 0, 0, 0),
False, False))
self.beacons.append(
Beacon('baliza2', jderobot.Pose3DData(5, 0, 0, 0, 0, 0, 0, 0),
False, False))
self.beacons.append(
Beacon('baliza3', jderobot.Pose3DData(0, -5, 0, 0, 0, 0, 0, 0),
False, False))
self.beacons.append(
Beacon('baliza4', jderobot.Pose3DData(-5, 0, 0, 0, 0, 0, 0, 0),
False, False))
self.beacons.append(
Beacon('baliza5', jderobot.Pose3DData(10, 0, 0, 0, 0, 0, 0, 0),
False, False))
self.beacons.append(
Beacon('inicio', jderobot.Pose3DData(0, 0, 0, 0, 0, 0, 0, 0),
False, False))
def initBeacons(self):
self.beacons.append(
Beacon('goal1', jderobot.Pose3DData(0, 5, 0, 0, 0, 0, 0, 0), False,
False))
self.beacons.append(
Beacon('goal2', jderobot.Pose3DData(5, 0, 0, 0, 0, 0, 0, 0), False,
False))
self.beacons.append(
Beacon('goal3', jderobot.Pose3DData(0, -5, 0, 0, 0, 0, 0, 0),
False, False))
self.beacons.append(
Beacon('goal4', jderobot.Pose3DData(-5, 0, 0, 0, 0, 0, 0, 0),
False, False))
self.beacons.append(
Beacon('goal5', jderobot.Pose3DData(10, 0, 0, 0, 0, 0, 0, 0),
False, False))
self.beacons.append(
Beacon('start', jderobot.Pose3DData(0, 0, 0, 0, 0, 0, 0, 0), False,
False))
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.5
self.cmd.angularY = 1.0
try:
ic = Ice.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, 1
self.filterValues = ColorFilterValues()
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
targetCentreMetres = (0, 0)
landDecision = True
data = jderobot.CMDVelData()
if landDecision:
data.linearZ = -1 # Landing Velocity
else:
data.linearZ = 0
PH_CMDVel.setCMDVelData(data)
### send waypoint to server ###
targetCentreMetresDiff = frameChange2D(targetCentreMetres,
vehicleYaw)
wayPoint = jderobot.Pose3DData()
wayPoint.x = vehicleXYZ[0] + targetCentreMetres[0]
wayPoint.y = vehicleXYZ[1] + targetCentreMetres[1]
wayPoint.z = MissionHeight
WP_Pose3D.setPose3DData(wayPoint)
# print wayPoint
else:
print 'Searching for target'
# command, updatedTrajectory = nextWaypointCMDVel(xyz, trajectory)
updatedTrajectory = nextWaypointPose3D(vehicleXYZ, trajectory)
trajectory = updatedTrajectory
#restart trajectory when finish
def sendWP(self, send2APM):
'''HARDCODED
item = self.table.item(0,0)
pose = jderobot.Pose3DData()
pose.x = -35.362938
pose.y = 149.165085
pose.z = 0
pose.h = 500
pose.q0 = 0
pose.q1 = 0
pose.q2 = 0
pose.q3 = 0
'''
colCount = self.table.rowCount()
mission = jderobot.MissionData()
mission.mission = []
print(mission)
i = 0
for row in range(colCount):
pose = jderobot.Pose3DData(0, 0, 0, 0, 0, 0, 0, 0)
text = self.table.item(row, 0).text()
pos_lat = text.find("lat:")
pos_lon = text.find("lon:")
if pos_lat != -1:
print("WP")
lat = (text[pos_lat + 4:pos_lon])
pose.x = float(lat)
lon = (text[pos_lon + 4:])
pose.y = float(lon)
else:
if "LAND in " in text:
print("LAND")
pos = text.find("LAND in ") + 8
position = text[pos:].split(" ")
lat = position[0]
lon = position[1]
pose.x = float(lat)
pose.y = float(lon)
self.extra.land()
else:
pos = text.find("TAKE OFF to ") + 12
print("Take of detected")
position = text[pos:].split(" ")
lat = position[0]
lon = position[1]
pose.x = float(lat)
pose.y = float(lon)
self.extra.takeoff()
alt = self.table.item(row, 1)
pose.h = int(alt.text())
print(mission.mission)
mission.mission.append(pose)
# Workaround to avoid the fist waypoint un-understanded
if i == 0:
mission.mission.append(pose)
# end Workaround
i += 1
print(text + alt.text())
self.mission.setMissionData(mission)
#send the mission
print("sending... " + str(self.mission.getMissionData()))
MissionTheading = threading.Thread(target=ice_init.send_mission,
args=(mission_to_send, ),
name='Mission_Theading')
MissionTheading.daemon = True
MissionTheading.start()
