def drone_creeping_line(self): #if maybe wall if self.Drone_lidar.detect_wall() == 2: self.jetbot_move_pub.publish("stop") self.cmd_pub.publish(controls.hold()) #if definitely wall: # turn around and search and align to jetbot if self.Drone_lidar.detect_wall() == 1: print("turn around") self.jetbot_move_pub.publish("stop") if self.wall_count % 2 == 0: self.cmd_pub.publish(controls.control(az=1)) else: self.cmd_pub.publish(controls.control(az=-1)) time.sleep(3.5) self.wall_count += 1 while not self.drone_search_jetbot(True): pass if not self.wall_count == self.max_wallcount: self.jetbot_move_pub.publish("patrol") self.cmd_pub.publish((controls.control(z=0.8))) time.sleep(1) #if no wall if self.Drone_lidar.detect_wall() == 0: self.cmd_pub.publish(controls.control(y=0.8)) self.jetbot_move_pub.publish("patrol") print("no wall") """
def camera_callback(self, msg): img = bridge.imgmsg_to_cv2(msg, "bgr8") img = imutils.resize(img, width=300) debug_img = img.copy() img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) if not self.lander.init_background(img): return # Filter background dfilter = self.lander.deriv_filter(img) img_filtered = cv2.bitwise_and(img_hsv, img_hsv, mask=dfilter) vel = [0,0,0,0] if self.last_range is not None: range_time, range = self.last_range if time.time()-range_time < 0.5: #self.update_state(vel, range) if range < 1500: self.cmd_vel.publish(controls.control(az=0.7)) rospy.sleep(4) else: vel[1] = 0.5 self.cmd_vel.publish(controls.control(x=vel[0], y=vel[1], z=vel[2], az=vel[3])) # Setup velocity timeout if set #if self.vel_timeout is not None: # self.vel_timeout.shutdown() #self.vel_timeout = rospy.Timer(rospy.Duration(1), self.reset_vel(), oneshot=True) self.bg_img.publish(bridge.cv2_to_imgmsg(dfilter)) self.debug_img.publish(bridge.cv2_to_imgmsg(debug_img))
def go_trough_gate(self): logging.info("Decision Control - Go Through Gate") sx = 1 sz = 0.6 self.cmd_pub.publish(controls.control(y=sx, z=-sz)) time.sleep(1.5) self.cmd_pub.publish(controls.control(y=sx, z=sz)) time.sleep(1.5) self.cmd_pub.publish(controls.hold()) time.sleep(1.5) return 1
def camera_callback(self, msg): img = bridge.imgmsg_to_cv2(msg, "bgr8") debugimg = img.copy() #pltimg = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) #plt.imshow(pltimg) #plt.show() #image is already w = 300 #img = imutils.resize(img, width=300) img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) if not self.lander.init_background(img): return # Filter background dfilter = self.lander.deriv_filter(img) img_filtered = cv2.bitwise_and(img_hsv, img_hsv, mask=dfilter) #img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) #img_filtered = cv2.bitwise_and(img, img, mask=dfilter) # Find craft position #craft_pos = self.lander.find_craft_leds(img_hsv, self.led_blue, self.led_red_lower, self.led_red_higher, debug_img=img) craft_pos = self.lander.find_craft_frame(dfilter, debug_img=debugimg) # Update craft velocity vx, vy, vz = 0, 0, 0 if craft_pos is not None: x, y, a, h = craft_pos land_pos = (0.5, 0.5) vx, vy = self.lander.land_update(img_filtered, x, y, a, h, land_pos, debug_img=debugimg) ih, iw, _ = img.shape target = (iw * land_pos[0], ih * land_pos[1]) if abs(x - target[0]) < 0.05 * iw and abs( y - target[1] < 0.05 * ih): vz = -0.4 * self.landingtimer() if h < 0.62: self.lander.land() else: self.landingtimer(reset=True) # Publish velocity command self.cmd_vel.publish(controls.control(x=vx, y=vy, z=vz)) # Setup velocity timeout if set #if self.vel_timeout is not None: # self.vel_timeout.shutdown() #self.vel_timeout = rospy.Timer(rospy.Duration(1), self.reset_vel(), oneshot=True) #""" self.bg_img.publish(bridge.cv2_to_imgmsg(dfilter)) self.debug_img.publish(bridge.cv2_to_imgmsg(debugimg))
def search(self): self.cmd_pub.publish(controls.control(az = np.sign(self.metronomedir)*0.5 )) time.sleep(0.3) self.metronome += self.metronomedir print("metronome, dir", self.metronome, self.metronomedir) if abs(self.metronome) > self.metronome_sweepsize: self.metronomedir = -self.metronomedir self.metronome_sweepsize +=1 if self.metronome == 0 and self.metronomedir == 1: self.cmd_pub.publish(controls.control(y=0.8)) time.sleep(1) print("go forward") """
def move_around_gate(self, alpha, gate_dist, gate_x, gate_z): # y+ # ^ # x- <- -> x+ # v # y- # . = z+ # + = z- # clockwise = az+ ? # cntrclockwise = az- ? # # looking from gate: if drone is left: alpha < 0 # right: alpha > 0 #looking from drone: if gate is left: x < 0 # right: x > 0 # high: z < 0 # low: z > 0 #print("move") #print(alpha,gate_x,gate_z,gate_dist) #see what input is like, the convert alpha to deg, with 0 = center, +90 = left, -90 = right #convert x and z to % of image #set speeds to 0 sx,sy,sz,saz = 0,0,0,0 #TODO check signs for all #angle ranges from -0.1 to + 0.1 alpha = np.sign(alpha)*min(abs(alpha*10), 0.3) sy += alpha saz += -alpha print("alpha,dist,x,y" ,(alpha, gate_dist, gate_x, gate_z)) #if abs(gate_x < 0.1): # gate_x = np.sign(gate_x)*0.1 #if abs(gate_z < 0.1): # gate_z = np.sign(gate_z)*0.1 control_multiple = 1.5 gate_x = gate_x*control_multiple gate_z = gate_z*control_multiple*2 gate_x = np.sign(gate_x)*min(abs(gate_x), 0.3) gate_z = np.sign(gate_z)*min(abs(gate_z), 0.3) sy += gate_x sz += -gate_z sx = min(gate_dist - self.target_dist, 0.2) #max speed =1 sx = sx*control_multiple if use_sqrt_for_motion: sqrt_cali = 10.0 #sx = np.sign(sx)*np.sqrt(abs(sx)/sqrt_cali) sy = np.sign(sy)*np.sqrt(abs(sy)/sqrt_cali) sz = np.sign(sz)*np.sqrt(abs(sz)/sqrt_cali) #saz = np.sign(saz)*np.sqrt(abs(saz)/sqrt_cali) self.cmd_pub.publish(controls.control(y = sx, x = sy, z = sz, az = saz))
def align_to_platform(self, platform_pos): self.reset_metronome() self.searctime = None platform_x, platform_y, platform_size = platform_pos platform_size = (platform_size - 70.0) / 70.0 self.align_pid_angle.setpoint = 0 self.align_pid_dist.setpoint = 0 self.align_pid_height.setpoint = 0.7 print("platform_pos: ", platform_pos) saz = platform_x / 2.0 sz = 0.75 * (platform_size - 70) / 70.0 sy = 1.0 * (-1) * (platform_y - 0.7) saz = self.align_pid_angle(-platform_x) sz = self.align_pid_height(platform_y) sy = self.align_pid_dist(platform_size) self.cmd_pub.publish(controls.control(az=saz, y=sy, z=sz)) if abs(platform_x) < 0.15 and abs(platform_y) < 0.8 and abs( platform_y) > 0.6 and abs(platform_size) < 0.15: return 1 return 0
def search(self): if self.searctime is None: self.searctime = time.time() if time.time() - self.searctime < 1: pass #self.cmd_pub.publish(controls.control(az = 0.5)) else: self.cmd_pub.publish(controls.control(y=-0.4))
def both_scramble(self): self.cmd_takeoff.publish(Empty()) time.sleep(4.0) self.jetbot_move_pub.publish("circle") self.cmd_pub.publish(controls.control(y=0.7, az=0.7)) time.sleep(4.0) self.cmd_pub.publish(controls.hold()) time.sleep(1.0)
def search(self): logging.info("Decision Control - Search for Gate") self.cmd_pub.publish(controls.control(az=np.sign(self.metronome) * 0.6)) time.sleep(0.5) self.metronome += 1 if self.metronome > 10: self.metronome = -15
def fly_to_platform(self, platform_x): if abs(platform_x) > 0.05: self._align_to_platform(platform_x) else: sx = 1 saz = platform_x / 3.0 self.cmd_pub.publish(controls.control(y=sx, az=saz)) print("fly" + str(saz))
def rotation_search(self): self.cmd_pub.publish( controls.control(az=np.sign(self.metronomedir) * 0.6, y=-0.2)) time.sleep(0.3) self.metronome += self.metronomedir print("metronome, dir", self.metronome, self.metronomedir) if abs(self.metronome) > self.metronome_sweepsize: self.metronomedir = -self.metronomedir self.metronome_sweepsize += 1
def go_trough_gate(self): sx = 1.7 sz = 0.6 stop = 1 for i in range(5): print("GO") self.cmd_pub.publish(controls.hold()) time.sleep(0.2) self.cmd_pub.publish(controls.control(y=sx/4, z = -sz/4)) time.sleep(0.5) self.cmd_pub.publish(controls.control(y=sx/2, z = -sz/2)) time.sleep(0.5) self.cmd_pub.publish(controls.control(y=3*sx/4, z = -3*sz/4)) time.sleep(0.5) self.cmd_pub.publish(controls.control(y=sx, z = -sz)) time.sleep(1.5) self.cmd_pub.publish(controls.control(y=-stop)) time.sleep(1) self.cmd_pub.publish(controls.control(z = sz)) time.sleep(1.5) self.cmd_pub.publish(controls.hold()) time.sleep(1) self.reset_metronome() return 1
def move_around_gate(self, alpha, gate_x, gate_z, gate_dist): logging.info("Decision Control - Angle Correction") # y+ # ^ # x- <- -> x+ # v # y- # . = z+ # + = z- # clockwise = az+ ? # cntrclockwise = az- ? # # looking from gate: if drone is left: alpha < 0 # right: alpha > 0 #looking from drone: if gate is left: x < 0 # right: x > 0 # high: z < 0 # low: z > 0 #print("move") #print(alpha,gate_x,gate_z,gate_dist) #see what input is like, the convert alpha to deg, with 0 = center, +90 = left, -90 = right #convert x and z to % of image #set speeds to 0 sx, sy, sz, saz = 0, 0, 0, 0 #TODO check signs for all #angle ranges from -0.1 to + 0.1 sy += alpha * 10.0 saz += -alpha * 10.0 print("alpha,dist,x,y", (alpha, gate_dist, gate_x, gate_z)) #if abs(gate_x < 0.1): # gate_x = np.sign(gate_x)*0.1 #if abs(gate_z < 0.1): # gate_z = np.sign(gate_z)*0.1 control_multiple = 1 gate_x = gate_x * control_multiple gate_z = gate_z * control_multiple sy += gate_x sz += -gate_z sx = min(gate_dist - self.target_dist, 1) #max speed =1 sx = sx * control_multiple self.cmd_pub.publish(controls.control(y=sx, x=sy, z=sz, az=saz))
def fly_over_platform(self, platform_pos): if platform_pos is None: return 0 platform_x, platform_y, platform_size = platform_pos self.searctime = None if platform_pos is None: return 0 elif abs(platform_x) > 0.1 and abs(platform_y) > 0.8 and abs( platform_y ) < 0.6 and platform_size > 60 and platform_size < 80: self.align_to_platform(platform_pos) return 0 self.cmd_pub.publish(controls.hold()) time.sleep(0.2) sx = 1 self.cmd_pub.publish(controls.control(y=sx)) return 1
def move_around_gate(self, alpha, gate_x, gate_z, gate_dist): #see what input is like, the convert alpha to deg, with 0 = center, +90 = left, -90 = right #convert x and z to % of image target_dist = 1 #m #set speeds to 0 sx, sy, sz, saz = 0, 0, 0, 0 #check signs for all sy += alpha / 90.0 #at 90 deg, speed=max=1 saz += alpha / 90.0 sy += gate_x sz += -gate_z sx = min(gate_dist - target_dist, 1) #max speed =1 self.cmd_pub.publish(controls.control(y=sx, x=sy, z=sz, az=saz))
def go_trough_gate(self): sx = 1 self.cmd_pub.publish(controls.control(y=sx)) time.sleep(2) self.cmd_pub.publish(controls.hold()) return 1
def _align_to_platform(self, platform_x): saz = platform_x self.cmd_pub.publish(controls.control(az=saz)) print("align" + str(saz))
def search_pattern(): self.cmd_pub.publish(controls.control(az=0.5))
def image_processing(self, img): #img = bridge.imgmsg_to_cv2(msg, "bgr8") debugimg = img img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) ####calling init_background, this will return 1 when process complete if not self.lander.init_background(img): return # Filter background dfilter = self.lander.deriv_filter(img) img_filtered = cv2.bitwise_and(img_hsv, img_hsv, mask=dfilter) # img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # img_filtered = cv2.bitwise_and(img, img, mask=dfilter) # Find craft position # craft_pos = self.lander.find_craft_leds(img_hsv, self.led_blue, self.led_red_lower, self.led_red_higher, debug_img=img) ####craft pos = x,y,a,h (==posx, posy, angle, height) #### = None if spotted elements too small craft_pos = self.lander.find_craft_frame(dfilter, debug_img=debugimg) # Update craft velocity vx, vy, vz = 0, 0, 0 if craft_pos is not None: x, y, a, h = craft_pos land_pos = (0.5, 0.5) vx, vy = self.lander.land_update(img_filtered, x, y, a, h, land_pos, debug_img=debugimg) ih, iw, _ = img.shape target = (iw * land_pos[0], ih * land_pos[1]) if abs(x - target[0]) < 0.03 * iw and abs( y - target[1] < 0.03 * ih): vz = -0.4 * self.landingtimer() print("go down") if h < 0.62 and self.landingtimer() and not self.landed: self.lander.land() self.landed = 1 self.time_of_landing = time.time() print("land") else: self.landingtimer(reset=True) # Publish velocity command self.cmd_vel.publish(controls.control(x=vx, y=vy, z=vz)) #after 15 sec of landing, check if landing was succesful if self.landed == 1 and time.time() - self.time_of_landing > 15: if craft_pos is None: self.landed = 0 self.landing_done_pub.publish(self.landed) self.landed = 0 # Setup velocity timeout if set # if self.vel_timeout is not None: # self.vel_timeout.shutdown() # self.vel_timeout = rospy.Timer(rospy.Duration(1), self.reset_vel(), oneshot=True) # """ #publish debugs at limited fps if time.time() - self.debug_pubtimer > 0.2: self.debug_pubtimer = time.time() self.bg_img.publish(bridge.cv2_to_imgmsg(dfilter)) self.debug_img.publish(bridge.cv2_to_imgmsg(debugimg))