def run(self): while True: if self.vehicle.mode.name !="GUIDED": #if we are no longer in guided mode stop receving messages logging.info("follower vehicle aborted GUIDED mode") self.recv_thread.stop() break try: (message,address) = self.msg_queue.get() self.haversine_distance((message.lat,message.lon),(self.vehicle.location.global_relative_frame.lat,self.vehicle.location.global_relative_frame.lon)) if not self.called: #set home location to home location of master vehicle PositionVector.set_home_location(LocationGlobal(message.home_location.lat,message.home_location.lon,message.home_location.alt)) logging.debug("master home location is...") logging.debug(message.home_location) self.called = True logging.debug("message received %s %s %s" %(message.lat, message.lon,message.alt)) #altitude = 5 #in metres separation = self.distance #distance between drones original_pos = PositionVector.get_from_location(message) logging.debug("position for leader vehicle -> %s heading -> %s" %(original_pos,message.heading)) logging.debug("leader vehicle groundspeed is %.3f" %message.groundspeed) logging.debug("leader vehicle lidar altitude is %.4f m"%message.rangefinder) new_pos = selection_coding(original_pos,message.heading,separation) new_pos = PositionVector(new_pos[0],new_pos[1],new_pos[2]) logging.debug("position for follower vehicle is %s" %(new_pos)) lat = new_pos.get_location().lat lon = new_pos.get_location().lon alt = new_pos.get_location().alt - message.home_location.alt dest = LocationGlobalRelative(lat,lon,alt) logging.debug("gps locationglobal data for follower vehicle to procced to is %s" %new_pos.get_location()) logging.debug("gps locationglobalrelative data for follower vehicle to procced to is %s" %dest) self.vehicle.simple_goto(dest) set_yaw(self.vehicle,message.heading) logging.debug("vehicle groundspeed is %.3f" %self.vehicle.groundspeed) logging.debug("vehicle heading is %.3f" %self.vehicle.heading) logging.debug("vehicle lidar altitude is %.4f m "%self.vehicle.rangefinder.distance) actualpath = PositionVector.get_from_location(self.vehicle.location.global_frame) logging.debug("vehicle actual path in X direction is %.5f m" %actualpath.x) logging.debug("vehicle actual path in Y direction is %.5f m" %actualpath.y) time.sleep(0.1) logging.debug("**************") self.count+=1 self.msg_queue.task_done() except Queue.Empty: print ("all messages processed")
# Set `wait_ready=True` to ensure default attributes are populated before `connect()` return print "\nConnecting to vehicle on: %s" % connection_string vehicle = connect(connection_string, wait_ready=True, baud=args.baudrate) v = Vehicle_params() param = Params(vehicle, v) #get home position ... home_position = v.home_location logging.info("home position is %s", home_position) PositionVector.set_home_location( LocationGlobal(home_position.lat, home_position.lon, home_position.alt)) arm_and_takeoff(vehicle, 5) vehicle.simple_goto(pos1.get_location()) #set_yaw(vehicle,0) #set yaw while True: pos = PositionVector.get_from_location(vehicle.location.global_frame) logging.debug("vehicle_pos in N-S direction from home is %f" % pos.x) logging.debug("vehicle_pos in W-E direction from home is %f" % pos.y) print "distance from vehicle to pos1 is %s" % PositionVector.get_distance_xy( PositionVector.get_from_location(vehicle.location.global_frame), pos1) logging.debug( "distance from vehicle to pos1 is %s" % PositionVector.get_distance_xy( PositionVector.get_from_location(vehicle.location.global_frame), pos1)) time.sleep(0.5) if PositionVector.get_distance_xy( PositionVector.get_from_location(vehicle.location.global_frame),
def get_location(self,timestamp): pnt = self._interpolate(self.location_buffer,timestamp) if pnt is not None: pos = PositionVector(pnt.x, pnt.y, pnt.z) return pos.get_location() return None
def get_location(self, timestamp): pnt = self._interpolate(self.location_buffer, timestamp) if pnt is not None: pos = PositionVector(pnt.x, pnt.y, pnt.z) return pos.get_location() return None