def inside_landing_area(self): return 1 ''' vehPos = PositionVector.get_from_location(veh_control.get_location()) landPos = PositionVector.get_from_location(veh_control.get_landing()) ''' vehPos = PositionVector.get_from_location(Location(0,0,10)) landPos = PositionVector.get_from_location(Location(0,0,0)) '''
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 get_frame(self):
if self.use_simulator:
veh_pos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame)
frame = balloon_sim.get_simulated_frame(veh_pos, self.vehicle.attitude.roll, self.vehicle.attitude.pitch, self.vehicle.attitude.yaw)
else:
frame = balloon_video.get_image()
return frame
def get_frame(self):
if self.use_simulator:
veh_pos = PositionVector.get_from_location(self.vehicle.location)
frame = balloon_sim.get_simulated_frame(veh_pos, self.vehicle.attitude.roll, self.vehicle.attitude.pitch, self.vehicle.attitude.yaw)
else:
frame = balloon_video.get_image()
return frame
def analisis_image(self):
# record time
now = time.time()
#PositionVectors = PositionVector()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(
self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
#create an image processor
if self.mission_on_guide_mode == 4:
self.fire_found, xpos, ypos, size = fire_finder.filter(f)
#result = detector.analyze_frame(f)
self.attitude = self.get_attitude()
if self.fire_found == True:
self.b = 0
sc_logger.text(sc_logger.GENERAL, "Target terdeteksi!")
print xpos, ypos, size
if self.vehicle.mode.name == "GUIDED":
sc_logger.text(sc_logger.GENERAL,
"Target terdeteksi, mendekati target!")
self.move_to_target_fire(xpos, ypos, size, self.attitude,
self.vehicle_pos)
self.relay1(1) #lampu indikator on
#self.servo(0)#test untuk servo kamera
elif self.vehicle.mode.name == "GUIDED":
self.relay1(0) #lampu indikator off
#self.servo(1)#test untuk servo kamera
self.b += 1
if self.b > 25: #15
self.b = 0
#print "Api tidak terdeteksi, ubah mode ke AUTO"
#self.lanjut_cmd += 1
sc_logger.text(
sc_logger.GENERAL,
"Target tidak terdeteksi, ubah mode ke AUTO")
self.a = False
self.waktu1 = 0
#self.c = True
self.controlling_vehicle = False
self.vehicle.mode = VehicleMode("AUTO")
self.vehicle.flush()
#rend_Image = detector.add_target_highlights(f, result[3])
sc_logger.image(sc_logger.GUI, f)
if not self.writer is None:
# save image for debugging later
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()
def get_simulated_frame(self, veh_pos, vehicle_roll, vehicle_pitch, vehicle_yaw):
# get balloon position
balloon_pos = PositionVector.get_from_location(self.fake_balloon_location)
# get background
sim_frame = self.get_background(vehicle_roll, vehicle_pitch)
# draw balloon on background
self.draw_fake_balloon(sim_frame, veh_pos, balloon_pos, vehicle_roll, vehicle_pitch, vehicle_yaw)
return sim_frame
def inside_landing_area(self): vehPos = PositionVector.get_from_location(veh_control.get_location()) landPos = PositionVector.get_from_location(veh_control.get_landing()) ''' vehPos = PositionVector.get_from_location(Location(0,0,10)) landPos = PositionVector.get_from_location(Location(0,0,0)) ''' if(PositionVector.get_distance_xy(vehPos,landPos) < self.landing_area_radius): #below area if(vehPos.z < self.landing_area_min_alt): return -1 #in area else: return 1 #outside area else: return 0
def inside_landing_area(self):
vehPos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame)
landPos = PositionVector.get_from_location(PositionVector.get_home_location())
'''
vehPos = PositionVector.get_from_location(Location(0,0,10))
landPos = PositionVector.get_from_location(Location(0,0,0))
'''
if(PositionVector.get_distance_xy(vehPos,landPos) < self.landing_area_radius):
#below area
if(vehPos.z < self.landing_area_min_alt):
print "VehPos.Z: %f " % (vehPos.z)
return 1
#in area
else:
return 1
#outside area
else:
return 0
def inside_landing_area(self):
vehPos = PositionVector.get_from_location(veh_control.get_location())
landPos = PositionVector.get_from_location(veh_control.get_landing())
'''
vehPos = PositionVector.get_from_location(Location(0,0,10))
landPos = PositionVector.get_from_location(Location(0,0,0))
'''
if (PositionVector.get_distance_xy(vehPos, landPos) <
self.landing_area_radius):
#below area
if (vehPos.z < self.landing_area_min_alt):
return -1
#in area
else:
return 1
#outside area
else:
return 0
def analyze_image(self):
# record time
now = time.time()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(
self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
# look for balloon in image using blob detector
self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)
# add artificial horizon
balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll,
veh_att_delayed.pitch)
if self.balloon_found:
# record time balloon was found
self.last_spotted_time = now
# convert x, y position to pitch and yaw direction (in radians)
self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(
xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch,
veh_att_delayed.yaw)
self.balloon_pitch = math.radians(self.balloon_pitch)
self.balloon_pitch_top = self.balloon_pitch + balloon_video.pixels_to_angle_y(
size) # add balloon radius so we aim for top of balloon
self.balloon_heading = math.radians(self.balloon_heading)
# get distance
self.balloon_distance = get_distance_from_pixels(
size, balloon_finder.balloon_radius_expected)
# updated estimated balloon position
self.balloon_pos = balloon_finder.project_position(
self.vehicle_pos, self.balloon_pitch, self.balloon_heading,
self.balloon_distance)
# save image for debugging later
if not self.writer is None:
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()
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 analyze_image(self):
# record time
now = time.time()
# capture vehicle position
self.vehicle_pos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame)
# capture vehicle attitude in buffer
self.att_hist.update()
# get delayed attitude from buffer
veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)
# get new image from camera
f = self.get_frame()
# look for balloon in image using blob detector
self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)
# add artificial horizon
balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll, veh_att_delayed.pitch)
if self.balloon_found:
# record time balloon was found
self.last_spotted_time = now
# convert x, y position to pitch and yaw direction (in radians)
self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch, veh_att_delayed.yaw)
self.balloon_pitch = math.radians(self.balloon_pitch)
self.balloon_pitch_top = self.balloon_pitch + balloon_video.pixels_to_angle_y(size) # add balloon radius so we aim for top of balloon
self.balloon_heading = math.radians(self.balloon_heading)
# get distance
self.balloon_distance = get_distance_from_pixels(size, balloon_finder.balloon_radius_expected)
# updated estimated balloon position
self.balloon_pos = balloon_finder.project_position(self.vehicle_pos, self.balloon_pitch, self.balloon_heading, self.balloon_distance)
# save image for debugging later
if not self.writer is None:
self.writer.write(f)
# increment frames analysed for stats
self.num_frames_analysed += 1
if self.stats_start_time == 0:
self.stats_start_time = time.time()
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()
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),
pos1) <= dist_tol:
print vehicle.location.global_relative_frame
print("reached........hurrrayyyyyyy")
logging.debug("vehicle reached position1")
#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)
while dist > DIST_THRESH:
#calculate target_heading and heading error
target_heading = math.degrees(PositionVector.get_bearing(veh_posvec,wp_posvec))
error = target_heading - vehicle.heading
#remap error from 0 to 360 -> -180 to 180
if error > 180:
error = error - 360
if error < -180:
error = 360 + error
print "going to wp {}: {} meters, {} error".format(wp_cnt,dist,error)
#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)
while dist > DIST_THRESH:
#calculate target_heading and heading error
target_heading = math.degrees(
PositionVector.get_bearing(veh_posvec, wp_posvec))
error = target_heading - vehicle.heading
#remap error from 0 to 360 -> -180 to 180
if error > 180:
error = error - 360
if error < -180:
error = 360 + error
