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