def check_home(self):
if self.home_initialised:
return True
if self.vehicle.gps_0.fix_type < 2:
print "Waiting for GPS .......", self.vehicle.gps_0.fix_type
return False
if self.vehicle.location.global_relative_frame is None:
print 'Waiting for vehicle position ...'
return False
if self.vehicle.location.global_relative_frame.lat is None or self.vehicle.location.global_relative_frame.lon is None or self.vehicle.location.global_relative_frame.alt is None:
print 'Waiting for vehicle position ...'
return False
if self.mission_cmds is None:
self.fetch_mission()
return False
if self.vehicle.home_location is None:
print 'Waiting for home location ...'
self.fetch_mission()
return False
PositionVector.set_home_location(
LocationGlobal(self.vehicle.home_location.lat,
self.vehicle.home_location.lon, 0))
self.home_initialised = True
print 'Home location acquired'
return self.home_initialised
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")
def main(self):
# set home to tridge's home field (absolute alt = 270)
PositionVector.set_home_location(
LocationGlobal(-35.362938, 149.165085, 0))
# calculate balloon position
fake_balloon_pos = PositionVector.get_from_location(
self.fake_balloon_location)
# vehicle attitude and position
veh_pos = PositionVector(0, 0, fake_balloon_pos.z) # at home location
veh_roll = math.radians(0) # leaned right 10 deg
veh_pitch = math.radians(0) # pitched back at 0 deg
veh_yaw = PositionVector.get_bearing(
veh_pos, fake_balloon_pos) # facing towards fake balloon
# display positions from home
print "Vehicle %s" % veh_pos
print "Balloon %s" % fake_balloon_pos
# generate simulated frame of balloon 10m north, 2m above vehicle
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
while (True):
# move vehicle towards balloon
veh_pos = veh_pos + (fake_balloon_pos - veh_pos) * 0.01
# regenerate frame
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch,
veh_yaw)
# look for balloon in image using blob detector
found_in_image, xpos, ypos, size = balloon_finder.analyse_frame(
img)
# display actual vs real distance
dist_actual = PositionVector.get_distance_xyz(
veh_pos, fake_balloon_pos)
dist_est = balloon_utils.get_distance_from_pixels(
size, balloon_finder.balloon_radius_expected)
print "Dist Est:%f Act:%f Size Est:%f Act:%f" % (
dist_est, dist_actual, size, self.last_balloon_radius)
# show image
cv2.imshow("fake balloon", img)
# wait for keypress
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
# destroy windows
cv2.destroyAllWindows()
def check_home(self):
# return immediately if home has already been initialised
if self.home_initialised:
return True
# check for home no more than once every two seconds
if (time.time() - self.last_home_check > 2):
# update that we have performed a status check
self.last_home_check = time.time()
# check if we have a vehicle
if self.vehicle is None:
self.vehicle = self.get_vehicle_with_dronekit()
return
# download the vehicle waypoints (and home) if we don't have them already
if self.mission_cmds is None:
self.fetch_mission()
return False
# ensure the vehicle's position is known
if self.vehicle.location.global_relative_frame is None:
print "waiting for vehicle position.."
return False
if self.vehicle.location.global_relative_frame.lat is None or self.vehicle.location.global_relative_frame.lon is None or self.vehicle.location.global_relative_frame.alt is None:
print "waiting for vehicle position.."
return False
# get home location from mission command list
if self.vehicle.home_location is None:
print "waiting for home location.."
self.fetch_mission()
return False
# sanity check home position
if self.vehicle.home_location.lat == 0 and self.vehicle.home_location.lon == 0:
print "home position all zero, re-fetching.."
self.fetch_mission()
return False
# set home position
PositionVector.set_home_location(
LocationGlobal(self.vehicle.home_location.lat,
self.vehicle.home_location.lon, 0))
self.home_initialised = True
print "got home location"
# To-Do: if we wish to have the same home position as the flight controller
# we must download the home waypoint again whenever the vehicle is armed
# return whether home has been initialised or not
return self.home_initialised
def check_home(self):
# return immediately if home has already been initialised
if self.home_initialised:
return True
# check for home no more than once every two seconds
if (time.time() - self.last_home_check > 2):
# update that we have performed a status check
self.last_home_check = time.time()
# check if we have a vehicle
if self.vehicle is None:
self.vehicle = self.get_vehicle_with_dronekit()
return
# download the vehicle waypoints (and home) if we don't have them already
if self.mission_cmds is None:
self.fetch_mission()
return False
# ensure the vehicle's position is known
if self.vehicle.location.global_relative_frame is None:
print "waiting for vehicle position.."
return False
if self.vehicle.location.global_relative_frame.lat is None or self.vehicle.location.global_relative_frame.lon is None or self.vehicle.location.global_relative_frame.alt is None:
print "waiting for vehicle position.."
return False
# get home location from mission command list
if self.vehicle.home_location is None:
print "waiting for home location.."
self.fetch_mission()
return False
# sanity check home position
if self.vehicle.home_location.lat == 0 and self.vehicle.home_location.lon == 0:
print "home position all zero, re-fetching.."
self.fetch_mission()
return False
# set home position
PositionVector.set_home_location(LocationGlobal(self.vehicle.home_location.lat,self.vehicle.home_location.lon,0))
self.home_initialised = True
print "got home location"
# To-Do: if we wish to have the same home position as the flight controller
# we must download the home waypoint again whenever the vehicle is armed
# return whether home has been initialised or not
return self.home_initialised
def check_home(self):
# return immediately if home has already been initialised
if self.home_initialised:
return True
# check for home no more than once every two seconds
if (time.time() - self.last_home_check > 2):
# update that we have performed a status check
self.last_home_check = time.time()
# check if we have a vehicle
if self.vehicle is None:
self.vehicle = self.api.get_vehicles()[0]
return
# ensure the vehicle's position is known
if self.vehicle.location is None:
return False
if self.vehicle.location.lat is None or self.vehicle.location.lon is None or self.vehicle.location.alt is None:
return False
# download the vehicle waypoints if we don't have them already
if self.mission_cmds is None:
self.fetch_mission()
return False
# get the home lat and lon
home_lat = self.mission_cmds[0].x
home_lon = self.mission_cmds[0].y
# sanity check the home position
if home_lat is None or home_lon is None:
return False
# sanity check again and set home position
if home_lat <> 0 and home_lon <> 0:
PositionVector.set_home_location(
Location(home_lat, home_lon, 0))
self.home_initialised = True
else:
self.mission_cmds = None
# To-Do: if we wish to have the same home position as the flight controller
# we must download the home waypoint again whenever the vehicle is armed
# return whether home has been initialised or not
return self.home_initialised
def check_home(self):
# return immediately if home has already been initialised
if self.home_initialised:
return True
# check for home no more than once every two seconds
if (time.time() - self.last_home_check > 2):
# update that we have performed a status check
self.last_home_check = time.time()
# check if we have a vehicle
if self.vehicle is None:
self.vehicle = self.api.get_vehicles()[0]
return
# ensure the vehicle's position is known
if self.vehicle.location is None:
return False
if self.vehicle.location.lat is None or self.vehicle.location.lon is None or self.vehicle.location.alt is None:
return False
# download the vehicle waypoints if we don't have them already
if self.mission_cmds is None:
self.fetch_mission()
return False
# get the home lat and lon
home_lat = self.mission_cmds[0].x
home_lon = self.mission_cmds[0].y
# sanity check the home position
if home_lat is None or home_lon is None:
return False
# sanity check again and set home position
if home_lat <> 0 and home_lon <> 0:
PositionVector.set_home_location(Location(home_lat,home_lon,0))
self.home_initialised = True
else:
self.mission_cmds = None
# To-Do: if we wish to have the same home position as the flight controller
# we must download the home waypoint again whenever the vehicle is armed
# return whether home has been initialised or not
return self.home_initialised
def main(self):
# set home to tridge's home field (absolute alt = 270)
PositionVector.set_home_location(Location(-35.362938,149.165085,0))
# calculate balloon position
fake_balloon_pos = PositionVector.get_from_location(self.fake_balloon_location)
# vehicle attitude and position
veh_pos = PositionVector(0,0,fake_balloon_pos.z) # at home location
veh_roll = math.radians(0) # leaned right 10 deg
veh_pitch = math.radians(0) # pitched back at 0 deg
veh_yaw = PositionVector.get_bearing(veh_pos,fake_balloon_pos) # facing towards fake balloon
# display positions from home
print "Vehicle %s" % veh_pos
print "Balloon %s" % fake_balloon_pos
# generate simulated frame of balloon 10m north, 2m above vehicle
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
while(True):
# move vehicle towards balloon
veh_pos = veh_pos + (fake_balloon_pos - veh_pos) * 0.01
# regenerate frame
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
# look for balloon in image using blob detector
found_in_image, xpos, ypos, size = balloon_finder.analyse_frame(img)
# display actual vs real distance
dist_actual = PositionVector.get_distance_xyz(veh_pos, fake_balloon_pos)
dist_est = balloon_utils.get_distance_from_pixels(size, balloon_finder.balloon_radius_expected)
print "Dist Est:%f Act:%f Size Est:%f Act:%f" % (dist_est, dist_actual, size, self.last_balloon_radius)
# show image
cv2.imshow("fake balloon", img)
# wait for keypress
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
# destroy windows
cv2.destroyAllWindows()
if not args.connect:
print "vehicle path not specified"
sys.exit(1) #abort cause of error..
# Connect to the Vehicle.
# 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),
while not vehicle.armed:
print " Waiting for arming..."
time.sleep(1)
print " Vehicle is armed: %s" % vehicle.armed
#get Vehicle Home location
while not vehicle.home_location:
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
if not vehicle.home_location:
print " Waiting for home location ..."
#Set origin point to home
PositionVector.set_home_location(vehicle.home_location)
#Read waypoints from mission file
waypoints = mission_to_locations(read_mission("../thunderhill.mission"))
#navigate waypoints in manual mode
DIST_THRESH = 4 #distance to waypoint before moving onto next waypoint
P_TURN = 1 #p term of navigation controller
SPEED = 100 #throttle level
wp_cnt = 1
for wp in waypoints:
#convert global lat/lon local meters(position vectors)
wp_posvec = PositionVector.get_from_location(wp)
veh_posvec = PositionVector.get_from_location(vehicle.location.global_relative_frame)
dist = PositionVector.get_distance_xy(veh_posvec,wp_posvec)
vehicle.armed = True
while not vehicle.armed:
print " Waiting for arming..."
time.sleep(1)
print " Vehicle is armed: %s" % vehicle.armed
#get Vehicle Home location
while not vehicle.home_location:
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
if not vehicle.home_location:
print " Waiting for home location ..."
#Set origin point to home
PositionVector.set_home_location(vehicle.home_location)
#Read waypoints from mission file
waypoints = mission_to_locations(read_mission("thunderhill.mission"))
#navigate waypoints in manual mode
DIST_THRESH = 4 #distance to waypoint before moving onto next waypoint
P_TURN = 1 #p term of navigation controller
SPEED = 100 #throttle level
wp_cnt = 1
for wp in waypoints:
#convert global lat/lon local meters(position vectors)
wp_posvec = PositionVector.get_from_location(wp)
veh_posvec = PositionVector.get_from_location(
vehicle.location.global_relative_frame)
