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