def image_capture_background(self, imgcap_connection): # exit immediately if imgcap_connection is invalid if imgcap_connection is None: sc_logger.text(sc_logger.GENERAL, "image_capture failed because pipe is uninitialised") return # clear latest image latest_image = None while True: # constantly get the image from the webcam success_flag, image=self.camera.read() # if successful overwrite our latest image if success_flag: latest_image = image # check if the parent wants the image if imgcap_connection.poll(): recv_obj = imgcap_connection.recv() # if -1 is received we exit if recv_obj == -1: break # otherwise we return the latest image imgcap_connection.send(latest_image) # release camera when exiting self.camera.release()
def image_capture_background(self, imgcap_connection): # exit immediately if imgcap_connection is invalid if imgcap_connection is None: sc_logger.text( sc_logger.GENERAL, "image_capture failed because pipe is uninitialised") return # clear latest image latest_image = None while True: # constantly get the image from the webcam success_flag, image = self.camera.read() # if successful overwrite our latest image if success_flag: latest_image = image # check if the parent wants the image if imgcap_connection.poll(): recv_obj = imgcap_connection.recv() # if -1 is received we exit if recv_obj == -1: break # otherwise we return the latest image imgcap_connection.send(latest_image) # release camera when exiting self.camera.release()
def set_velocity(self, velocity_x, velocity_y, velocity_z): #only let commands through at 10hz if (time.time() - self.last_set_velocity) > self.vel_update_rate: self.last_set_velocity = time.time() # create the SET_POSITION_TARGET_LOCAL_NED command msg = self.vehicle.message_factory.set_position_target_local_ned_encode( 0, # time_boot_ms (not used) 0, 0, # target system, target component mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame 0x01C7, # type_mask (ignore pos | ignore acc) 0, 0, 0, # x, y, z positions (not used) velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s 0, 0, 0, # x, y, z acceleration (not used) 0, 0) # yaw, yaw_rate (not used) # send command to vehicle self.vehicle.send_mavlink(msg) self.vehicle.flush() sc_logger.text( sc_logger.AIRCRAFT, 'Sent Vx: {0}, Vy: {1}, Vz: {2}'.format( velocity_x, velocity_y, velocity_z))
def retreive(self): if self.is_available(): #grab results results = self.parent_conn.recv() #update processTime. All processes must return a runtime through the pipe self.processTime, self.processTimeSet = self.rolling_average( self.processTimeSet, results[0]) #Calculate real runtime. Diagnostic purposes only #In an ideal system the dispatch rate would equal the retreival rate. That is not the case here actualRunTime = current_milli_time() - self.lastRetreival self.lastRetreival = current_milli_time() sc_logger.text( sc_logger.PERFORMANCE, "DispatchHz: {0} runHz: {1} ProcessHz: {2} CaptureHz: {3} Processes: {4} " .format(round(1000.0 / (self.runTime)), round(1000.0 / actualRunTime), round((1000.0 / results[0])), round(1000.0 / self.captureTime), len(multiprocessing.active_children()))) return results return None
def move_to_target_cam_forward(self,target_info,attitude,location): x,y = target_info[1] pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw) yaw_dir = yaw_dir % 360 print "Target Yaw:%f" %(yaw_dir) target_distance = target_info[2]*10 print "Target Distance:%f" %(target_distance) #shift origin to center of image print "Found Target at yaw:%f heading:%f Alt:%f Dist:%f" % (attitude.yaw,yaw_dir,location.z,target_distance) sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2))) print "mode detection" #self.condition_yaw(math.fabs(math.degrees(target_heading))) if(target_distance > 4): vx = 0 vy = 0 vz = 0 else: vx = 0 vy = 0 vz = 0 #send velocity commands toward target heading self.send_nav_velocity(vx,vy,vz) self.condition_yaw(yaw_dir)
def get_home(self, wait_for_arm=False): #wait unitl we are armed to grab the INITIAL home position. This will lock up the program. if (wait_for_arm and self.last_home is None): sc_logger.text( sc_logger.GENERAL, 'Waiting for intial home lock: Requires armed status....') while (self.vehicle.armed == False): time.sleep(0.3) sc_logger.text(sc_logger.GENERAL, 'Got valid intial home location') if (time.time() - self.last_home_call > self.home_update_rate): self.last_home_call = time.time() # download the vehicle waypoints mission_cmds = self.vehicle.commands mission_cmds.download() mission_cmds.wait_valid() # get the home lat and lon home_lat = mission_cmds[0].x home_lon = mission_cmds[0].y self.last_home = Location(home_lat, home_lon, 0) return self.last_home
def release_control(self): sc_logger.text(sc_logger.GENERAL, 'Releasing control') #put vehicle in stable state veh_control.set_velocity(0, 0, 0) #autopilot_land() #dont let us take control ever again self.pl_enabled = False '''
def climb(self): if (veh_control.get_location().alt < self.climb_altitude): sc_logger.text(sc_logger.GENERAL, 'climbing') veh_control.set_velocity(0, 0, self.climb_rate) self.climbing = True else: sc_logger.text(sc_logger.GENERAL, 'Reached top of search zone') veh_control.set_velocity(0, 0, 0) self.climbing = False
def climb(self): if(veh_control.get_location().alt < self.climb_altitude): sc_logger.text(sc_logger.GENERAL, 'climbing') veh_control.set_velocity(0,0,self.climb_rate) self.climbing = True else: sc_logger.text(sc_logger.GENERAL, 'Reached top of search zone') veh_control.set_velocity(0,0,0) self.climbing = False
def release_control(self): sc_logger.text(sc_logger.GENERAL, 'Releasing control') #put vehicle in stable state veh_control.set_velocity(0,0,0) #autopilot_land() #dont let us take control ever again self.pl_enabled = False '''
def get_camera(self,index): # setup video capture self.camera = cv2.VideoCapture(index) self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,self.img_width) self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,self.img_height) # check we can connect to camera if not self.camera.isOpened(): sc_logger.text(sc_logger.GENERAL,"failed to open camera, exiting!") sys.exit(0) return self.camera
def run(self): sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name())) #start a video capture sc_video.start_capture(self.camera_index) #create an image processor detector = CircleDetector() #create a queue for images imageQueue = Queue.Queue() while True: #update how often we dispatch a command sc_dispatcher.calculate_dispatch_schedule() # grab an image capStart = current_milli_time() frame = sc_video.get_image() capStop = current_milli_time() #update capture time sc_dispatcher.update_capture_time(capStop-capStart) #Process image #We schedule the process as opposed to waiting for an available core #This brings consistancy and prevents overwriting a dead process before #information has been grabbed from the Pipe if sc_dispatcher.is_ready(): #queue the image for later use: displaying image, overlays, recording imageQueue.put(frame) #the function must be run directly from the class sc_dispatcher.dispatch(target=detector.analyze_frame, args=(frame,None,)) #retreive results if sc_dispatcher.is_available(): #results of image processor results = sc_dispatcher.retreive() # get image that was passed with the image processor img = imageQueue.get() #overlay gui rend_Image = gui.add_target_highlights(img, results[3]) #show/record images sc_logger.image(sc_logger.RAW, img) sc_logger.image(sc_logger.GUI, rend_Image)
def get_camera(self, index): # setup video capture self.camera = cv2.VideoCapture(index) self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, self.img_width) self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, self.img_height) # check we can connect to camera if not self.camera.isOpened(): sc_logger.text(sc_logger.GENERAL, "failed to open camera, exiting!") sys.exit(0) return self.camera
def move_to_target(self,target_info,attitude,location): x,y = target_info[1] #shift origin to center of image x,y = shift_to_origin((x,y),self.camera_width,self.camera_height) #this is necessary because the simulator is 100% accurate if(self.simulator): hfov = 48.7 vfov = 49.7 else: hfov = self.camera_hfov vfov = self.camera_vfov #stabilize image with vehicle attitude x -= (self.camera_width / hfov) * math.degrees(attitude.roll) y += (self.camera_height / vfov) * math.degrees(attitude.pitch) #convert to distance X, Y = self.pixel_point_to_position_xy((x,y),location.alt) #convert to world coordinates target_heading = math.atan2(Y,X) % (2*math.pi) target_heading = (attitude.yaw - target_heading) target_distance = math.sqrt(X**2 + Y**2) sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2))) #calculate speed toward target speed = target_distance * self.dist_to_vel #apply max speed limit speed = min(speed,self.vel_speed_max) #calculate cartisian speed vx = speed * math.sin(target_heading) * -1.0 vy = speed * math.cos(target_heading) #only descend when on top of target if(target_distance > self.descent_radius): vz = 0 else: vz = self.descent_rate #send velocity commands toward target heading veh_control.set_velocity(vx,vy,vz)
def move_to_target(self, target_info, attitude, location): x, y = target_info[1] #pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw) #yaw_dir = yaw_dir % 360 #print "Target Yaw:%f" %(yaw_dir) #target_distance = target_info[2] #print "Target Distance:%f" %(target_distance*100) #shift origin to center of image x, y = shift_to_origin((x, y), self.camera_width, self.camera_height) hfov = self.camera_hfov vfov = self.camera_vfov #stabilize image with vehicle attitude x -= (self.camera_width / hfov) * math.degrees(attitude.roll) y += (self.camera_height / vfov) * math.degrees(attitude.pitch) #convert to distance X, Y = self.pixel_point_to_position_xy((x, y), location.z) #convert to world coordinates target_headings = math.atan2(Y, X) #% (2*math.pi) target_heading = (attitude.yaw - target_headings) target_distance = math.sqrt(X**2 + Y**2) sc_logger.text( sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance, 2))) #calculate speed toward target speed = target_distance * self.dist_to_vel #apply max speed limit speed = min(speed, self.vel_speed_max) #calculate cartisian speed vx = speed * math.sin(target_heading) * -1.0 vy = speed * math.cos(target_heading) print "Found Target go to vx:%f vy:%f Alt:%f target_distance:%f" % ( vx, vy, location.z, target_distance) #only descend when on top of target if (target_distance > self.descent_radius): vz = 0 else: vz = self.descent_rate #send velocity commands toward target heading self.send_nav_velocity(vx, vy, vz)
def check_video_out(self): # return immediately if video has already been started if not self.writer is None: return #self.writer = balloon_video.open_video_writer() # start video once vehicle is armed if self.vehicle.armed and self.arm: sc_logger.text(sc_logger.GENERAL, "armed") self.arm = False #self.writer = balloon_video.open_video_writer() if not self.vehicle.armed and not self.arm: sc_logger.text(sc_logger.GENERAL, "disarm") self.arm = True
def move_to_target(self, target_info, attitude, location): x, y = target_info[1] #shift origin to center of image x, y = shift_to_origin((x, y), self.camera_width, self.camera_height) #this is necessary because the simulator is 100% accurate if (self.simulator): hfov = 48.7 vfov = 49.7 else: hfov = self.camera_hfov vfov = self.camera_vfov #stabilize image with vehicle attitude x -= (self.camera_width / hfov) * math.degrees(attitude.roll) y += (self.camera_height / vfov) * math.degrees(attitude.pitch) #convert to distance X, Y = self.pixel_point_to_position_xy((x, y), location.alt) #convert to world coordinates target_heading = math.atan2(Y, X) % (2 * math.pi) target_heading = (attitude.yaw - target_heading) target_distance = math.sqrt(X**2 + Y**2) sc_logger.text( sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance, 2))) #calculate speed toward target speed = target_distance * self.dist_to_vel #apply max speed limit speed = min(speed, self.vel_speed_max) #calculate cartisian speed vx = speed * math.sin(target_heading) * -1.0 vy = speed * math.cos(target_heading) #only descend when on top of target if (target_distance > self.descent_radius): vz = 0 else: vz = self.descent_rate #send velocity commands toward target heading veh_control.set_velocity(vx, vy, vz)
def retreive(self): if self.is_available(): #grab results results = self.parent_conn.recv() #update processTime. All processes must return a runtime through the pipe self.processTime, self.processTimeSet = self.rolling_average(self.processTimeSet, results[0]) #Calculate real runtime. Diagnostic purposes only #In an ideal system the dispatch rate would equal the retreival rate. That is not the case here actualRunTime = current_milli_time() - self.lastRetreival self.lastRetreival = current_milli_time() sc_logger.text(sc_logger.PERFORMANCE, "DispatchHz: {0} runHz: {1} ProcessHz: {2} CaptureHz: {3} Processes: {4} ".format(round(1000.0/(self.runTime)), round(1000.0/actualRunTime), round((1000.0/results[0])),round(1000.0/self.captureTime), len(multiprocessing.active_children()))) return results return None
def set_velocity(self, velocity_x, velocity_y, velocity_z): #only let commands through at 10hz if(time.time() - self.last_set_velocity) > self.vel_update_rate: self.last_set_velocity = time.time() # create the SET_POSITION_TARGET_LOCAL_NED command msg = self.vehicle.message_factory.set_position_target_local_ned_encode( 0, # time_boot_ms (not used) 0, 0, # target system, target component mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame 0x01C7, # type_mask (ignore pos | ignore acc) 0, 0, 0, # x, y, z positions (not used) velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s 0, 0, 0, # x, y, z acceleration (not used) 0, 0) # yaw, yaw_rate (not used) # send command to vehicle self.vehicle.send_mavlink(msg) self.vehicle.flush() sc_logger.text(sc_logger.AIRCRAFT, 'Sent Vx: {0}, Vy: {1}, Vz: {2}'.format(velocity_x,velocity_y,velocity_z))
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_home(self, wait_for_arm = False): #wait unitl we are armed to grab the INITIAL home position. This will lock up the program. if(wait_for_arm and self.last_home is None): sc_logger.text(sc_logger.GENERAL, 'Waiting for intial home lock: Requires armed status....') while(self.vehicle.armed == False): time.sleep(0.3) sc_logger.text(sc_logger.GENERAL, 'Got valid intial home location') if(time.time() - self.last_home_call > self.home_update_rate): self.last_home_call = time.time() # download the vehicle waypoints mission_cmds = self.vehicle.commands mission_cmds.download() mission_cmds.wait_valid() # get the home lat and lon home_lat = mission_cmds[0].x home_lon = mission_cmds[0].y self.last_home = Location(home_lat,home_lon,0) return self.last_home
def run(self): sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
def check_status(self): # Reset lanjut_cmd if (self.vehicle.mode.name == "LOITER" or self.vehicle.mode.name == "STABILIZE") and self.lanjut_cmd > 1: sc_logger.text(sc_logger.GENERAL, "Resetting Mission") self.c = False self.drop = 0 self.relay(0) self.lanjut_cmd = 1 # download the vehicle waypoints if we don't have them already # To-Do: do not load waypoints if vehicle is armed if self.mission_cmds is None: self.fetch_mission() return # Check for Auto mode and executing Nav-Guided command if self.vehicle.mode.name == "AUTO": self.relay1(0) #lampu indikator off # get active command number active_command = self.vehicle.commands.next #print "Active Command dan Lanjut Command adalah %s, %s" % (active_command,self.lanjut_cmd) sc_logger.text(sc_logger.GENERAL, "Active dan Lanjut Command: %s, %s" % (active_command, self.lanjut_cmd)) # Buat ubah ke GUIDED if active_command == 1 and self.lanjut_cmd == 1: sc_logger.text(sc_logger.GENERAL, "Taking Off!!!") self.lanjut_cmd = 3 if (active_command < self.vehicle.commands.count) and (active_command == self.lanjut_cmd): # WP cek api self.lanjut_cmd += 1 sc_logger.text(sc_logger.GENERAL, "ON TARGET, ubah Mode ke GUIDED") self.c = True self.a = True self.waktu1 = 0 self.vehicle.mode = VehicleMode("GUIDED") self.vehicle.flush() if active_command == self.vehicle.commands.count: sc_logger.text(sc_logger.GENERAL, "Misi selesai, Mode RTL dan LAND") self.vehicle.mode = VehicleMode("RTL") self.vehicle.flush() # we are active in guided mode if self.vehicle.mode.name == "GUIDED": if not self.controlling_vehicle: self.controlling_vehicle = True print "taking control of vehicle in GUIDED" print "Mulai cari Target" sc_logger.text(sc_logger.GENERAL, "taking control of vehicle in GUIDED dan mulai cari Target") return # if we got here then we are not in control if self.controlling_vehicle: self.controlling_vehicle = False print "giving up control of vehicle in %s" % self.vehicle.mode.name
def move_to_target_fire(self,x,y,size,attitude,location): # exit immediately if we are not controlling the vehicle if not self.controlling_vehicle: return # get active command active_command = self.vehicle.commands.next # get current time now = time.time() # exit immediately if it's been too soon since the last update if (now - self.guided_last_update) < self.vel_update_rate: return; # if we have a new balloon position recalculate velocity vector if (self.fire_found): #x,y = target_info[1] #pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw) #yaw_dir = yaw_dir % 360 #print "Target Yaw:%f" %(yaw_dir) #target_distance = target_info[2] #print "Target Distance:%f" %(target_distance*100) #shift origin to center of image x,y = shift_to_origin((x,y),self.camera_width,self.camera_height) hfov = self.camera_hfov vfov = self.camera_vfov #stabilize image with vehicle attitude x -= (self.camera_width / hfov) * math.degrees(attitude.roll) y += (self.camera_height / vfov) * math.degrees(attitude.pitch) #convert to distance X, Y = self.pixel_point_to_position_xy((x,y),location.z) #convert to world coordinates target_headings = math.atan2(Y,X) #% (2*math.pi) target_heading = (attitude.yaw - target_headings) target_distance = math.sqrt(X**2 + Y**2) #sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2))) #calculate speed toward target speed = target_distance * self.dist_to_vel #apply max speed limit speed = min(speed,self.vel_speed_max) #calculate cartisian speed vx = speed * math.sin(target_heading) * -1.0 vy = speed * math.cos(target_heading) #*-1.0 print "Found Target go to vx:%f vy:%f Alt:%f target_distance:%f headings:%f heading:%f " % (vx,vy,location.z,target_distance,target_headings,target_heading) #only descend when on top of target if(location.z > 3.5): vz = 0.25 else: vz = 0 if active_command == 3: #PICK MP1 #Jika ketinggian sudah dibawah 4 meter, maka MAGNET ON #self.servo(0)#kamera siap untuk pick if (location.z < 4.0): if(location.z > 1.6): vz = 0.2 else: vz = 0 # if (location.z > 1.60): #Menurunkan Kecepatan Descent # vz = 0.2 # else: # vz = 0 if (self.c == True): self.relay2(1) #magnet ON self.waktu = 0 self.c = False if (location.z < 1.7): #if (GPIO.20 == HIGH): print ("Payload sudah diambil, lanjut misi") sc_logger.text(sc_logger.GENERAL, "Payload sudah diambil, lanjut misi") self.controlling_vehicle = False self.vehicle.mode = VehicleMode("AUTO") self.vehicle.flush() # if active_command = 3: # if if active_command == 4: #DROP MP1 #self.servo(1)#kamera siap untuk drop if(location.z > 2.0): vz = 0.25 else: if (location.z > 2.5): #Menurunkan Kecepatan Descent vz = 0.2 else: vz = 0 if (self.c == True): self.relay2(0) #magnet OFF self.waktu = 0 self.c = False self.drop = 1 print ("Payload sudah DROP, lanjut misi") sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi") self.vehicle.commands.next = 5 self.controlling_vehicle = False self.vehicle.mode = VehicleMode("AUTO") self.vehicle.flush() if active_command == 5: #DROP MP1 atau MP2 #self.servo(1)#kamera siap untuk drop if(location.z > 2.0): vz = 0.25 else: if (location.z > 2.5): #Menurunkan Kecepatan Descent vz = 0.2 else: vz = 0 if (self.c == True): self.relay2(0) #magnet OFF self.waktu = 0 self.c = False self.drop = 2 print ("Payload sudah DROP, lanjut misi") sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi") self.controlling_vehicle = False self.vehicle.mode = VehicleMode("AUTO") self.vehicle.flush() if active_command == 6: #DROP LOG #self.servo(1)#kamera siap untuk drop if(location.z > 2.5): vz = 0.25 else: if (location.z > 3.5): #Menurunkan Kecepatan Descent vz = 0.2 else: vz = 0 if (self.c == True): self.relay(1) #buka payload self.waktu = 0 self.c = False print ("Payload sudah DROP, lanjut misi") sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi") self.controlling_vehicle = False self.vehicle.mode = VehicleMode("AUTO") self.vehicle.flush() if active_command == 7: #PICK MP1 #Jika ketinggian sudah dibawah 4 meter, maka MAGNET ON #self.servo(0)#kamera siap untuk pick if (location.z < 4.0): if(location.z > 1.6): vz = 0.2 else: vz = 0 # if (location.z > 1.60): #Menurunkan Kecepatan Descent # vz = 0.2 # else: # vz = 0 if (self.c == True): self.relay2(1) #magnet ON self.waktu = 0 self.c = False if (location.z < 1.7): #if (GPIO.20 == HIGH): print ("Payload sudah diambil, lanjut misi") sc_logger.text(sc_logger.GENERAL, "Payload sudah diambil, lanjut misi") self.controlling_vehicle = False if (self.drop == 2): self.vehicle.commands.next = 8 self.lanjut_cmd += 1 self.vehicle.mode = VehicleMode("AUTO") self.vehicle.flush() if active_command == 9: #DROP MP2 #self.servo(1)#kamera siap untuk drop if(location.z > 2.0): vz = 0.25 else: if (location.z > 2.5): #Menurunkan Kecepatan Descent vz = 0.2 else: vz = 0 if (self.c == True): self.relay2(0) #magnet OFF self.waktu = 0 self.c = False print ("Payload sudah DROP, lanjut misi") sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi") self.controlling_vehicle = False self.vehicle.mode = VehicleMode("AUTO") self.vehicle.flush() #send velocity commands toward target heading self.send_nav_velocity(vx,vy,vz)
def run(self): sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name())) #start a video capture if(self.simulator): sc_logger.text(sc_logger.GENERAL, 'Using simulator') #-35.362664, 149.166803 -35.362902 149.166249 sim.set_target_location(Location(-35.363134 ,149.165429,0)) #sim.set_target_location(Location(0,0,0)) else: sc_video.start_capture(self.camera_index) #camera = balloon_video.get_camera() video_writer = balloon_video.open_video_writer() #create an image processor detector = CircleDetector() #create a queue for images imageQueue = Queue.Queue() #create a queue for vehicle info vehicleQueue = Queue.Queue() while veh_control.is_connected(): ''' #kill camera for testing if(cv2.waitKey(2) == 1113938): self.kill_camera = not self.kill_camera ''' #Reintialize the landing program when entering a landing mode if veh_control.controlling_vehicle(): if not self.in_control: if(self.allow_reset): sc_logger.text(sc_logger.GENERAL, 'Program initialized to start state') self.initialize_landing() self.in_control = True else: self.in_control = False # seach if(self.flag==False): # has issues #search in area if(self.point==1): point1 = Location(self.point1_lat, self.point1_lon, 5, is_relative=True) #point1 = Location(30.264233, 120.118813, 3, is_relative=True) self.vehicle.commands.goto(point1) self.vehicle.flush() if(veh_control.get_location().lon>=self.point1_lon): self.point=2 if(self.point==2): point2 = Location(self.point2_lat, self.point2_lon, 5, is_relative=True) #point1 = Location(30.264233, 120.118813, 3, is_relative=True) self.vehicle.commands.goto(point2) self.vehicle.flush() if(veh_control.get_location().lat>=self.point1_lat): self.point=3 if(self.point==3): point3 = Location(self.point3_lat, self.point3_lon, 5, is_relative=True) #point1 = Location(30.264233, 120.118813, 3, is_relative=True) self.vehicle.commands.goto(point3) self.vehicle.flush() if(veh_control.get_location().lon>=self.point3_lon): self.point=4 if(self.point==4): point4 = Location(self.point4_lat, self.point4_lon, 5, is_relative=True) #point1 = Location(30.264233, 120.118813, 3, is_relative=True) self.vehicle.commands.goto(point4) self.vehicle.flush() if(veh_control.get_location().lat>=self.point4_lat): self.point=1 ''' print "Going to first point..." #30.264233, 120.118813 -35.362664, 149.166803 point1 = Location(30.264233, 120.118813, 4, is_relative=True) self.vehicle.commands.goto(point1) self.vehicle.flush() ''' #we are in the landing zone or in a landing mode and we are still running the landing program #just because the program is running does not mean it controls the vehicle #i.e. in the landing area but not in a landing mode #FIXME add inside_landing_area() back to conditional if (self.in_control or self.always_run) and self.pl_enabled: #update how often we dispatch a command sc_dispatcher.calculate_dispatch_schedule() #get info from autopilot location = veh_control.get_location() attitude = veh_control.get_attitude() #update simulator if(self.simulator): sim.refresh_simulator(location,attitude) # grab an image capStart = current_milli_time() frame = self.get_frame() capStop = current_milli_time() # write the frame video_writer.write(frame) #update capture time sc_dispatcher.update_capture_time(capStop-capStart) #Process image #We schedule the process as opposed to waiting for an available core #This brings consistancy and prevents overwriting a dead process before #information has been grabbed from the Pipe if sc_dispatcher.is_ready(): #queue the image for later use: displaying image, overlays, recording imageQueue.put(frame) #queue vehicle info for later use: position processing vehicleQueue.put((location,attitude)) #the function must be run directly from the class sc_dispatcher.dispatch(target=detector.analyze_frame, args=(frame,attitude,)) #retreive results if sc_dispatcher.is_available(): sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count)) self.frame_count += 1 #results of image processor results = sc_dispatcher.retreive() # get image that was passed with the image processor img = imageQueue.get() #get vehicle position that was passed with the image processor location, attitude = vehicleQueue.get() #overlay gui rend_Image = gui.add_target_highlights(img, results[3]) #show/record images sc_logger.image(sc_logger.RAW, img) sc_logger.image(sc_logger.GUI, rend_Image) #display/log data sc_logger.text(sc_logger.ALGORITHM,'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'.format(results[0],results[1],results[2],results[3])) sc_logger.text(sc_logger.AIRCRAFT,attitude) sc_logger.text(sc_logger.AIRCRAFT,location) #send commands to autopilot if(results[2]!=-1): self.flag = True if(self.flag==True): self.control(results,attitude,location) else: if(self.pl_enabled == False): sc_logger.text(sc_logger.GENERAL, 'Landing disabled') else: sc_logger.text(sc_logger.GENERAL, 'Not in landing mode or Landing Area') #terminate program sc_logger.text(sc_logger.GENERAL, 'Vehicle disconnected, Program Terminated') if(self.simulator == False): sc_video.stop_capture()
parser.add_argument('-c', '--camera', default=0, help="Select the camera index for opencv") parser.add_argument('-i', '--input', default=False, help='use a video filename as an input instead of a webcam') parser.add_argument('-f', '--file', default='Smart_Camera.cnf', help='load a config file other than the default') args, unknown = parser.parse_known_args() ''' #run a strategy Strategy = None if args.Strategy == 'land': Strategy = PrecisionLand(args) if args.Strategy == 'redballoon': Strategy = RedBalloon(args) if args.Strategy == 'opticalflow': Strategy = OpticalFlow(args) #configure a logger sc_logger.set_name(Strategy.name()) sc_logger.text(sc_logger.GENERAL, 'Starting {0}'.format(Strategy.name())) Strategy.run()
def run(self): sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name())) #start a video capture if(self.simulator): sc_logger.text(sc_logger.GENERAL, 'Using simulator') sim.set_target_location(veh_control.get_home()) #sim.set_target_location(Location(0,0,0)) else: sc_video.start_capture(self.camera_index) #create an image processor detector = CircleDetector() #create a queue for images imageQueue = Queue.Queue() #create a queue for vehicle info vehicleQueue = Queue.Queue() while veh_control.is_connected(): ''' #kill camera for testing if(cv2.waitKey(2) == 1113938): self.kill_camera = not self.kill_camera ''' #Reintialize the landing program when entering a landing mode if veh_control.controlling_vehicle(): if not self.in_control: if(self.allow_reset): sc_logger.text(sc_logger.GENERAL, 'Program initialized to start state') self.initialize_landing() self.in_control = True else: self.in_control = False #we are in the landing zone or in a landing mode and we are still running the landing program #just because the program is running does not mean it controls the vehicle #i.e. in the landing area but not in a landing mode #FIXME add inside_landing_area() back to conditional if (self.in_control or self.always_run) and self.pl_enabled: #update how often we dispatch a command sc_dispatcher.calculate_dispatch_schedule() #get info from autopilot location = veh_control.get_location() attitude = veh_control.get_attitude() ''' #get info from autopilot location = Location(0.000009,0,location.alt) attitude = Attitude(0,0,0) ''' #update simulator if(self.simulator): sim.refresh_simulator(location,attitude) # grab an image capStart = current_milli_time() frame = self.get_frame() capStop = current_milli_time() ''' if(self.kill_camera): frame[:] = (0,255,0) ''' #update capture time sc_dispatcher.update_capture_time(capStop-capStart) #Process image #We schedule the process as opposed to waiting for an available core #This brings consistancy and prevents overwriting a dead process before #information has been grabbed from the Pipe if sc_dispatcher.is_ready(): #queue the image for later use: displaying image, overlays, recording imageQueue.put(frame) #queue vehicle info for later use: position processing vehicleQueue.put((location,attitude)) #the function must be run directly from the class sc_dispatcher.dispatch(target=detector.analyze_frame, args=(frame,attitude,)) #retreive results if sc_dispatcher.is_available(): sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count)) self.frame_count += 1 #results of image processor results = sc_dispatcher.retreive() # get image that was passed with the image processor img = imageQueue.get() #get vehicle position that was passed with the image processor location, attitude = vehicleQueue.get() ''' #overlay gui rend_Image = gui.add_target_highlights(img, results[3]) #show/record images sc_logger.image(sc_logger.RAW, img) sc_logger.image(sc_logger.GUI, rend_Image) ''' #display/log data sc_logger.text(sc_logger.ALGORITHM,'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'.format(results[0],results[1],results[2],results[3])) sc_logger.text(sc_logger.AIRCRAFT,attitude) sc_logger.text(sc_logger.AIRCRAFT,location) #send commands to autopilot self.control(results,attitude,location) else: if(self.pl_enabled == False): sc_logger.text(sc_logger.GENERAL, 'Landing disabled') else: sc_logger.text(sc_logger.GENERAL, 'Not in landing mode or Landing Area') #terminate program sc_logger.text(sc_logger.GENERAL, 'Vehicle disconnected, Program Terminated') if(self.simulator == False): sc_video.stop_capture()
''' #parse arguments parser = argparse.ArgumentParser(description="Use image processing to peform visual navigation") #optional arguments parser.add_argument('-S', '--Strategy', action="store", default='land', type=str, choices=['land','redballoon','opticalflow'], help='Land: vision assisted landing \n redballoon: redballoon finder for AVC 2014 \n opticalflow: Optical flow using webcam') parser.add_argument('-c', '--camera', default=0, help="Select the camera index for opencv") parser.add_argument('-i', '--input', default=False, help='use a video filename as an input instead of a webcam') parser.add_argument('-f', '--file', default='Smart_Camera.cnf', help='load a config file other than the default') args, unknown = parser.parse_known_args() ''' #run a strategy Strategy = None if args.Strategy == 'land': Strategy = PrecisionLand(args) if args.Strategy == 'redballoon': Strategy = RedBalloon(args) if args.Strategy == 'opticalflow': Strategy = OpticalFlow(args) #configure a logger sc_logger.set_name(Strategy.name()) sc_logger.text(sc_logger.GENERAL, 'Starting {0}'.format(Strategy.name())) Strategy.run()
def control(self,target_info,attitude,location): #we have control from autopilot if self.in_control: valid_target = False #now=0; #print (time.time()-now); now = time.time() print now; #detected a target if target_info[1] is not None: self.target_detected = True valid_target = True initial_descent = False self.last_valid_target = now #attempt to use precision landing if(self.inside_landing_area() == 1): #we have detected a target in landing area if(self.target_detected): self.climbing = False self.initial_descent = False #we currently see target if(valid_target): sc_logger.text(sc_logger.GENERAL, 'Target detected. Moving to target') ##add myself if(veh_control.get_location().alt < 1): self.autopilot_land() sc_logger.text(sc_logger.GENERAL, 'Landing!!!!!!!!') #move to target else: self.move_to_target(target_info,attitude,location) #lost target else: #we have lost the target for more than settle_time if(now - self.last_valid_target > self.settle_time): self.target_detected = False #temporarily lost target, #top section of cylinder if(veh_control.get_location().alt > self.abort_height): sc_logger.text(sc_logger.GENERAL, 'Lost Target: Straight Descent') #continue descent self.straight_descent() else: sc_logger.text(sc_logger.GENERAL, 'Lost Target: Holding') #neutralize velocity veh_control.set_velocity(0,0,0) #there is no known target in landing area else: #currently searching if(self.climbing): self.climb() #not searching, decide next move else: #top section of cylinder if(veh_control.get_location().alt > self.abort_height): #initial descent entering cylinder if(self.initial_descent): sc_logger.text(sc_logger.GENERAL, 'No Target: Initial Descent') #give autopilot control self.autopilot_land() #all other attempts prior to intial target detection else: sc_logger.text(sc_logger.GENERAL, 'No target: Straight descent') #straight descent self.straight_descent() #lower section of cylinder else: #we can attempt another land if(self.attempts < self.search_attempts): self.attempts += 1 sc_logger.text(sc_logger.GENERAL, 'Climbing to attempt {0}'.format(self.attempts)) #start climbing self.climb() #give up and else: sc_logger.text(sc_logger.GENERAL, 'Out of attempts: Giving up') #give autopilot control self.autopilot_land() #final descent elif(self.inside_landing_area() == -1): sc_logger.text(sc_logger.GENERAL, 'In final descent') #straight descent #change LAND mode #self.straight_descent() self.vehicle.mode = VehicleMode("LAND") self.vehicle.flush() self.target_detected = False #outside cylinder else: sc_logger.text(sc_logger.GENERAL, 'Outside landing zone') #give autopilot control self.autopilot_land() self.target_detected = False self.initial_descent = True #the program is running but the autopilot is in an invalid mode else: sc_logger.text(sc_logger.GENERAL, 'Not in control of vehicle')
def run(self): sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name())) #start a video capture ''' if(self.simulator): sc_logger.text(sc_logger.GENERAL, 'Using simulator') sim.set_target_location(veh_control.get_home()) #sim.set_target_location(Location(0,0,0)) else:''' sc_video.start_capture(self.camera_index) #camera = balloon_video.get_camera() video_writer = balloon_video.open_video_writer() #create an image processor detector = CircleDetector() #create a queue for images imageQueue = Queue.Queue() #create a queue for vehicle info vehicleQueue = Queue.Queue() while veh_control.is_connected(): #get info from autopilot location = veh_control.get_location() attitude = veh_control.get_attitude() print location print attitude # Take each frame #_, frame = camera.read() #update how often we dispatch a command sc_dispatcher.calculate_dispatch_schedule() # grab an image capStart = current_milli_time() frame = sc_video.get_image() capStop = current_milli_time() #frame = sc_video.undisort_image(frame) #cv2.imshow('frame',frame) # write the frame video_writer.write(frame) #update capture time sc_dispatcher.update_capture_time(capStop-capStart) #Process image #We schedule the process as opposed to waiting for an available core #This brings consistancy and prevents overwriting a dead process before #information has been grabbed from the Pipe if sc_dispatcher.is_ready(): #queue the image for later use: displaying image, overlays, recording imageQueue.put(frame) #queue vehicle info for later use: position processing vehicleQueue.put((location,attitude)) #the function must be run directly from the class ####### sc_dispatcher.dispatch(target=balloon_finder.analyse_frame, args=(frame,)) #retreive results if sc_dispatcher.is_available(): sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count)) self.frame_count += 1 #results of image processor results = sc_dispatcher.retreive() # get image that was passed with the image processor img = imageQueue.get() #get vehicle position that was passed with the image processor location, attitude = vehicleQueue.get() #overlay gui #rend_Image = gui.add_target_highlights(img, results[3]) #show/record images sc_logger.image(sc_logger.RAW, img) #sc_logger.image(sc_logger.GUI, rend_Image) #display/log data sc_logger.text(sc_logger.ALGORITHM,'found: {0} x: {1} y: {2} radius: {3}'.format(results[0],results[1],results[2],results[3]))
#get vehicle position that was passed with the image processor location, attitude = vehicleQueue.get() #overlay gui #rend_Image = gui.add_target_highlights(img, results[3]) #show/record images sc_logger.image(sc_logger.RAW, img) #sc_logger.image(sc_logger.GUI, rend_Image) #display/log data sc_logger.text(sc_logger.ALGORITHM,'found: {0} x: {1} y: {2} radius: {3}'.format(results[0],results[1],results[2],results[3])) #print Location(-35.363554, 149.165139,0) # if starting from mavproxy if __name__ == "__builtin__": # start precision landing strat = GetPara() # connect to droneapi sc_logger.text(sc_logger.GENERAL, 'Connecting to vehicle...') strat.connect() sc_logger.text(sc_logger.GENERAL, 'Vehicle connected!') # run strategy strat.run()
def get_frame(self): if (self.simulator): return sim.get_frame() else: return sc_video.get_image() #pixel_point_to_position_xy - convert position in pixels to position in meters #pixel_position - distance in pixel from CENTER of image #distance- distance from the camera to the object in meters def pixel_point_to_position_xy(self, pixel_position, distance): thetaX = pixel_position[0] * self.camera_hfov / self.camera_width thetaY = pixel_position[1] * self.camera_vfov / self.camera_height x = distance * math.tan(math.radians(thetaX)) y = distance * math.tan(math.radians(thetaY)) return (x, y) # if starting from mavproxy if __name__ == "__builtin__": # start precision landing strat = PrecisionLand() # connect to droneapi sc_logger.text(sc_logger.GENERAL, 'Connecting to vehicle...') strat.connect() sc_logger.text(sc_logger.GENERAL, 'Vehicle connected!') # run strategy strat.run()
def run(self): sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name())) #start a video capture if (self.simulator): sc_logger.text(sc_logger.GENERAL, 'Using simulator') sim.set_target_location(veh_control.get_home()) #sim.set_target_location(Location(0,0,0)) else: sc_video.start_capture(self.camera_index) #create an image processor detector = CircleDetector() #create a queue for images imageQueue = Queue.Queue() #create a queue for vehicle info vehicleQueue = Queue.Queue() while veh_control.is_connected(): ''' #kill camera for testing if(cv2.waitKey(2) == 1113938): self.kill_camera = not self.kill_camera ''' #Reintialize the landing program when entering a landing mode if veh_control.controlling_vehicle(): if not self.in_control: if (self.allow_reset): sc_logger.text(sc_logger.GENERAL, 'Program initialized to start state') self.initialize_landing() self.in_control = True else: self.in_control = False #we are in the landing zone or in a landing mode and we are still running the landing program #just because the program is running does not mean it controls the vehicle #i.e. in the landing area but not in a landing mode #FIXME add inside_landing_area() back to conditional if (self.in_control or self.always_run) and self.pl_enabled: #update how often we dispatch a command sc_dispatcher.calculate_dispatch_schedule() #get info from autopilot location = veh_control.get_location() attitude = veh_control.get_attitude() ''' #get info from autopilot location = Location(0.000009,0,location.alt) attitude = Attitude(0,0,0) ''' #update simulator if (self.simulator): sim.refresh_simulator(location, attitude) # grab an image capStart = current_milli_time() frame = self.get_frame() capStop = current_milli_time() ''' if(self.kill_camera): frame[:] = (0,255,0) ''' #update capture time sc_dispatcher.update_capture_time(capStop - capStart) #Process image #We schedule the process as opposed to waiting for an available core #This brings consistancy and prevents overwriting a dead process before #information has been grabbed from the Pipe if sc_dispatcher.is_ready(): #queue the image for later use: displaying image, overlays, recording imageQueue.put(frame) #queue vehicle info for later use: position processing vehicleQueue.put((location, attitude)) #the function must be run directly from the class sc_dispatcher.dispatch(target=detector.analyze_frame, args=( frame, attitude, )) #retreive results if sc_dispatcher.is_available(): sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count)) self.frame_count += 1 #results of image processor results = sc_dispatcher.retreive() # get image that was passed with the image processor img = imageQueue.get() #get vehicle position that was passed with the image processor location, attitude = vehicleQueue.get() #overlay gui rend_Image = gui.add_target_highlights(img, results[3]) #show/record images sc_logger.image(sc_logger.RAW, img) sc_logger.image(sc_logger.GUI, rend_Image) #display/log data sc_logger.text( sc_logger.ALGORITHM, 'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}' .format(results[0], results[1], results[2], results[3])) sc_logger.text(sc_logger.AIRCRAFT, attitude) sc_logger.text(sc_logger.AIRCRAFT, location) #send commands to autopilot self.control(results, attitude, location) else: if (self.pl_enabled == False): sc_logger.text(sc_logger.GENERAL, 'Landing disabled') else: sc_logger.text(sc_logger.GENERAL, 'Not in landing mode or Landing Area') #terminate program sc_logger.text(sc_logger.GENERAL, 'Vehicle disconnected, Program Terminated') if (self.simulator == False): sc_video.stop_capture()
def control(self, target_info, attitude, location): #we have control from autopilot if self.in_control: valid_target = False now = time.time() #detected a target if target_info[1] is not None: self.target_detected = True valid_target = True initial_descent = False self.last_valid_target = now #attempt to use precision landing if (self.inside_landing_area() == 1): #we have detected a target in landing area if (self.target_detected): self.climbing = False self.initial_descent = False #we currently see target if (valid_target): sc_logger.text(sc_logger.GENERAL, 'Target detected. Moving to target') #move to target self.move_to_target(target_info, attitude, location) #lost target else: #we have lost the target for more than settle_time if (now - self.last_valid_target > self.settle_time): self.target_detected = False #temporarily lost target, #top section of cylinder if (veh_control.get_location().alt > self.abort_height): sc_logger.text(sc_logger.GENERAL, 'Lost Target: Straight Descent') #continue descent self.straight_descent() else: sc_logger.text(sc_logger.GENERAL, 'Lost Target: Holding') #neutralize velocity veh_control.set_velocity(0, 0, 0) #there is no known target in landing area else: #currently searching if (self.climbing): self.climb() #not searching, decide next move else: #top section of cylinder if (veh_control.get_location().alt > self.abort_height): #initial descent entering cylinder if (self.initial_descent): sc_logger.text(sc_logger.GENERAL, 'No Target: Initial Descent') #give autopilot control self.autopilot_land() #all other attempts prior to intial target detection else: sc_logger.text(sc_logger.GENERAL, 'No target: Straight descent') #straight descent self.straight_descent() #lower section of cylinder else: #we can attempt another land if (self.attempts < self.search_attempts): self.attempts += 1 sc_logger.text( sc_logger.GENERAL, 'Climbing to attempt {0}'.format( self.attempts)) #start climbing self.climb() #give up and else: sc_logger.text(sc_logger.GENERAL, 'Out of attempts: Giving up') #give autopilot control self.autopilot_land() #final descent elif (self.inside_landing_area() == -1): sc_logger.text(sc_logger.GENERAL, 'In final descent') #straight descent self.straight_descent() self.target_detected = False #outside cylinder else: sc_logger.text(sc_logger.GENERAL, 'Outside landing zone') #give autopilot control self.autopilot_land() self.target_detected = False self.initial_descent = True #the program is running but the autopilot is in an invalid mode else: sc_logger.text(sc_logger.GENERAL, 'Not in control of vehicle')