class SettMission (object): def __init__ (self): # rospy.init_node('sett_srv', anonymous=True) print "Now do Set Course" #### SETT ## subscribe vision sett_srv = 'setcourse_srv' rospy.wait_for_service(sett_srv) print 'service starts top srv' self.detect_sett = rospy.ServiceProxy(sett_srv, SetCourse_Srv) #### SETT self.aicontrol = AIControl() self.hw = Hardware() def find_sett (self): count = 50 self.aicontrol.drive_z (const.SET_DETECTING_DEPTH) ##### CHANGE ME !! while not rospy.is_shutdown() and not self.aicontrol.is_fail(count): sett_data = self.detect_sett(String('setcourse'),String('big')) sett_data = sett_data.data print sett_data if len(sett_data.appear) != 0 and sett_data.appear[0]: print 'found' if sett_data.area[0] > 15000: ###### CHANGE ME !! print 'near' bc = 0.1 else: print 'far' bc = 0.3 if self.aicontrol.is_center([sett_data.x[0],sett_data.y[0]],-bc,bc,-bc,bc): if sett_data.area[0] > 16000: ###### CHANGE ME !! print 'should see 4 sq, find big complete !!' break else: print 'forward' self.aicontrol.drive_x (0.05) else: self.aicontrol.drive ([0,sett_data.x[0],sett_data.y[0],0,0,0]) rospy.sleep(0.5) print 'set to center' self.aicontrol.stop (1) else: self.aicontrol.stop (1) print 'not found' count -= 1 def find_sq (self, yd, zd, la): ### y_direction & z_direction & launcher count = 20 saw = False # self.aicontrol.drive_y (0.6*yd/0.05) while not rospy.is_shutdown() and not self.aicontrol.is_fail(count): sq_data = self.detect_sett(String('setcourse'),String('small')) sq_data = sq_data.data print sq_data if len(sq_data.appear) == 1: if self.aicontrol.is_center ([sq_data.x[0],sq_data.y[0]],-0.05,0.05,-0.05,0.05): if sq_data.area[0] > 6000: print 'forward to fire' self.hw.command (String(la), String('fire')) self.hw.command (String(la), String('close')) print 'fire complete' self.aicontrol.stop (10) self.aicontrol.drive_x (-2) break else: print 'forward' self.aicontrol.drive_x (0.02) else: print 'move' self.aicontrol.drive ([0,sq_data.x[0],sq_data.y[0],0,0,0]) rospy.sleep(0.5) elif len(sq_data.appear) == 2 or len(sq_data.appear) == 3: self.aicontrol.drive_x (0.2) else: state = self.aicontrol.get_pose() if -4 < state[2] < -2: ###### CHANGE ME !!! self.aicontrol.drive ([0,0,zd,0,0,0]) rospy.sleep(0.5) self.aicontrol.drive ([0.2,yd,0,0,0,0]) rospy.sleep (0.2) return def run (self): v = 0.05 eq = ['fire_left', 'fire_right'] self.find_sett () self.find_sq (-v, v, eq[0]) #### right top - + self.find_sett () self.find_sq (v, -v, eq[1])
class BinnMission (object): def __init__ (self): print "Now do bin" #### BINN ## subscribe vision srv_name = 'bin_srv' rospy.wait_for_service(srv_name) print 'service starts' self.detect_binn = rospy.ServiceProxy(srv_name, Bin_Srv) #### BINN self.aicontrol = AIControl() self.hw = Hardware() def getdata (self): binn_data = self.detect_binn(String('bin'),String('white')) binn_data = binn_data.data return binn_data def run (self, cover): # if cover = 1, uncover = 0 print 'Go to bin' count = 50 self.aicontrol.drive_z (-1) while not rospy.is_shutdown() and not self.aicontrol.is_fail(count): print 'in while' binn_data = self.getdata() print binn_data i = -1 if len(binn_data.appear) == 2: if not binn_data.cover[0]: i = 0 else: i = 1 elif len(binn_data.appear) == 1 and not binn_data.cover[0]: i = 0 else: # self.aicontrol.stop(1) print 'not found' count -= 1 if i != -1: if self.aicontrol.is_center ([binn_data.x[i],binn_data.y[i]],-0.05,0.05,-0.05,0.05) and -3 < binn_data.angle[i] < 3: print 'Center' print binn_data.x[i] print binn_data.y[i] self.aicontrol.drive_z (const.BIN_DETECTING_DEPTH) ##### DEPTH !!! rospy.sleep (1) break elif self.aicontrol.is_center ([binn_data.x[i],binn_data.y[i]],-0.05,0.05,-0.05,0.05): self.aicontrol.turn_yaw_relative (binn_data.angle[i]) rospy.sleep (1) else : print 'Not Center' vx = binn_data.x[i] vy = binn_data.y[i] self.aicontrol.drive ([vx,vy,0,0,0,0]) rospy.sleep(1) rospy.sleep(0.25) ''' if cover == 1: # self.hw.command ('gripper', 'grab') ### grab self.aicontrol.drive_z (-2) ### up -> open binn ### self.aicontrol.drive ([0,1,0,0,0,0]) ### move -> drop cover ### rospy.sleep(0.1) # self.hw.command ('gripper', 'leave') ### leave cover alone self.aicontrol.drive ([0,-1,0,0,0,0]) ### move back to above bin ### rospy.sleep(0.1) self.aicontrol.drive_z (-2.8) ''' ## drop x2 times self.aicontrol.drive_z (const.BIN_DROP_DEPTH) self.aicontrol.stop (2) self.hw.command('drop_left', 'drop') self.aicontrol.stop (2) print 'drop laew eiei' rospy.sleep(1) self.aicontrol.drive_y (0.3) rospy.sleep(1) self.hw.command('drop_right', 'drop') rospy.sleep(1) print 'drop marker yet' print 'bin complete' return
class BouyMission (object): def __init__ (self): print "Now do bouy" print 'eiei' #### BOUY ## subscribe vision bouy_srv = 'find_obj' ### P'Ink service rospy.wait_for_service(bouy_srv) print 'service starts bouy srv' self.detect_bouy = rospy.ServiceProxy(bouy_srv, vision_srv) #### BOUY #### PATH path_srv = 'vision2' rospy.wait_for_service(path_srv) print 'service starts path srv' self.detect_path = rospy.ServiceProxy(path_srv, vision_srv) #### PATH self.aicontrol = AIControl() self.hw = Hardware() self.oldx = 0 self.oldy = 0 def check_point(self, newx, newy): if abs(self.oldx - newx) < 0.1 and abs(self.oldy - newy) < 0.1: return True else: return False def run (self): red_bouy = self.detect_bouy(String('bouy'),String('red')) red_bouy = red_bouy.data rospy.sleep(2) green_bouy = self.detect_bouy(String('bouy'),String('green')) green_bouy = green_bouy.data rospy.sleep(2) bouy_color = ['red', 'green', 'yellow'] if green_bouy.appear: move = 0 while red_bouy.appear == False and move != 10: self.aicontrol.drive_y (0.5) red_bouy = self.detect_bouy(String('bouy'),String('red')) red_bouy = red_bouy.data rospy.sleep(2) move += 1 if red_bouy.appear: print 'found red bouy' self.aicontrol.stop (2) for i in xrange(2): print 'will hit ' + bouy_color[i] count = 20 self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) ############# CHANGE ME !!!! print 'drive z const complete' while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : now_pose = self.aicontrol.get_pose() bouy_data = self.detect_bouy(String('bouy'),String(bouy_color[i])) bouy_data = bouy_data.data print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 400 : ### near ### print 'near' bc = 0.05 sr = 0.2 else : ### far ### print 'far' bc = 0.1 sr = 0.4 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 600: ### CHANGE ME !!! print 'go to bouy' print 'drive_x 3 meter' self.aicontrol.drive_x (3) rospy.sleep(2) break else: print 'drive_x 0.2 meter so far' self.aicontrol.drive_x (0.2) rospy.sleep(2) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.stop(0.2) count -= 1 ### end while ### if self.aicontrol.is_fail(count): self.aicontrol.drive_x (2) move = 0 self.aicontrol.stop (1) print 'stop state after hit bouy' print 'backward' print 'go to set point' if i == 1: xx = -2 else: xx = -3 self.aicontrol.drive_x (xx) rospy.sleep(5) if i == 0: self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) self.aicontrol.drive_y (-3) print 'slide right 3 meter' green_bouy = self.detect_bouy(String('bouy'),String('green')) green_bouy = green_bouy.data rospy.sleep(2) while green_bouy.appear == False and move != 20: self.aicontrol.drive_y (-0.1) rospy.sleep(0.5) green_bouy = self.detect_bouy(String('bouy'),String('green')) green_bouy = green_bouy.data rospy.sleep(1) move += 1 if not self.check_point(green_bouy.x, green_bouy.y): if self.oldx > green_bouy.x: self.aicontrol.drive ([0,bouy_data.x,bouy_data.y,0,0,0]) rospy.sleep (2) else: self.aicontrol.drive ([0,self.oldx,self.oldy,0,0,0]) rospy.sleep (2) if self.oldy > green_bouy.y: truey = self.oldy truex = self.oldx self.oldy = green_bouy.y self.oldx = green_bouy.x elif i == 1: self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) yellow_bouy = self.detect_bouy(String('bouy'),String('yellow')) yellow_bouy = yellow_bouy.data rospy.sleep(2) self.aicontrol.drive_y(1.2) while yellow_bouy.appear == False and move != 10: self.aicontrol.stop (2) yellow_bouy = self.detect_bouy(String('bouy'),String('yellow')) yellow_bouy = yellow_bouy.data self.aicontrol.drive_y (0.1) rospy.sleep(1) move += 1 rospy.sleep(3) print 'set point' print 'finish ' + bouy_color[i] ### end for ### print 'finish 2 bouy' self.yellow_bouy () # self.find_path() def find_path (self): self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data count = 80 found = False while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : if path_data.appear: print 'found path' self.aicontrol.stop(2) break else: yy = 0.1 self.aicontrol.drive_x (0.3) for i in xrange(8): self.aicontrol.drive_y (yy) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data if path_data.appear: found = True break if not found: self.aicontrol.drive_y(-0.5) yy = -0.1 for i in xrange(8): self.aicontrol.drive_y (yy) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data if path_data.appear: found = True break count -= 1 if found: return else: self.aicontrol.drive_y (0.5) return def yellow_bouy (self): self.aicontrol.drive_z (-3) print 'do yellow bouy' count = 50 self.hw.command ('gripper', 'close') print 'open gripper' while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : bouy_data = self.detect_bouy(String('bouy'),String('yellow')) bouy_data = bouy_data.data print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 400: ### near ### print 'near' bc = 0.05 sr = 0.3 else : ### far ### print 'far' bc = 0.1 sr = 0.5 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 600: ### change area value print 'go to bouy' print 'drive_x 2 meter' self.aicontrol.drive_x (2) rospy.sleep (1) self.hw.command ('gripper', 'grab') rospy.sleep (1) print 'grab la na eiei' self.aicontrol.drive_z (const.BOUY_YELLOW_PULL_DEPTH) ####### CHANGE ME !!! print 'drive_z complete' print 'pull down' self.hw.command ('gripper', 'close') self.aicontrol.stop (2) print 'release' self.aicontrol.drive_x (-2) self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) self.aicontrol.drive_y (-0.8) break else: print 'drive_x 0.3 meter so far' self.aicontrol.drive_x (0.3) rospy.sleep(0.5) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.stop(0.2) count -= 1 ### end while ### self.aicontrol.stop (3) print 'stop state after hit bouy' self.find_path()
class BouyMission (object): def __init__ (self): print "Now do bouy" print 'eiei' #### BOUY ## subscribe vision bouy_srv = 'find_obj' ### P'Ink service rospy.wait_for_service(bouy_srv) print 'service starts bouy srv' self.detect_bouy = rospy.ServiceProxy(bouy_srv, vision_srv) #### BOUY #### PATH ## subscribe vision bot_srv = 'vision2' rospy.wait_for_service(bot_srv) print 'service starts bot srv' self.detect_path = rospy.ServiceProxy(bot_srv, vision_srv) #### PATH self.aicontrol = AIControl() self.hw = Hardware() def run (self, q): self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) if q == 'A': mul = 1 else: mul = -1 red_bouy = self.detect_bouy(String('bouy'), String('red')) red_bouy = red_bouy.data if not red_bouy.appear: green_bouy = self.detect_bouy(String('bouy'), String('green')) green_bouy = green_bouy.data if green_bouy.appear: move = 0 while red_bouy.appear == False and move != 5: self.aicontrol.drive_y (1*mul) red_bouy = self.detect_bouy(String('bouy'),String('red')) red_bouy = red_bouy.data rospy.sleep(2) move += 1 if red_bouy.appear: print 'found red bouy' else: self.aicontrol.drive_y (-5*mul) ############## self.aicontrol.stop (2) bouy_color = ['red', 'green', 'yellow'] for i in xrange(1): print 'will hit ' + bouy_color[i] count = 20 self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) ############# CHANGE ME !!!! print 'drive z const complete' while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : now_pose = self.aicontrol.get_pose() bouy_data = self.detect_bouy(String('bouy'),String(bouy_color[i])) bouy_data = bouy_data.data print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 900 : ### near ### print 'near' bc = 0.05 sr = 0.2 else : ### far ### print 'far' bc = 0.1 sr = 0.4 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 1000: ### CHANGE ME !!! print 'go to bouy' print 'drive_x 2 meter' self.aicontrol.drive_x (2) break else: print 'drive_x 0.5 meter so far' self.aicontrol.drive_x (1) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.drive_x (0.1) count -= 1 ### end while ### self.aicontrol.stop (1) print 'stop state after hit bouy' if count != 0: self.aicontrol.drive_x (-1) print 'backward' self.aicontrol.drive_y (mul) print 'slide to hit green bouy' self.aicontrol.turn_yaw_relative (90) self.aicontrol.drive_y (-3) self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) self.aicontrol.turn_yaw_relative (-90) else: self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) # print 'backward' # print 'go to set point' # self.aicontrol.drive_x (-3) # rospy.sleep(1) # print 'set point' # print 'finish ' + bouy_color[i] # if i == 0: # move = 0 # green_bouy = self.detect_bouy(String('bouy'),String('green')) # green_bouy = green_bouy.data # while green_bouy.appear == False and move != 5: # self.aicontrol.drive_y (1*-mul) # green_bouy = self.detect_bouy(String('bouy'),String('green')) # green_bouy = green_bouy.data # rospy.sleep(2) # move += 1 # if green_bouy.appear: # print 'found green bouy' # else: # self.aicontrol.drive_y (-5*-mul) # ##### # else: # move = 0 # yellow_bouy = self.detect_bouy(String('bouy'),String('yellow')) # yellow_bouy = yellow_bouy.data # while yellow_bouy.appear == False and move != 5: # self.aicontrol.drive_y (1*-mul) # yellow_bouy = self.detect_bouy(String('bouy'),String('yellow')) # yellow_bouy = yellow_bouy.data # rospy.sleep(2) # move += 1 # if green_bouy.appear: # print 'found yellow bouy' # else: # self.aicontrol.drive_y (-5*-mul) ##### ### end for ### print 'finish 2 bouy' self.aicontrol.drive_x(-1) self.aicontrol.drive_z(const.PATH_DETECTING_DEPTH) self.aicontrol.drive_x (3) if self.aicontrol.is_fail(count): return False else: return True # self.yellow_bouy() def red_bouy (self): def yellow_bouy (self): self.aicontrol.drive_z (const.BOUY_DETECTING_DEPTH) print 'do yellow bouy' count = 20 self.hw.command ('gripper', 'close') print 'open gripper' while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : bouy_data = self.detect_bouy(String('bouy'),String('yellow')) bouy_data = bouy_data.data print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 400: ### near ### print 'near' bc = 0.05 sr = 0.3 else : ### far ### print 'far' bc = 0.1 sr = 0.5 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 600: ### change area value print 'go to bouy' print 'drive_x 2 meter' self.aicontrol.drive_x (2) rospy.sleep (1) self.hw.command ('gripper', 'grab') rospy.sleep (1) print 'grab la na eiei' self.aicontrol.drive_z (const.BOUY_YELLOW_PULL_DEPTH) ####### CHANGE ME !!! print 'drive_z complete' print 'pull down' self.hw.command ('gripper', 'close') self.aicontrol.stop (2) print 'release' self.aicontrol.drive_x (-2) self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) self.aicontrol.drive_y (-0.8) break else: print 'drive_x 1 meter so far' self.aicontrol.drive_x (1) rospy.sleep(0.5) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.stop(0.2) count -= 1 ### end while ### self.aicontrol.stop (3) print 'stop state after hit bouy' self.find_path () def find_path (self): self.aicontrol.drive_z (const.PATH_DETECTING_DEPTH) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data count = 30 found = False while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : if path_data.appear: print 'found path' self.aicontrol.stop(2) break else: yy = 0.1 self.aicontrol.drive_x (1) for i in xrange(8): self.aicontrol.drive_y (yy) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data if path_data.appear: found = True break if not found: self.aicontrol.drive_y(-0.5) yy = -0.1 for i in xrange(8): self.aicontrol.drive_y (yy) path_data = self.detect_path(String('path1'),String('orange')) path_data = path_data.data print path_data if path_data.appear: found = True break count -= 1 return found ''' def slide_to_do_bouy (self): while not rospy.is_shutdown() and not self.aicontrol.is_fail(count) : now_pose = self.aicontrol.get_pose() bouy_data = self.detect_bouy(String('bouy'),String('red') # bouy_data = bouy_data.data # print bouy_data if bouy_data.appear: vx = (1/bouy_data.area)*500 vy = bouy_data.x vz = bouy_data.y if bouy_data.area > 600 : ### near ### print 'near' bc = 0.05 sr = 0.2 else : ### far ### print 'far' bc = 0.1 sr = 0.4 if self.aicontrol.is_center([bouy_data.x,bouy_data.y],-bc,bc,-bc,bc) : print bouy_data if bouy_data.area > 1200: ### CHANGE ME !!! print 'go to bouy' print 'drive_x 3 meter' self.aicontrol.drive_x (3) rospy.sleep(2) break else: print 'drive_x 0.5 meter so far' self.aicontrol.drive_x (0.5) rospy.sleep(2) else : self.aicontrol.drive([0,vy,vz,0,0,0]) print 'set to center' rospy.sleep (sr) else : self.aicontrol.stop(0.2) self.aicontrol.drive_x (0.1) count -= 1 ### end while ### print 'SLIDE TO HIT' self.aicontrol.drive_y (5) self.aicontrol.drive_z (-0.05) return ''' if __name__ == '__main__': print 'do bouy' rospy.init_node('bouy_ai', anonymous=True) self.run() # self.yellow_bouy() print 'RED AND GREEN BOUY COMPLETE !!'