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 !!'
