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
