from __future__ import print_function
from zeabus.control.command_interfaces import CommandInterfaces
import rospy
import math
if __name__ == "__main__":
rospy.init_node("testing")
name_node = rospy.get_name()
auv_control = CommandInterfaces(name_node)
auv_control.reset_state(0, 0)
auv_control.echo_data()
print("Now I will absolute yaw to north")
auv_control.absolute_yaw(math.pi / 2)
auv_control.echo_data()
print("I will waiting yaw")
while (not rospy.is_shutdown()):
if (auv_control.check_yaw(0.5)):
class Gate:
def __init__(self, gate_proxy):
self.gate_proxy = gate_proxy
self.param = {
'checkDeep': {
'wanted': -0.85,
'acceptableError': 0.10,
},
'firstFinding': {
'threshold': 0.9,
'rotateAngle': 30 * 3.1416 / 180, # Tune value
# Should be ~120 when switch is used.
'maxAngle': 120 * 3.1416 / 180,
},
'forwardToGate': {
'cxThresold': 0.2, # Tune value
'rotateAngle': 10,
'moveDist': 0.25,
'normalDist': 0.5,
# If don't found gat will go to last mode. Use in Step02
'timeLimit': 30,
},
'finalMoveDist': 4.0,
'endThreshold': 0.2,
'visionStatusHistory': 20,
}
self.control = CommandInterfaces('Gate')
listGateStatus = [0]
def setGateStatus(self, status=0):
self.listGateStatus.append(status)
if len(self.listGateStatus) > self.param['visionStatusHistory']:
del self.listGateStatus[0]
def getGateStatus(self):
return sum(self.listGateStatus) / len(self.listGateStatus)
def isEnd(self):
return self.getGateStatus() < self.param['endThreshold']
def step00_checkDeep(self):
rospy.loginfo('RESETING STATE')
self.control.reset_state()
rospy.loginfo('Adjusting deep.')
self.control.absolute_z(self.param['checkDeep']['wanted'])
r = rospy.Rate(10)
while not rospy.is_shutdown():
if self.control.check_z(0.15):
break
r.sleep()
rospy.loginfo('AUV Deep is as wanted.')
return True
def step01_rotateAndFindGate(self):
"""
TODO: check switch (middle switch = ccw, edge switch = cw)
"""
rotate_count = 0
r = rospy.Rate(10)
while not rospy.is_shutdown():
vsResp = self.gate_proxy()
# print(vsResp)
if vsResp.found == 1:
self.setGateStatus(1)
else:
self.setGateStatus(0)
if self.getGateStatus() >= self.param['firstFinding']['threshold']:
self.gateYaw = self.control.current_pose[5]
rospy.loginfo('[LookToTheLeftLookToTheRight] Found gate.')
return True
# Code when switch is available
# if rotate_count > 90:
if rotate_count > self.param['firstFinding']['maxAngle']:
rospy.logwarn('Reach maximum finding angle')
self.gateYaw = 1.570
return False
# rotate command ccw self.param['firstFinding']['rotateAngle'] deg
if self.control.check_yaw(0.0873):
self.control.relative_yaw(
self.param['firstFinding']['rotateAngle'])
rotate_count += abs(self.param['firstFinding']['rotateAngle'])
r.sleep()
return True
def step01_5_lockYawToGate(self):
self.control.absolute_yaw(self.gateYaw)
r = rospy.Rate(10)
rospy.loginfo('Waiting yaw to stable.')
rospy.sleep(5)
rospy.loginfo('Yaw is stable.')
widths = []
cxs = []
rospy.loginfo('Trying to locate the gate.')
while not rospy.is_shutdown():
vsResp = self.gate_proxy()
cxs.append(vsResp.cx1)
widths.append(abs(vsResp.x_left - vsResp.x_right))
if len(cxs) > 50:
break
r.sleep()
rospy.loginfo('Gate is located.')
rospy.loginfo('Adjusting heading.')
self.gateDist = self.estimateDist(sum(widths) / len(widths))
gateAngle = self.estimateAngle(sum(cxs) / len(cxs), self.gateDist)
self.gateYaw += gateAngle
self.control.absolute_yaw(self.gateYaw)
rospy.loginfo('GateDist: %.2fm, GateAngle: %.4f' %
(self.gateDist, gateAngle))
# overide parameter.
if self.gateDist / 3 > self.param['forwardToGate']['moveDist'] * 1.5:
self.param['forwardToGate']['normalDist'] = self.gateDist / 3
def estimateDist(self, width):
realGateW = 3.048 # meters
return 2 * realGateW / width
def estimateAngle(self, cx, dist):
return -math.atan(cx / dist)
def step02_forwardWithMoveLeftRight(self):
"""
if center x is negative and less than ... (might be -0.5) and not in moving phase:
move left until ... (might be -0.1)
elif center x is positive and more than ... (might be 0.5) and not in moving phase:
move right until ... (might be 0.1)
continue_forward_command
"""
curDist = 0
estDist = self.gateDist
start = time.time()
r = rospy.Rate(10)
while not rospy.is_shutdown():
self.param['forwardToGate']['moveDist'] = (
(estDist - curDist)**2) / 50 + 0.25
print("MoveDist: %.2f m" % self.param['forwardToGate']['moveDist'])
vsResp = self.gate_proxy()
if vsResp.found == 1:
self.setGateStatus(1)
else:
self.setGateStatus(0)
if self.control.check_xy(0.15,
0.15) and self.control.check_yaw(0.15):
endCond = ((self.isEnd() and
(time.time() - start >
self.param['forwardToGate']['timeLimit']))
or curDist >= estDist)
if endCond:
rospy.loginfo('[GoToGate] Passed gate.')
return True
rospy.sleep(5)
if vsResp.cx1 < -self.param['forwardToGate']['cxThresold']:
rospy.loginfo("[GoToGate] Too left")
self.control.relative_xy(
self.param['forwardToGate']['normalDist'],
self.param['forwardToGate']['moveDist'])
elif vsResp.cx1 > self.param['forwardToGate']['cxThresold']:
rospy.loginfo("[GoToGate] Too right")
self.control.relative_xy(
self.param['forwardToGate']['normalDist'],
-self.param['forwardToGate']['moveDist'])
else:
rospy.loginfo("[GoToGate] Forward")
self.control.relative_xy(
self.param['forwardToGate']['normalDist'], 0)
curDist += self.param['forwardToGate']['normalDist']
r.sleep()
def step03_moveForward(self):
rospy.loginfo(
'[MoveMore] I need more move to passed the gate all of robot.')
self.control.relative_xy(self.param['finalMoveDist'], 0)
r = rospy.Rate(10)
while not rospy.is_shutdown():
if self.control.check_xy(0.15, 0.15):
break
r.sleep()
rospy.loginfo('[MoveMore] Passed gate.')
return True
class Buoy:
def __init__(self):
self.vision = AnalysisBuoy("base_buoy")
self.control = CommandInterfaces("BUOY")
self.rate = rospy.Rate(5)
self.status_mission = 0
self.ok_count = 5
def start_mission(self): # status_mission is 0
self.control.reset_state()
self.control.publish_data("START Waiting z depth")
self.control.absolute_z(BUOY_START_DEPTH)
while (not self.control.check_z(0.15)):
self.rate.sleep()
self.control.publish_data("START Waiting yaw ok")
while (not self.control.check_yaw(0.15)):
self.rate.sleep()
self.control.publish_data("Start mision find target")
self.vision.call_data()
self.vision.echo_data()
if (self.vision.result['found']):
temp_y = self.vision.result['center_x'] * -2 / 100
self.control.publish_data(
"Find target change to lock target and survey " + str(temp_y))
self.control.relative_xy(2, temp_y)
self.lock_target()
else:
self.find_target()
def find_target(self): # status_mission = 1
# Please thinking about plus sign to estimate value
self.control.publish_data("Welcome to mode find_target")
self.control.publish_data(
"Find target Have to waiting depth before go to find target")
while (not self.control.check_z(0.15)):
self.rate.sleep()
relative_y = -0.8
relative_x = 1
self.control.publish_data("Find target move relative y " +
str(relative_y))
self.control.relative_xy(0, relative_y)
self.control.sleep()
relative_y *= -2
# mode 0 is forward , mode 1 is survey
mode = 1
while (not rospy.is_shutdown()):
self.rate.sleep()
count = 0
while ((not rospy.is_shutdown()) and (count < BUOY_FOUND_PICTURE)):
self.vision.call_data()
self.vision.echo_data()
if (self.vision.result['found']):
self.control.publish_data("FIND_TARGET Found picture")
count += 1
else:
self.control.publish_data(
"FIND_TARGET don't found picture")
break
if (count == BUOY_FOUND_PICTURE):
self.control.publish_data(
"Find target Found 2 round move to mode lock_target")
break
if (not (self.control.check_xy(0.15, 0.15)
and self.control.check_yaw(0.15))):
continue
if (mode == 0):
self.control.publish_data("Find target forward " +
str(relative_x) + " meter")
self.control.relative_xy(relative_x, 0)
self.control.sleep()
mode = 1
else:
mode = 0
self.control.publish_data("Find target survey " +
str(relative_y) + " meter")
self.control.relative_xy(0, relative_y)
self.control.sleep()
relative_y *= -1
self.control.publish_data("FIND_TARGET reset position xy")
self.control.relative_xy(0, 0)
self.lock_target()
def lock_target(self): # status_mission = 2
self.control.publish_data("Welcome to mode lock target")
unfound = 0
distance = 3
warning_time = 5
time_out = BUOY_TIME_LOCK_TARGET # time_out seconds when you have lock target
self.control.publish_data("You have limit time in mode lock_target " +
str(time_out))
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.deactivate(["x", "y"])
while ((not rospy.is_shutdown()) and (diff_time < time_out)):
self.control.publish_data(
"Lock Target mode lock_target call vision")
self.vision.call_data()
self.vision.echo_data()
relative_x = 0
relative_y = 0
if (self.vision.result['found']):
unfound = 0
if (self.vision.result['center_x'] > 20):
relative_y = TARGET_RIGHT
elif (self.vision.result['center_x'] < -20):
relative_y = TARGET_LEFT
else:
relative_x = SUPER_FORWARD
self.control.publish_data(
"Lock Target command force ({:4.2f},{:4.2f})".format(
relative_x, relative_y))
self.control.force_xy(relative_x, relative_y)
if (self.vision.result['area'] > BUOY_AREA_ABORT):
self.control.publish_data("Break from area condition")
break
else:
unfound += 1
self.control.publish_data("Don't found target")
if (unfound == 2):
self.control.publish_data("Move to dash_mode")
break
diff_time = (rospy.get_rostime() - start_time).to_sec()
if ((diff_time / warning_time) > 1):
self.control.publish_data(
"Warning over {:6.2f} limit at {:6.2f}".format(
warning_time, time_out))
warning_time += 5
self.dash_mode(distance)
self.control.force_xy(0, 0)
self.rate.sleep()
self.control.publish_data("Finish dash mode move back")
self.finish_task()
self.control.activate(["x", "y"])
def dash_mode(self, distance):
self.control.force_xy(1, 0, True)
start_time = rospy.get_rostime()
self.control.publish_data("DASH Move forward limit time at " +
str(BUOY_LIMIT_TIME))
diff_time = (rospy.get_rostime() - start_time).to_sec()
while ((not rospy.is_shutdown()) and (diff_time < BUOY_LIMIT_TIME)):
self.rate.sleep()
diff_time = (rospy.get_rostime() - start_time).to_sec()
temp = self.control.force_xy(SUPER_FORWARD, 0)
self.control.publish_data("DASH Now distance is " + str(temp) +
" and time is " + str(diff_time))
def finish_task(self):
self.rate.sleep()
self.control.publish_data("FINISH this task, Move back")
self.control.absolute_z(BUOY_TARGET_DEPTH_FINISH)
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while ((not rospy.is_shutdown()) and diff_time < BUOY_TIME_TO_BACK):
self.rate.sleep()
self.control.force_xy(-1.0 * BUOY_FORCE_FORWARD, 0)
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.publish_data("FINISH back current , limit " +
repr((diff_time, BUOY_TIME_TO_BACK)))
self.control.publish_data("FINISH Survey right")
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while ((not rospy.is_shutdown()) and diff_time < BUOY_TIME_TO_SURVEY):
self.rate.sleep()
self.control.force_xy(0, BUOY_FORCE_SURVEY)
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.publish_data("FINISH survey current , limit " +
repr((diff_time, BUOY_TIME_TO_SURVEY)))
self.control.force_false()
self.control.publish_data("FINISH Waiting depth")
while (not self.control.check_z(0.15)):
self.rate.sleep()
self.control.publish_data("FINISH Forward")
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while ((not rospy.is_shutdown()) and diff_time <
(BUOY_TIME_TO_BACK + 5)):
self.rate.sleep()
self.control.force_xy(BUOY_FORCE_FORWARD, 0)
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.publish_data("FINISH forward current , limit " +
repr((diff_time, BUOY_TIME_TO_BACK + 5)))
self.control.force_xy(0, 0)
self.control.publish_data("Finish task buoy?")
class StrategySpeed:
def __init__( self ):
self.control = CommandInterfaces( "strategy" )
self.control.publish_data( "Waiting service name /vision/gate" )
rospy.wait_for_service('/vision/gate')
self.rate = rospy.Rate( 10 )
# Step setup mission Gate
self.mission_gate = Gate()
# Step setup mission Path
self.mission_path = Path()
self.vision_path = AnalysisPath()
# Step setup mission Buoy
self.mission_buoy = Buoy()
self.vision_buoy = AnalysisBuoy()
# Step setup mission Drop
self.mission_drop = Drop()
self.vision_drop = AnalysisDrop()
# Step setup mission Exposed analysis coffin
self.mission_exposed = Exposed()
self.vision_coffin = AnalysisCoffin( "base_coffin" )
self.mission_stake = Stake()
self.vision_stake = AnalysisStake( "base_stake" )
self.current_play = False
# Step setup service of strategy_mission
self.service_service = rospy.Service(
'/mission/strategy' , SendBool() , self.callback_service )
self.control.publish_data( "Waiting command to run mission")
while( not rospy.is_shutdown() ):
if( self.current_play ):
self.main()
self.current_play = False
break
self.rate.sleep()
# Start part of all mission
def main( self ):
self.control.reset_state()
self.control.publish_data( "STRATEGY I will start run mission now")
self.mission_gate.start_mission()
self.control.publish_data( "STRATEGY Finish gate next forward and found path")
# This step will use to movement forward
self.control.update_target()
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.deactivate( ['x' , 'y' ] )
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
while( ( not rospy.is_shutdown() ) and diff_time < STRATEGY_TIME_SURVEY_PATH ):
self.rate.sleep()
self.control.force_xy( 0 , STRATEGY_FORCE_SURVEY_PATH )
self.control.publish_data( "STRATEGY survey before path " + str(diff_time) )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
count = 0
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
if STRATEGY_NO_PATH :
self.control.publish_data( "STRATEGY no play path")
temp_yaw = 0
if STRATEGY_FIX_YAW_GATE :
self.control.update_target()
temp_yaw = self.control.target_pose[5]
self.control.publish_data( "STRATEGY remember yaw is " + str( temp_yaw ) )
while( ( not rospy.is_shutdown() ) and diff_time < STRATEGY_TIME_GATE_PATH ):
self.rate.sleep()
self.vision_path.call_data()
self.vision_path.echo_data()
if( self.vision_path.num_point != 0 ):
count += 1
self.control.force_xy( 0 , 0 )
else:
count = 0
self.control.force_xy( STRATEGY_FORCE_GATE_PATH , 0 )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data( "STRATEGY forward time is " + str( diff_time ) )
if( count == 3 ):
if STRATEGY_NO_PATH :
self.control.publish_data( "!!!!! STRATEGY_NO_PATH FIND PATH !!!!!!!!")
self.control.force_xy( 0 , 0 )
break
self.control.publish_data("!!!!!!!!! STRATEGY FIND PATH !!!!!!!!!!!!!" )
target_depth = -1.0
count_unfound = 0
while( not rospy.is_shutdown() ):
self.rate.sleep()
self.vision_path.call_data()
self.vision_path.echo_data()
relative_x = 0
relative_y = 0
ok_x = False
ok_y = False
if( self.vision_path.num_point == 0 ):
rospy.logfatal("STRATGY path disappear noooooooo")
relative_y = 0
count = 0
relative_y = 0
count_unfound += 1
if count_unfound == 3:
break
continue
elif( self.vision_path.x_point[0] > 30 ):
relative_y = SURVEY_RIGHT
elif( self.vision_path.x_point[0] < -30 ):
relative_y = SURVEY_LEFT
else:
ok_y = True
count_unfound = 0
if( self.vision_path.num_point == 0):
relative_x = 0
elif( self.vision_path.y_point[0] > 20 ):
relative_x = SURVEY_FORWARD
elif( self.vision_path.y_point[0] < -20 ):
relative_x = SURVEY_BACKWARD
else:
ok_x = True
if( ok_x and ok_y ):
self.control.force_xy( 0 , 0 )
if( self.control.check_z( 0.15 ) ):
if( target_depth < PATH_TARGET_DEPTH ):
self.control.publish_data( "STRATEGY Breaking and setup point")
break
else:
target_depth -= 0.5
self.control.publish_data( "STRATEGY I command depth to "
+ str( target_depth ) )
self.control.absolute_z( target_depth )
else:
self.control.publish_data( "STRATEGY command "
+ repr(( relative_x , relative_y ) ) )
self.control.force_xy( relative_x , relative_y )
self.control.force_xy( 0 , 0 )
break
self.control.relative_xy( 0 , 0 )
self.control.activate( ['x' , 'y'] )
# Part to move forward for path If you see path
if( count == 3 ) and not STRATEGY_NO_PATH:
self.control.publish_data( "STRATEGY start mission path on setup_point" )
self.mission_path.setup_point()
elif STRATEGY_NO_PATH :
self.control.publish_data( "STRATEGY don't play path")
self.control.relative_yaw( STRATEGY_ROTATION_GATE_BUOY )
self.control.sleep()
else:
self.control.publish_data( "STRATEGY don't found picture I will rotation" )
self.control.relative_yaw( STRATEGY_ROTATION_GATE_BUOY )
self.control.sleep()
if STRATEGY_FIX_YAW_GATE :
self.control.publish_data( "STRATEGY command fix yaw same rotation of gate")
self.control.absolute_yaw( temp_yaw )
self.control.sleep()
while not self.control.check_yaw( 0.12 ):
self.rate.sleep()
self.control.publish_data( "STRATEGY waiting yaw before start buoy")
while( not self.control.check_yaw( 0.15 ) ):
self.rate.sleep()
# Part of mission buoy
self.control.publish_data( "STRATEGY Command depth " + str( STRATEGY_DEPTH_BOUY ) )
self.control.absolute_z( STRATEGY_DEPTH_BOUY )
self.control.sleep()
while( ( not self.control.check_z( 0.12 ) ) ):
self.rate.sleep()
self.control.publish_data( "STRATEGY start survey for init do mission buoy" )
self.control.deactivate( ['x' , 'y'] )
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
while( ( not rospy.is_shutdown() ) and diff_time < STRATEGY_TIME_BUOY_SURVEY ):
self.control.force_xy( 0 , STRATEGY_FORCE_BUOY_SURVEY )
self.rate.sleep()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data( "STRATEGY Survey is diff time " + str( diff_time ) )
self.control.force_xy( 0 , 0 )
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
count_found = 0
pass_buoy = False
while( ( not rospy.is_shutdown() ) and diff_time < STRATEGY_TIME_BUOY ):
self.rate.sleep()
self.vision_buoy.call_data()
self.vision_buoy.echo_data()
if( self.vision_buoy.result[ 'found' ] ):
count_found += 1
if( count_found == 3 ):
self.control.publish_data( "STRATEGY I will call mission_buoy lock_target")
self.mission_buoy.lock_target()
pass_buoy = True
else:
self.control.publish_data( "STRATEGY count found buoy is "
+ str( count_found ) )
force_x = 0
force_y = 0
if( self.vision_buoy.result[ 'center_x'] < -30 ):
force_y = SURVEY_LEFT
elif( self.vision_buoy.result[ 'center_x'] > 30 ):
force_y = SURVEY_RIGHT
else:
force_x = TARGET_FORWARD
self.control.publish_data( "Command force_xy is " + repr((force_x , force_y)) )
self.control.force_xy( force_x , force_y )
else:
self.control.publish_data( "STRATEGY I not found bouy and time is "
+ str( diff_time ) )
count_found = 0
self.control.force_xy( STRATEGY_FORCE_BUOY , 0 )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.activate( ['x' , 'y'] )
if( not pass_buoy ):
self.control.publish_data( "STRATEGY will give process to find_target")
self.mission_buoy.find_target()
else:
self.control.publish_data( "STRATEGY Finish buoy I will go next for path")
# Start path move forward and searching
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
if STRATEGY_NO_PATH :
self.control.publish_data( "STRATEGY Don't play path")
else:
self.control.publish_data( "STRATEGY command depth -1 meter")
self.control.absolute_z( -1 )
self.control.deactivate( ['x' , 'y' ] )
count = 0
while( ( not rospy.is_shutdown() ) and diff_time < STRATEGY_TIME_BUOY_PATH ):
self.rate.sleep()
self.vision_path.call_data()
self.vision_path.echo_data()
if( self.vision_path.num_point != 0 ):
count += 1
self.control.force_xy( 0 , 0 )
else:
self.control.force_xy( STRATEGY_FORCE_BUOY_PATH , 0 )
count = 0
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data( "STRATEGY forward time is " + str( diff_time ) )
if( count == 4 ):
if STRATEGY_NO_PATH :
self.control.publish_data("!!!!!!! STRATEGY_NO_PATH Find path !!!!!!!!")
self.control.force_xy( 0 , 0 )
break
self.control.publish_data("!!!!!!!!! STRATEGY FIND PATH !!!!!!!!!!!!!" )
target_depth = -1.0
count_unfound = 0
while( not rospy.is_shutdown() ):
self.rate.sleep()
self.vision_path.call_data()
self.vision_path.echo_data()
relative_x = 0
relative_y = 0
ok_x = False
ok_y = False
if( self.vision_path.num_point == 0 ):
rospy.logfatal("STRATGY path disappear noooooooo")
count = 0
relative_y = 0
count_unfound += 1
if count_unfound == 3:
break
continue
elif( self.vision_path.x_point[0] > 30 ):
relative_y = TARGET_RIGHT
elif( self.vision_path.x_point[0] < -30 ):
relative_y = TARGET_LEFT
else:
ok_y = True
count_unfound = 0
if( self.vision_path.num_point == 0):
relative_x = 0
elif( self.vision_path.y_point[0] < -60 ):
relative_x = TARGET_BACKWARD * 2.0
elif( self.vision_path.y_point[0] > 20 ):
relative_x = TARGET_FORWARD
elif( self.vision_path.y_point[0] < -20 ):
relative_x = TARGET_BACKWARD
else:
ok_x = True
if( ok_x and ok_y ):
self.control.force_xy( 0 , 0 )
if( self.control.check_z( 0.15 ) ):
if( target_depth < PATH_TARGET_DEPTH ):
self.control.publish_data( "Breaking and go to setup point")
break
else:
target_depth -= 0.5
self.control.publish_data( "STRATGY I command depth to "
+ str( target_depth ) )
self.control.absolute_z( target_depth )
else:
self.control.publish_data( "STRATEGY command "
+ repr(( relative_x , relative_y ) ) )
self.control.force_xy( relative_x , relative_y )
self.control.force_xy( 0 , 0 )
break
# End part to search parh
self.control.activate( ['x' , 'y'] )
self.control.relative_xy( 0 , 0 )
self.control.sleep()
if( count == 4 ) and not STRATEGY_NO_PATH:
self.control.publish_data( "I start path by ever found path" )
self.mission_path.setup_point()
elif STRATEGY_NO_PATH :
self.control.publish_data( "STRATEGY No play path")
self.control.relative_yaw( STRATEGY_ROTATION_BUOY_DROP )
self.control.sleep()
else:
self.control.publish_data( "Try to survey find path")
if( self.mission_path.find_path() ):
self.control.publish_data( "Good luck You finish second path")
else:
self.control.publish_data( "It bad you failure mission path rotation to drop" )
self.control.relative_yaw( STRATEGY_ROTATION_BUOY_DROP )
# Start part for search drop garliac mission
self.control.activate( ['x' , 'y'] )
self.control.sleep()
self.control.absolute_z( STRATEGY_DEPTH_FIND_DROP )
self.control.sleep()
self.control.publish_data( "STRATEGY Command depth at "
+ str( STRATEGY_DEPTH_FIND_DROP - 0.5) )
while( not self.control.check_z( 0.15 ) ):
self.rate.sleep()
self.control.absolute_z( STRATEGY_DEPTH_FIND_DROP )
self.control.publish_data( "STRATEGY Command depth at " + str(STRATEGY_DEPTH_FIND_DROP))
self.control.publish_data( "Waiting yaw")
while( not self.control.check_yaw( 0.12 ) ):
self.rate.sleep()
self.control.deactivate( ['x' , 'y'] )
self.control.force_xy( STRATEGY_FORCE_DROP , 0 )
# Move free move
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data( "STRATEGY Move free time for drop")
while( (not rospy.is_shutdown() ) and diff_time < STRATEGY_FREE_TIME_DROP ):
self.rate.sleep()
self.control.force_xy( STRATEGY_FREE_FORCE_DROP , 0 )
self.control.publish_data( "STRATEGY Move free time is "
+ repr( ( diff_time , STRATEGY_FREE_TIME_DROP ) ) )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data( "START FORWARD TO FIND DROP MISSION" )
count_found = 0
self.control.force_xy( STRATEGY_FORCE_DROP , 0 , True )
while( ( not rospy.is_shutdown() ) and diff_time < STRATEGY_TIME_DROP ):
self.rate.sleep()
self.vision_drop.call_data( DROP_FIND_TARGET )
self.vision_drop.echo_data()
if( self.vision_drop.result['found'] ):
count_found += 1
if( count_found == 5 ):
self.control.publish_data( "STRATEGY Focuse on target")
target_depth = STRATEGY_DEPTH_FIND_DROP
self.control.force_xy( 0 , 0 )
while( ( not rospy.is_shutdown() ) and count_found > 0 ):
self.rate.sleep()
self.vision_drop.call_data( DROP_FIND_TARGET )
self.vision_drop.echo_data()
if( self.vision_drop.result['found'] ):
count_found = 5
else:
count_found -= 1
self.control.publish_data( "Don't found picture "
+ str( count_found ) )
continue
force_x = 0
force_y = 0
if( ( abs( self.vision_drop.result['center_x'] ) > 15 ) or
( abs( self.vision_drop.result['center_y'] ) > 15 ) ):
if( self.vision_drop.result['center_y'] < -15 ):
force_x = TARGET_BACKWARD
elif( self.vision_drop.result['center_y'] > 15 ):
force_x = TARGET_FORWARD
else:
pass
if( self.vision_drop.result['center_x'] > 15 ):
force_y = SURVEY_RIGHT
elif( self.vision_drop.result['center_x'] < -15 ):
force_y = SURVEY_LEFT
else:
pass
self.control.publish_data("Command force {:6.3f} , {:6.3f}".format(
force_x , force_y ) )
self.control.force_xy( force_x , force_y )
else:
if( self.control.check_z( 0.12 ) ):
if( target_depth > DROP_START_DEPTH ):
self.control.publish_data( "Command to depth at "
+ str( DROP_START_DEPTH ) )
self.control.absolute_z( DROP_START_DEPTH )
target_depth = DROP_START_DEPTH
else:
self.control.publish_data( "Depth is ok")
break
else:
self.control.force_xy( 0 , 0 )
self.control.publish_data( "Now center waiting detph")
break
else:
count_found = 0
distance = self.control.force_xy( STRATEGY_FORCE_DROP , 0 )
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.publish_data(
"time ( {:6.3f} , {:6.3f} ) and distance ( {:6.3f} , {:6.3f} ) found ".format(
diff_time , STRATEGY_TIME_DROP , distance , STRATEGY_DISTANCE_DROP )
+ str( count_found ) )
if( distance > STRATEGY_DISTANCE_DROP ):
self.control.publish_data( "Abort to find drop by distance")
break
self.control.activate( ['x' , 'y' ] )
self.control.relative_xy( 0 , 0 )
if( count_found > 0 ):
self.control.publish_data( "Found picture next play drop by operator function")
self.mission_drop.operator()
else:
self.control.publish_data( "Don't found drop" )
if STRATEGY_CHOICE_PROCESS == 0 :
self.control.publish_data( "STRATEGY to choose process last mission by don't use dvl")
self.not_use_dvl()
elif STRATEGY_CHOICE_PROCESS == 1 :
self.control.publish_data( "STRATEGY to choose process last mission by use DVL")
self.use_dvl()
else:
self.control.publish_data( "STRATEGY to choose process last missiob by use pinger")
# End part of play all mission
self.control.publish_data( "Finish all strategy mission" )
self.control.deactivate( ("x", "y", "z", "roll", "pitch", "yaw") )
def callback_service( self , request ):
if( request.data == True ): # Want to play
if( not self.current_play ):
self.current_play = True
else:
rospy.logfatal( "Warning node alredy play you can't do that" )
else:
rospy.loginfo( "Service call to code node")
rospy.signal_shutdown( "Service call to close or stop mission")
return SendBoolResponse()
def use_dvl( self ):
self.control.publish_data( "STRATEGY use DVL start mission by target at exposed")
self.control.update_target()
collective_target_yaw = zeabus_math.bound_radian(
self.control.target[ 5 ] + STRATEGY_ROTATION_STAKE )
self.control.absolute_z( STRATEGY_EXPOSED_FIND )
self.control.publish_data( "STRATEGY command depth to " + str( STRATEGY_EXPOSED_FIND ) )
self.control.sleep()
self.control.relative_yaw( STRATEGY_ROTATION_EXPOSED )
self.control.publish_data( "STRATEGY rotation for find target coffin to exposed " + str(
STRATEGY_ROTATION_EXPOSED ) )
self.control.sleep()
while not self.control.check_yaw( 0.15 ):
self.rate.sleep()
self.control.publish_data( "STRATEGY distance survey is " + str( STRATEGY_DISTANCE_SURVEY ) )
self.control.relative_xy( 0 , STRATEGY_DISTANCE_SURVEY )
self.control.sleep()
while not self.control.check_xy( 0.15 , 0.15 ):
self.rate.sleep()
self.control.publish_data( "STRATEGY distance forward is " + str(STRATEGY_DISTANCE_FORWARD) )
self.control.relative_xy( STRATEGY_DISTANCE_FORWARD , 0 )
self.control.update_target()
collective_point = ( self.control.target_pose[0] , self.control.target_pose[1] )
play_mission = False
count_found = 0
while not self.control.check_xy( 0.15 , 0.15 ) :
self.rate.sleep()
self.vision_coffin.call_data()
self.vision_coffin.echo_data()
if self.vision_coffin.result['num_object'] > 0 :
count_found += 1
if count_found == 3:
self.control.publish_data( "STRATEGY Find coffind stop survey and make center")
self.control.deactivate( ('x' , 'y' ) )
count_unfound = 0
while ( not rospy.is_shutdown() ) and count_unfound < 3 :
self.rate.sleep()
self.vision_coffin.call_data()
self.vision_coffin.echo_data()
if self.vision_coffin.result['num_object'] > 0 :
count_unfound = 0
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if self.vision_coffin.result['center_x'] < -20 :
force_y = SURVEY_LEFT
elif self.vision_coffin.result['center_x' ] > 20 :
force_y = SURVEY_RIGHT
else:
ok_y = True
if self.vision_coffin.result['center_y'] < -20 :
force_x = SURVEY_BACKWARD
elif self.vision_coffin.result['center_y'] > 20 :
force_x = SURVEY_FORWARD
else:
ok_x = True
if ok_x and ok_y :
count_found = 3
self.control.publish_data( "STRATEGY now center point of coffin")
self.control.force_xy( 0 , 0 )
break
else:
self.control.force_xy( force_x , force_y )
self.control.publish_data( "STRATEGY command force "
+ repr( (force_x , force_y ) ) )
else:
count_unfound += 1
count_found = 0
self.control.activate( ('x' , 'y' ) )
if count_unfound == 3 :
self.control.publish_data( "STRATEGY think target it is noise continue")
self.control.absolute_xy( collective_point[0] , collective_point[1] )
self.control.sleep()
else:
break
else:
self.control.publish_data( "STRATEGY found coffin object is "
+ str( count_found ))
else:
count_found = 0
result = False
if count_found == 3 :
self.mission_exposed.operator()
result = True
else:
result = self.mission_exposed.find()
if result:
self.control.publish_data( "STRATEGY finish play exposed consider try to play stake")
self.control.absolute_yaw( collective_target_yaw )
self.control.publish_data( "STRATEGY command target yaw is "
+ str( collective_target_yaw ) )
self.control.absolute_z( STRATEGY_STAKE_DEPTH )
self.control.publish_data( "STRATEGY command depth to "
+ str( STRATEGY_STAKE_DEPTH ) )
self.control.sleep()
while not self.control.check_yaw( 0.15 ):
self.rate.sleep()
self.control.relative_xy( 0 , STRATEGY_STAKE_DISTANCE_SURVEY )
self.control.publish_data( "STRATEGY command survey is "
+ str( STRATEGY_STAKE_DISTANCE_SURVEY ) )
self.control.sleep()
while not self.control.check_xy( 0.15 ):
self.rate.sleep()
self.control.relative_xy( STRATEGY_STAKE_DISTANCE_FORWARD , 0 )
self.control.publish_data( "STRATEGY command forward is "
+ str( STRATEGY_STAKE_DISTANCE_FORWARD ) )
self.control.sleep()
self.control.update_target()
collective_point = ( self.control.target_pose[0] , self.control.target_pose[1] )
count_found = 0
while ( not self.control.check_xy( 0.15 ) ) and count_found < 3 :
self.rate.sleep()
self.vision_stake.call_data( STAKE_FIND_TARGET )
self.vision_stake.echo_data()
if self.vision_stake.result['found'] :
count_found += 1
if count_found == 3 :
self.control.publish_data( "STRATEGY Now found target stop and focus")
count_unfound = 0
self.control.deactivate( ('x' , 'y' ) )
while ( not rospy.is_shutdown() ) and count_unfound < 3 :
self.rate.sleep()
self.vision_stake.call_data( STAKE_FIND_TARGET )
self.vision_stake.echo_data()
if self.vision.result['found'] :
count_unfound = 0
force_x = 0
force_y = 0
ok_x = False
ok_z = False
if self.vision.result['center'][0] > 10 :
force_y = TARGET_LEFT
elif self.vision.result['center'][0] < -10 :
force_y = TARGET_RIGHT
else:
ok_x = True
if self.vision.result['area'] < STRATEGY_STAKE_AREA_FOCUS / 1.5:
force_x = TARGET_FORWARD
else:
ok_z = True
if ok_x and ok_z :
count_found = 3
self.control.force_xy( 0 , 0 )
self.mission_stake.operator()
break
else:
self.control.force_xy( force_x , force_y )
self.control.publish_data("STRATEGY command force is " + repr(
force_x , force_y ) )
else:
count_unfound += 1
self.control.activate( ('x' , 'y' ) )
if count_unfound == 3:
self.control.publish_data( "STRATEGY target disapper" )
self.control.absolute_xy( collective_point[0] , collective_point[1] )
self.control.sleep()
else:
self.control.publish_data( "STRATEGY Give process to stake")
else:
self.control.publish_data( "STRATEGY found target " + str( count_found ) )
else:
count_found = 0
else:
self.control.publish_data( "STRATEGY don't continue to play because exposed false")
def not_use_dvl( self ):
self.control.publish_data( "STRATEGY not use DVL start mission by target at exposed")
self.control.update_target()
collective_target_yaw = zeabus_math.bound_radian(
self.control.target[5] + STRATEGY_ROTATION_STAKE )
self.control.absolute_z( STRATEGY_EXPOSED_FIND )
self.control.publish_data( "STRATEGY command depth to " + str( STRATEGY_EXPOSED_FIND ) )
self.control.sleep()
self.control.relative_yaw( STRATEGY_ROTATION_EXPOSED )
self.control.publish_data( "STRATEGY rotation for find target coffin to exposed " + str(
STRATEGY_ROTATION_EXPOSED ) )
self.control.sleep()
while not self.control.check_yaw( 0.12 ):
self.rate.sleep()
while not self.control.check_z( 0.12 ):
self.rate.sleep()
self.control.deactivate( ('x', 'y') )
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
while not rospy.is_shutdown() and diff_time < STRATEGY_TIME_SURVEY:
self.rate.sleep()
self.control.force_xy( 0 , STRATEGY_FORCE_SURVEY )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data( "STRATEGY Survey for expose on time " + str( diff_time ) )
self.control.force_xy( 0 , 0 )
self.control.activate( ('x' , 'y' ) )
self.control.publish_data( "STRATEGY Waiting yaw before forward")
while not self.control.check_yaw( 0.12 ) :
self.rate.sleep()
self.control.deactivate( ('x' , 'y') )
self.control.publish_data( "STRATEGY forward to find exposed" )
count_found = 0
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
while (not rospy.is_shutdown()) and diff_time < STRATEGY_TIME_FORWARD and count_found < 5 :
self.rate.sleep()
self.vision_coffin.call_data()
self.vision_coffin.echo_data()
if self.vision.result['num_object'] > 0 :
count_found += 1
force_x = 0
force_y = 0
ok_x = False
ok_y = False
self.control.publish_data( "STRATEGY found target command force "
+ repr( (force_x , force_y ) ) )
if self.vision_coffin.result['center_x'] < -20 :
force_y = SURVER_LEFT
elif self.vision_coffin.result['center_x'] > 20 :
force_y = SURVEY_RIGHT
else:
ok_y = True
if( self.vision_coffin.result['center_y'] < -20 ):
force_x = SURVEY_BACKWARD
elif self.vision_coffin.result['center_y'] > 20:
force_x = SURVEY_FORWARD
else:
ok_x = True
if( ok_x and ok_y ):
count_found = 5
self.control.publish_data( "STRATEGY now center point of coffin")
else:
self.control.force_xy( force_x , force_y )
self.control.publish_data( "STRATEGY Have picture command force " + repr( (
force_x , force_y ) ) )
else:
count_found = 0
self.control.force_xy( SURVEY_FORWARD , 0 )
self.control.publish_data("STRATEGY Don't found target command survey forward")
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.activate( [ 'x' , 'y' ] )
result = False
if( count_found == 5 ):
self.control.publish_data( "STRATEGY finish and found object let operator")
self.mission_exposed.operator()
result = True
else:
self.mission_exposed.publish_data( "STRATEGY finish forward search let find object")
result = self.mission_exposed.find()
if result :
self.control.publish_data( "STRATEGY success mission I will try to continue stake" )
self.control.absolute_yaw( collective_target_yaw )
self.control.publish_data( "STRATEGY command absolute yaw "
+ str( collective_target_yaw ) )
self.control.sleep()
while not self.control.check_yaw( 0.12 ):
self.rate.sleep()
self.control.absolute_z( STRATEGY_STAKE_DEPTH )
self.control.publish_data( "STRATEGY command absolute depth is "
+ str( STRATEGY_STAKE_DEPTH ) )
self.control.deactivate( ('x' , 'y' ) )
start_time = rospy.get_rostime()
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data("STRATEGY forward before survey to stake mission")
while ( not rospy.is_shutdown() ) and diff_time < STRATEGY_STAKE_TIME_FORWARD :
self.rate.sleep()
self.control.force_xy( STRATEGY_STAKE_FORCE_FORWARD , 0 )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.control.publish_data( "STRATEGY backward to stake on time " + str(diff_time) )
self.control.activate( ('x' , 'y') )
self.control.force_false()
self.control.publish_data( "STRATEGY waiting yaw")
while not self.control.check_yaw( 0.15 ):
self.rate.sleep()
self.control.deactivate( ( 'x' , 'y' ) )
stat_time = rospy.get_rostime()
diff_time = 0
count_found = 0
while ( not rospy.is_shutdown() ) and diff_time < STRATEGY_STAKE_TIME_SURVEY :
self.rate.sleep()
self.control.force_xy( 0.0 , STRATEGY_STAKE_FORCE_SURVEY )
diff_time = ( rospy.get_rostime() - start_time ).to_sec()
self.vision_stake.call_data()
self.vision_stake.echo_data()
if self.vision_stake.result['found']:
count_found += 1
if count_found == 3 :
self.control.publish_data( "STRATEGY find stake I will target on that")
count_unfound = 0
self.control.force_xy( 0 , 0 )
while not rospy.is_shutdown() and count_unfound < 3 :
self.rate.sleep()
self.vision_stake.call_data( STAKE_FIND_TARGET )
self.vision_stake.echo_data()
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if self.vision_stake.result['found'] :
count_unfound = 0
if self.vision_stake.result['center'][0] > 20 :
force_y = TARGET_RIGHT
elif self.vision_stake.result['center'][1] < -20 :
force_y = TARGET_LEFT
else:
ok_y = True
if self.vision.result['area'] < STRATEGY_STAKE_AREA_FOCUS :
force_x = TARGET_FORWARD
else:
ok_x = True
if ok_x and ok_y :
self.control.force_xy( 0 , 0 )
self.control.publish_data( "STRATEGY get process to stake")
self.mission_stake.operator()
break
else:
self.control.publish_data( "STRATEGY focuse target "
+ repr( ( force_x , force_y ) ) )
else:
count_unfound += 1
self.control.publish_data( "STRATEGY Don't found target "
+ str( count_unfound ) )
self.control.force_xy( 0 , 0 )
if count_unfound == 3:
self.control.publish_data( "Strategy Continue until time out")
count_found = 0
else:
self.control.publish_data( "STRATEGY Finish play break by after play")
break
else:
count_found = 0
self.control.publish_data( "STRATEGY count found " + str( count_found ) )
else:
self.control.publish_data( "STRATEGY faliure mission exposed don't play stake" )
self.control.publish_data( "STRATEGY finish function play not_use_dvl")
class Drop:
def __init__( self ):
self.vision = AnalysisDrop( "base_drop" )
self.control = CommandInterfaces( "DROP")
self.rate = rospy.Rate( 5 )
self.status_mission = 0
def start_mission( self ): # This use in case you only run single mission
self.control.publish_data( "START doing mission drop" )
self.control.reset_state( 0 , 0 )
target_depth = -1
self.control.absolute_z( target_depth )
self.control.publish_data( "START waiting depth")
while( not rospy.is_shutdown() ):
self.rate.sleep()
self.control.publish_data( "START forward TO FIND DROP" )
self.control.deactivate( ['x' , 'y' ] )
found_picture = False
while( not rospy.is_shutdown() ):
self.rate.sleep()
self.vision.call_data( DROP_FIND_TARGET )
self.vision.echo()
if( self.vision.result['found'] ):
force_x = 0
force_y = 0
ok_x = False
ok_y = False
found_picture = True
if( self.vision.result['center_x'] > 20 ):
force_y = -1.4
elif( self.vision.result['center_x'] < -20 ):
force_y = 1.4
else:
ok_y = True
if( self.vision.result['center_y'] > 20 ):
force_x = 1.0
elif( self.vision.result[ 'center_y' ] < -20 ):
force_x = -1.0
else:
ok_x = True
if( ok_x and ok_y ):
if( self.control.check_z( 0.12 ) ):
if( target_depth < -1.5 ):
break
else:
target_depth -= 0.3
self.control.absolute_z( target_depth )
self.control.publish_data("START command depth at "
+ str(target_depth) )
self.command.force_xy( force_x , force_y )
self.control.publish_data( "Command force ( x , y ) : "
+ repr( (force_x , fource_y ) ) )
else:
found_picture = False
self.command.force_xy( 0.8 , 0 )
self.command.force_false()
if( found_picture ):
self.control.publish_data( "Abort mission by command" )
else:
self.control.publish_data( "START Find target give process to operator")
self.control.activate( [ 'x' , 'y' ] )
def operator( self ):
self.control.publish_data( "OPERATOR Welcome to operator mission")
self.control.publish_data( "OPERATOR command z to " + str( DROP_START_DEPTH ) )
self.control.absolute_z( DROP_START_DEPTH )
# This function don't already to use
if( DROP_HAVE_TO_ROTATION ):
None
finish = False
count_unfound = 0
while( ( not rospy.is_shutdown() ) and count_unfound < 3 ):
self.rate.sleep()
self.vision.call_data( DROP_FIND_TARGET )
self.vision.echo_data()
if( self.vision.result['found'] ):
count_unfound = 0
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if( self.vision.result['center_x'] > 20 ):
force_y = -1.5
elif( self.vision.result['center_x'] < -20 ):
force_y = 1.5
else:
ok_y = True
if( self.vision.result['center_y'] > 20 ):
force_x = 1.2
elif( self.vision.result['center_y'] < -20 ):
force_x = -1.2
else:
ok_x = True
if( ok_x and ok_y ):
self.control.publish_data( "OPERATOR Now object is center of frame")
if( self.control.check_z( 0.12 ) ):
if( self.vision.result['type'] ):
self.control.publish_data( "OPERATOR Yes I see 4 point")
self.control.deactivate( ['x' , 'y' , 'yaw' ] )
while( not rospy.is_shutdown() ):
self.rate.sleep()
self.vision.call_data( DROP_FIND_TARGET )
self.vision.echo_data()
if( self.vision.result['rotation'] ):
else:
self.control.publish_data( "OPERATOR we don't see 4 point")
self.control.relative_z( 0.2 )
else:
count_unfound += 1
self.control.publish_data( "OPERATOR Don't found picture")
class Path:
def __init__(self):
self.vision = AnalysisPath("base_path")
self.control = CommandInterfaces("PATH")
self.rate = rospy.Rate(5)
self.status_mission = 0
self.ok_count = 5
def start_mission(self): # status_mission is 0
self.control.publish_data("START doing mission path")
# Reset state
self.control.reset_state(0, 0)
self.control.absolute_z(PATH_START_DEPTH)
self.control.sleep()
self.control.publish_data("START Path waiting depth")
while (not self.control.check_z(0.15)):
self.rate.sleep()
self.find_path()
# Path find will move only left and right please carefull to use this function
def find_path(self):
self.control.publish_data("FIND doing mission to find")
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.deactivate(['x', 'y'])
count = 0
mode = 1 # 1 : left 2 : right 3 : left
while (not rospy.is_shutdown()):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
if (self.vision.num_point != 0):
self.control.get_state()
count += 1
else:
count = 0
if (count == 2):
self.control.publish_data(
"FIND I find path now !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
target_depth = -1.0
real_found = True
count_unfound = 0
while (not rospy.is_shutdown()):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
relative_x = 0
relative_y = 0
ok_x = False
ok_y = False
if (self.vision.num_point == 0):
relative_y = 0
elif (self.vision.x_point[0] > 30):
relative_y = TARGET_RIGHT
elif (self.vision.x_point[0] < -30):
relative_y = TARGET_LEFT
else:
ok_y = True
if (self.vision.num_point == 0):
relative_x = 0
elif (self.vision.y_point[0] > 20):
relative_x = TARGET_FORWARD
elif (self.vision.y_point[0] < -20):
relative_x = TARGET_BACKWARD
else:
ok_x = True
if (ok_x and ok_y):
self.control.force_xy(0, 0)
if (self.control.check_z(0.15)):
if (target_depth < PATH_TARGET_DEPTH):
self.control.publish_data(
"FIND and go to setup point")
break
else:
target_depth -= 0.5
self.control.publish_data(
"FIND command depth to " +
str(target_depth))
self.control.absolute_z(target_depth)
elif self.vision.num_point == 0:
count_unfound += 1
self.control.publish_data("FIND unfound count is " +
str(count_unfound))
if count_unfound == 3:
real_found = False
break
else:
self.control.publish_data("FIND command " +
repr((relative_x,
relative_y)))
self.control.force_xy(relative_x, relative_y)
self.control.force_xy(0, 0)
if (real_found):
break
diff_time = (rospy.get_rostime() - start_time).to_sec()
if (mode == 1):
self.control.force_xy(0.1, SURVEY_RIGHT)
if (diff_time > PATH_FIND_TIME):
self.control.publish_data("FIND mode 1 time out")
mode = 2
start_time = rospy.get_rostime()
else:
self.control.publish_data(
"FIND mode 1 (diff , limit ) : " +
repr((diff_time, PATH_FIND_TIME)))
elif (mode == 2):
self.control.force_xy(0.1, SURVEY_LEFT)
if (diff_time > (PATH_FIND_TIME * 2.0) + 3):
self.control.publish_data("FIND mode 2 time out")
mode = 3
start_time = rospy.get_rostime()
else:
self.control.publish_data(
"FIND mode 2 (diff , limit ) : " +
repr((diff_time, PATH_FIND_TIME * 2.0 + 3)))
elif (mode == 3):
self.control.force_xy(0.1, SUPER_RIGHT)
if (diff_time > (PATH_FIND_TIME + 3)):
self.control.publish_data("FIND mode 3 time out")
mode = 4
else:
self.control.publish_data(
"FIND mode 3 (diff , limit ) : " +
repr((diff_time, PATH_FIND_TIME + 3)))
else:
self.control.force_false()
break
self.control.activate(['x', 'y'])
result = False
if (count == 2):
self.setup_point()
result = True
else:
self.control.publish_data("FIND Don't found path")
result = False
return result
def setup_point(self): # status_mission = 2
self.control.publish_data("SETUP Move to first point")
self.control.deactivate(['x', 'y'])
while ((not rospy.is_shutdown())):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
relative_x = 0
relative_y = 0
ok_x = False
ok_y = False
if (self.vision.num_point == 0):
relative_y = 0
elif (self.vision.x_point[0] > 10):
relative_y = TARGET_RIGHT
elif (self.vision.x_point[0] < -10):
relative_y = TARGET_LEFT
else:
ok_y = True
if (self.vision.num_point == 0):
relative_x = 0
elif (self.vision.y_point[0] > 0):
relative_x = TARGET_FORWARD
elif (self.vision.y_point[0] < -20):
relative_x = TARGET_BACKWARD
else:
ok_x = True
if (ok_x and ok_y):
self.control.publish_data("SETUP xy are ok")
self.control.force_xy(0, 0)
break
else:
self.control.publish_data("SETUP command force x : y === " +
str(relative_x) + " : " +
str(relative_y))
self.control.force_xy(relative_x, relative_y)
self.control.publish_data("SETUP I will deactivate yaw for rotation")
self.control.deactivate(['x', 'y', 'yaw'])
now_tune = False
while (not rospy.is_shutdown()):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
if ((abs(self.vision.x_point[0]) > 80
or abs(self.vision.y_point[0]) > 80) or now_tune):
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if not now_tune:
self.control.deactivate(['x', 'y'])
now_tune = True
if self.vision.x_point[0] > 10:
force_y = TARGET_RIGHT
elif self.vision.x_point[0] < -10:
force_y = TARGET_LEFT
else:
ok_x = True
if self.vision.y_point[0] > 10:
force_x = TARGET_FORWARD
elif self.vision.y_point[0] < -10:
force_x = TARGET_BACKWARD
else:
ok_y = True
self.control.publish_data("SETUP mode rotation force " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
if ok_x and ok_y:
self.control.force_xy(0, 0)
self.control.deactivate(['x', 'y', 'yaw'])
now_tune = False
continue
diff = zeabus_math.bound_radian(self.vision.rotation[0] -
(math.pi / 2))
self.control.publish_data("SETUP diff yaw is " + str(diff))
if (abs(diff) < PATH_OK_DIFF_YAW):
self.control.force_false()
self.control.publish_data("Now rotation are ok")
self.control.sleep()
self.control.activate(['x', 'y', 'yaw'])
break
elif (diff > 0):
self.control.publish_data("SETUP rotation left")
self.control.force_xy_yaw(0, 0.0, PATH_FORCE_YAW)
else:
self.control.publish_data("SETUP rotation right")
self.control.force_xy_yaw(0, 0.0, -1.0 * PATH_FORCE_YAW)
self.control.publish_data(
"SETUP Now rotation finish next is moving on path")
self.control.deactivate(['x', 'y'])
self.moving_on_path()
def moving_on_path(self): # status_mission = 3
# Move to center of path
self.control.publish_data(
"MOVING_ON_PATH move to center of first point")
while ((not rospy.is_shutdown())):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
force_x = 0
force_y = 0
target_point = 0
if (self.vision.x_point[0] < -15):
force_y = TARGET_LEFT
elif (self.vision.x_point[0] > 15):
force_y = TARGET_RIGHT
elif (self.vision.y_point[0] < -15):
force_x = TARGET_BACKWARD
elif (self.vision.y_point[0] > 15):
force_x = TARGET_FORWARD
elif (not self.control.check_yaw(0.12)):
self.control.publish_data(
"MOVING_ON_PATH POINT 1 now center Waiting yaw")
else:
self.control.publish_data(
"MOVING_ON_PATH POINT 1 now center move direct")
start_time = rospy.get_rostime()
while ((not rospy.is_shutdown())
and self.vision.num_point != 3):
self.rate.sleep()
self.vision.echo_data()
self.vision.call_data()
self.control.force_xy(1, 0)
self.control.publish_data(
"MOVING_ON_PATH POINT 1 move forward")
self.control.force_xy(1, 0)
break
self.control.publish_data("MOVING_ON_PATH POINT 1 command " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
if PATH_MODE:
self.control.publish_data(
"MOVING_ON_PATH SECTION try move to center of point 2")
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while ((not rospy.is_shutdown) and diff_time < 7):
self.rate.sleep()
self.control.publish_data(
"MOVING_ON_PATH try to find 3 point " + str(diff_time))
self.control.force_xy(TARGET_FORWARD, 0)
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.force_xy(0, 0)
self.control.publish_data(
"MOVING_ON_PATH Finish little mode to center")
count_unfound = 0
while ((not rospy.is_shutdown())):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
force_x = 0
force_y = 0
if PATH_MODE:
target_point = self.vision.num_point - 1
if target_point < 0:
self.control.publish_data(
"MOVING_ON_PATH Think more forward make num 0")
self.control.force_xy(TARGET_BACKWARD, 0)
continue
else:
target_point = 1
if (self.vision.x_point[1] < -10):
force_y = TARGET_LEFT
elif (self.vision.x_point[1] > 10):
force_y = TARGET_RIGHT
elif (self.vision.y_point[1] < -20):
force_x = TARGET_BACKWARD
elif (self.vision.y_point[1] > 00):
force_x = TARGET_FORWARD
else:
self.control.publish_data(
"MOVING_ON_PATH POINT 2 now center of point 2")
break
self.control.publish_data("MOVING_ON_PATH POINT 2 command " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
self.control.publish_data(
"MOVING_ON_PATH I will deactivate yaw for rotation")
self.control.deactivate(['x', 'y', 'yaw'])
now_tune = False
while (not rospy.is_shutdown()):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
diff = zeabus_math.bound_radian(
self.vision.rotation[self.vision.num_point - 2] -
(math.pi / 2))
if ((abs(self.vision.x_point[1]) > 80
or abs(self.vision.y_point[1]) > 80) or now_tune):
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if (not now_tune):
self.control.deactivate(['x', 'y'])
if self.vision.x_point[1] > 20:
force_y = TARGET_RIGHT
elif self.vision.x_point[1] < -20:
force_y = TARGET_LEFT
else:
ok_x = True
if self.vision.y_point[1] > 20:
force_x = TARGET_FORWARD
elif self.vision.y_point[1] < -20:
force_x = TARGET_BACKWARD
else:
ok_y = True
self.control.publish_data(
"MOVING_ON_PATH mode rotation force " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
if ok_x and ok_y:
now_tune = False
self.control.force_xy(0, 0)
self.control.deactivate(['x', 'y', 'yaw'])
continue
self.control.publish_data("MOVING_ON_PATH diff yaw is " +
str(diff))
if (abs(diff) < PATH_OK_DIFF_YAW):
self.control.force_false()
self.control.publish_data("Now rotation are ok")
self.control.sleep()
self.control.activate(['x', 'y', 'yaw'])
self.control.relative_yaw(0, False)
self.control.sleep()
break
elif (diff > 0):
self.control.publish_data("MOVING_ON_PATH rotation left")
self.control.force_xy_yaw(0, 0, PATH_FORCE_YAW)
else:
self.control.publish_data("MOVING_ON_PATH rotation right")
self.control.force_xy_yaw(0, 0, -1.0 * PATH_FORCE_YAW)
self.control.publish_data(
"MOVING_ON_PATH Now rotation finish next is moving on path")
self.control.deactivate(['x', 'y'])
self.control.absolute_z(PATH_END_DEPTH)
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while (not rospy.is_shutdown() and diff_time < PATH_LAST_TIME):
self.rate.sleep()
self.control.force_xy(SURVEY_FORWARD, 0)
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.publish_data("MOVING_ON_PATH diff , limit " +
repr((diff_time, PATH_LAST_TIME)))
self.control.activate(['x', 'y'])
self.control.relative_xy(0, 0)
#!/usr/bin/env python2
# FILE : master_control.py
# AUTHOR : K.Supasan
# CREATE ON : 2019, June 15 (UTC+0)
# MAINTAINER : K.Supasan
# README
# REFERENCE
from __future__ import print_function
from zeabus.control.command_interfaces import CommandInterfaces
import rospy
import math
if __name__ == "__main__":
rospy.init_node("master_test")
name_node = rospy.get_name()
auv_control = CommandInterfaces(name_node)
auv_control.master_call((False, False, False, True, True, False))
def __init__(self):
self.vision = AnalysisCoffin("base_coffin")
self.control = CommandInterfaces("PATH")
self.rate = rospy.Rate(5)
class Exposed:
def __init__(self):
self.vision = AnalysisCoffin("base_coffin")
self.control = CommandInterfaces("PATH")
self.rate = rospy.Rate(5)
def start_mission(self):
self.control.publish_data("START welcome to start mission")
self.control.publish_data("START command detph to " +
str(EXPOSED_START_DEPTH))
self.control.absolute_z(EXPOSED_START_DEPTH)
self.control.sleep()
while (not self.control.check_z(0.12)):
self.rate.sleep()
self.control.deactivate(['x', 'y'])
self.control.publish_data("START forwrd to find exposed")
count_found = 0
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while (not rospy.is_shutdown() and diff_time < STRATEGY_TIME_FORWARD
and count_found < 5):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
if (self.vision.result['num_object'] > 0):
count_found += 1
force_x = 0
force_y = 0
ok_x = False
ok_y = False
self.control.publish_data("START found target command force " +
repr((force_x, force_y)))
if (self.vision.result['center_x'] < -20):
force_y = SURVEY_LEFT
elif self.vision.result['center_x'] > 20:
force_y = SURVEY_RIGHT
else:
ok_y = True
if (self.vision.result['center_y'] < -20):
force_x = SURVEY_BACKWARD
elif self.vision.result['center_y'] > 20:
force_x = SURVEY_FORWARD
else:
ok_x = True
if (ok_x and ok_y):
count_found = 5
self.control.publish_data(
"START now center point of coffin")
else:
self.control.force_xy(force_x, force_y)
self.control.publish_data(
"START Have picture command force " +
repr((force_x, force_y)))
else:
count_found = 0
self.control.force_xy(SURVEY_FORWARD, 0)
self.control.publish_data(
"START Don't found picture command survey forward")
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.activate(['x', 'y'])
if (count_found == 5):
self.control.publish_data(
"START finish and found object let operator")
self.operator()
else:
self.control.publish_data(
"START finish and don't found object let find_coffin")
self.find()
def find(self):
self.control.publish_data("FIND start find expose")
mode = 0 # 0 -> survey left 1 -> survey forward 2 -> survey right
self.control.deactivate(['x', 'y'])
count_round = 0
count_found = 0
start_time = rospy.get_rostime()
while (not rospy.is_shutdown() and count_round < EXPOSED_LIMIT_ROUND):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
if (self.vision.result['num_object'] > 0):
count_found += 1
force_x = 0
force_y = 0
if (self.vision.result['center_x'] > 10):
force_y = SURVEY_RIGHT
elif self.vision.result['center_x'] < -10:
force_y = SUPER_LEFT
else:
pass
if (self.vision.result['center_y'] > 10):
force_x = SURVEY_FORWARD
elif self.vision.result['center_y'] < -10:
force_x = SURVEY_BACKWARD
else:
pass
self.control.publish_data("FIND force command " +
repr((force_x, force_y)))
continue
else:
count_found = 0
if (count_found == 3):
self.control.publish_data("FIND Break object")
self.control.force_xy(0, 0)
break
if (mode == 0):
self.control.force_xy(0, SURVEY_LEFT)
elif (mode == 1):
self.control.force_xy(SURVEY_FORWARD, 0)
else:
self.control.force_xy(0, SURVEY_RIGHT)
diff_time = (rospy.get_rostime() - start_time).to_sec()
if (diff_time > EXPOSED_LIMIT_TIME):
mode = (mode + 1) % 3
self.control.publish_data("FIND Change mode to " + str(mode))
else:
self.control.publish_data("FIND Diff time is " +
str(diff_time))
result = False
if (count_found == 3):
self.operator()
result = True
self.control.activate(['x', 'y'])
return result
def operator(self):
self.control.publish_data("OPERATOR Start mission")
self.control.deactivate(['x', 'y'])
count_unfound = 0
self.control.update_target()
target_depth = self.control.target_pose[2]
while not rospy.is_shutdown() and count_unfound < 3:
self.vision.call_data()
self.vision.echo_data()
if (self.vision.result['num_object'] == 1):
ok_x = False
ok_y = False
force_x = 0
force_y = 0
if (self.vision.result['center_x'] > 10):
force_y = TARGET_RIGHT
elif (self.vision.result['center_x'] < -10):
force_y = TARGET_LEFT
else:
ok_y = True
if (self.vision.result['center_y'] > 10):
force_x = TARGET_FORWARD
elif (self.vision.result['center_y'] < -10):
force_x = TARGET_BACKWARD
else:
ok_x = True
if (ok_x and ok_y):
self.control.force_xy(0, 0)
if (self.control.check_z(0.12)):
if (target_depth > EXPOSED_TARGET_DEPTH):
target_depth -= 0.2
self.control.absolute_z(target_depth)
self.control.publish_data("OPERATOR command z " +
str(target_depth))
else:
self.control.publish_data(
"OPERATOR now process rotation")
self.control.deactivate(['x', 'y', 'yaw'])
success_rotation = False
while (not rospy.is_shutdown()):
self.rate.sleep()
self.vision.call_data()
self.vision.echo_data()
if (self.vision.result['object_1']['rotation']
> 0.12):
self.control.publish_data(
"OPERATOR rotation left")
self.control.force_xy_yaw(
0, 0, EXPOSED_FORCE_YAW)
elif (self.vision.result['object_1']
['rotation'] < -0.12):
self.control.publish_data(
"OPERATOR rotation right")
self.control.force_xy_yaw(
0, 0, -EXPOSED_FORCE_YAW)
else:
success_rotation = True
break
self.control.activate(['x', 'y', 'yaw'])
self.control.sleep()
self.control.deactivate(['x', 'y'])
if (success_rotation):
self.tune_center()
break
else:
self.control.publish_data(
"OPERATOR now center waiting depth")
else:
self.control.publish_data(
"OPERATOR found single object force " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
elif self.vision.result['num_object'] == 2:
self.control.publish_data("OPERATOR found two object")
break
else:
count_unfound += 1
self.control.activate(('x', 'y'))
def tune_center(self):
self.control.publish_data(
"TUNE_CENTER welcome to tune_center function")
self.control.absolute_z(EXPOSED_TARGET_DEPTH)
self.control.publish_data("TUNE_CENTER waiting depth")
while (not self.control.check_z(0.12)):
self.rate.sleep()
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
can_go_up = False
while ((not rospy.is_shutdown())
and diff_time < EXPOSED_LIMIT_TIME_TO_FIND):
self.vision.call_data()
self.vision.echo_data()
if (self.vision.result['num_object'] == 2):
can_go_up = True
self.control.force_xy(0, 0)
self.control.publish_data("TUNE_CENTER found two object")
break
elif (self.vision.result['num_object'] == 1):
if ((self.vision.result['object_1']['center_x'] *
EXPOSED_CENTER_X_DIRECTION) > EXPOSED_CENTER_X_NEW_VALUE):
can_go_up = True
self.control.force_xy(0, 0)
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while (not rospy.is_shutdown()) and diff_time < 5:
self.rate.sleep()
self.control.force_xy(0, EXPOSED_FORCE_TO_BACK)
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.publish_data("TUNE_CENTER found new object")
break
else:
self.control.publish_data("TUNE_CENTER found same coffin")
else:
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.publish_data("TUNE_CENTER Don'found object " +
str(diff_time))
self.control.force_xy(0, EXPOSED_FORCE_TO_FIND)
if (not can_go_up):
self.control.publish_data("TUNE_CENTER We want to find again")
start_time = rospy.get_rostime()
diff_time = (rospy.get_rostime() - start_time).to_sec()
while (not rospy.is_shutdown()) and (
diff_time < EXPOSED_LIMIT_TIME_TO_FIND * 2.5):
self.vision.call_data()
self.vision.echo_data()
if (self.vision.result['num_object'] == 1):
while not rospy.is_shutdown():
self.vision.call_data()
self.vision.echo_data()
self.control.publish_data(
"TUNE_CENTER have to tune center again")
force_x = 0
force_y = 0
ok_x = False
ok_y = False
if (self.vision.result['center_x'] > 10):
force_y = TARGET_RIGHT
elif (self.vision.result['center_x'] < -10):
force_y = TARGET_LEFT
else:
ok_y = True
if (self.vision.result['center_y'] > 10):
force_x = TARGET_FORWARD
elif (self.vision.result['center_y'] < -10):
force_x = TARGET_BACKWARD
else:
ok_x = True
if (ok_x and ok_y):
self.control.publish_data("TUNE_CENTER now opk")
break
else:
self.control.publish_data("TUNE_CENTER force " +
repr((force_x, force_y)))
self.control.force_xy(force_x, force_y)
else:
self.control.force_xy(0, EXPOSED_FORCE_TO_BACK)
diff_time = (rospy.get_rostime() - start_time).to_sec()
self.control.publish_data("TUNE_CENTER have to move out and go up")
while (not rospy.is_shutdown()):
self.vision.call_data()
self.vision.echo_data()
if (self.vision.result['num_object'] == 0):
self.control.publish_data("TUNE_CENTER don't found object")
break
else:
self.control.publish_data("TUNE_CENTER still found object")
self.control.absolute_z(-0.2)
self.control.publish_data("TUNE_CENTER command to depth")
while (not self.control.check_z(0.2)):
self.rate.sleep()
self.control.publish_data("TUNE_CENTER have to sleep")
self.control.deactivate(['x', 'y', 'z', 'yaw'])
rospy.sleep(6)
self.control.activate(['x', 'y', 'z', 'yaw'])
self.control.publish_data("TUNE_CENTER have to wakeup")
self.control.absolute_z(-0.5)