class Flare(object):
def __init__(self):
print 'now do flare'
## vision service
gate_srv = 'gate'
rospy.wait_for_service(flare_srv)
print 'flare_srv start'
self.detect_gate = rospy.ServiceProxy(flare_srv)
self.check_flare = False
self.pos_center = self.detect_gate.data_gate.pos_flare
self.aicontrol = AIControl()
def bump(self):
auv = self.aicontrol
count = 15
print "mv to center"
while rospy.is_shutdown() and self.aicontrol.isFail(count) :
if self.pos_center >= 152 and self.pos_center <= 168:
print("good aim")
break
elif self.pos_center < 152:
auv.move('right',cons.AUV_SPEED)
elif self.pos_center > 168:
auv.move('left',cons.AUV_SPEED)
count-=1
## move to center
print "move complete"
print "bump!!"
while rospy.is_shutdown() and self.aicontrol.isFail(count)
auv.move('forward',cons.FLARE_SPEED)
## bump flare
auv.stop()
if __init__ == '__main__':
mission_gate = Flare()
mission_gate.bump()
print "finish gate"
#!/usr/bin/python2
from aicontrol import AIControl
import rospy
global aicontrol
if __name__ == '__main__':
aicontrol = AIControl()
cmd = raw_input(
'command \'stop or 1\' , \'depth or 2\' , \'move or 3\' : ')
if cmd == 'stop' or cmd == '1':
aicontrol.stop()
elif cmd == 'depth' or cmd == '2':
pos = int(raw_input('position : '))
aicontrol.depthAbs(pos)
elif cmd == 'move' or cmd == '3':
count = 5
pos = raw_input('pos : ')
speed = int(raw_input('speed : '))
while not rospy.is_shutdown() and not aicontrol.isFail(count):
aicontrol.move(pos, speed)
# rospy.loginfo('x: %f\ny: %f\nz: %f'%(aicontrol.auv_state[0], aicontrol.auv_state[1], aicontrol.auv_state[2]))
count -= 1
aicontrol.stop()
rospy.sleep(1)