class MissionAll( StandardMission ):
def __init__( self , name ):
self.name = name
StandardMission.__init__( self , self.name , "/mission/all" , self.callback )
self.vision_gate = VisionCollector( "gate" )
self.vision_flare = VisionCollector( "flare" )
self.vision_drum = VisionCollector( "drum" )
self.state = False
self.echo( self.name , "ALL MISSION SETUP FINISHED")
self.time_start = time.time()
def callback( self , request ):
result = False
self.state = request.data
if( not self.ok_state() ):
return False
self.echo( self.name , "START MISSION GATE" )
self.reset_target( "yaw" )
self.reset_target( "xy" )
self.reset_velocity( "xy" )
self.target_state()
self.start_yaw = self.temp_state[5]
self.fix_z( -0.8 )
self.wait_state( "z" , 0.15 , 5 )
self.find_gate( 4 , 30 , 2 , 15 , 1 ) # d_f , t_f , d_s , t_s
self.echo( "CONNECTION" , "Now can go pass gate" )
self.over_move( 10 , 70 , True )
self.fix_z( -1.2 )
self.velocity_xy( 0.2 , 0 )
while( self.ok_state() ):
self.sleep( 0.1 )
if( self.over_move( 10 , 70 ) ):
break
self.reset_velocity( "xy")
self.fix_z( -0.7 )
play_pick = False
if( self.ok_state() ): play_pick = self.find_drum()
if( self.ok_state() and play_pick ): self.move_out_in()
self.find_flare( 6 , 50 , -1 )
self.reset_velocity( "xy" )
self.hold_gripper()
self.active_control( False )
return True
def over_move( self , distance , time , setup = False ):
if( setup ):
self.time_start = time.time()
self.collect_state()
return True
else:
if( self.distance() > distance ):
return True
# elif( ( time.time() - self.time_start ) > time ):
# return True
else:
return False
def play_flare( self ):
self.hold_gripper()
self.free_xy( True )
self.fix_z( -1.6 )
count_unfound = 0
while( self.ok_state() ):
self.sleep( 0.1 )
self.vision_flare( "flare" , "near" , 5 )
if( not self.vision_flare.have_object() ):
count_unfound += 1
if( count_unfound == 5 ):
break
continue
count_unfound = 0
if( abs( self.vision_flare.center_x() ) < 0.1 ):
self.velocity( { 'x' : 0.2 } )
else:
self.velocity( { 'y' : math.copysign( 0.25 , self.vision_flare.center_x()*-1 ) })
self.fix_z( -1.3 )
self.echo("NEAR" , "wait depth" )
while( self.ok_state() ){
self.sleep( 0.1 )
self.velocity({ 'x':0.01 } )
if( self.check_position( "z" , 0.15 ) ): break
}
self.free_xy( False )
time_start = time.time()
diff_time = 0
self.velocity_xy( 0.2 )
while( self.ok_state() and diff_time < 6 ):
self.sleep( 0.1 )
diff_time = time.time() - time_start
return True
def find_flare( self , distance_survey , time_survey , direction ):
mode = 1 # 1 forward 2 survey
setup_mode = True
direction_survey = direction
current_fix_velocity = False
finish_flare = False
self.fix_z( -1.4 )
while( self.ok_state() ):
self.sleep( 0.1 )
self.vision_flare( "flare" , "near" , 5 )
if( self.vision_flare.have_object() ):
finish_flare = self.play_flare()
if( finish_flare ):
self.reset_velocity( "xy" )
break
self.vision_flare( "flare" , "far" , 5 )
if( self.vision_flare.have_object() ):
if( current_fix_velocity ):
self.reset_velocity( "xy" )
current_fix_velocity = False
setup_mode = True
if( abs( self.vision_flare.center_x() ) < 0.15 ):
self.velocity( { 'x' : 0.1 } )
else: self.velocity( {'y' : math.copysign( 0.12, self.vision_flare.center_x())} )
continue
if( setup_mode ):
if( mode == 1 ):
self.over_move( 1 , 15 , True )
else:
self.over_move( distance_survey ,time_survey , True )
setup_mode = False
if( not current_fix_velocity ):
if( mode == 1 ):
self.velocity_xy( 0.2 , 0 )
else:
self.velocity_xy( 0 , 0.2 * direction_survey )
current_fix_velocity = True
if( mode == 1 ):
if( self.over_move( 1 , 15 ) ):
self.echo( "FLARE" , "Change to mode survey" )
mode = 2
self.reset_velocity("xy")
current_fix_velocity = False
setup_mode = True
else:
if( self.over_move( distance_survey , time_survey ) ):
self.echo( "FLARE" , "Change to mode forward" )
distance_survey *= -1
self.reset_velocity("xy")
current_fix_velocity = False
setup_mode = True
mode = 1
def move_out_in( self ):
self.velocity_xy( -0.15 , 0 )
self.find_drum
count_unfound = 0
while( self.ok_state() ):
self.sleep( 0.06 )
self.vision_drum.analysis_all( "mat" , "sevinar" , 5 )
if( not self.vision_drum.have_object() ):
count_unfound += 1
if( count_unfound == 5 ):
break
else:
count_unfound = 0
self.reset_velocity( "xy" )
return self.find_drum( True )
def play_drum( self , drop_ball ):
self.fire_gripper()
self.echo( "DROP" , "Command to release gripper")
self.free_xy( True )
value_x = 0
value_y = 0
ok_y = True
ok_x = False
ever_fire_golf = False
wait_z = False
self.relative_z( -0.3 )
start_time = 0
count_unfound = 0
if( drop_ball ): self.hold_golf()
while( self.ok_state() ):
self.sleep( 0.06 )
self.vision_drum.analysis_all( "blue" , "left-right" , 5 )
if( not self.vision_drum.have_object() ):
count_unfound += 1
if( count_unfound == 5 ):
self.free_xy( False )
return False
continue
count_unfound = 0
self.vision_drum.result['cx_2'] -= 0.15
if( abs(self.vision_drum.result['cx_2']) < 0.1 ):
value_y = -0.05
ok_y = True
else:
value_y = math.copysign( 0.25 , self.vision_drum.result['cx_2'] * -1 )
ok_y = False
if( abs(self.vision_drum.result['cy_2']) < 0.1 ):
value_x = 0.02
ok_x = True
else:
value_x = math.copysign( 0.2 , self.vision_drum.result['cy_2'] )
ok_x = False
if( ok_y and ok_x and self.check_position( "z" , 0.15 )
and self.check_position("yaw" , 0.2 ) and ( not ever_fire_golf ) ):
self.target_state()
self.echo( "DRUM" , "Now depth is " + str( self.temp_state[2] ) )
if( self.temp_state[2] < -1.4 ):
if( not drop_ball ): self.fire_golf()
else: self.velocity_z( -0.15 )
ever_fire_golf = True
start_time = time.time()
else:
self.relative_z( -0.3 )
self.velocity( { 'x' : value_x , 'y' : value_y } )
if( wait_z ):
if(self.check_position( "z" , 0.15 ) ):
self.free_xy( False )
return True
else: continue
if( ever_fire_golf ):
if( ( time.time() - start_time ) > 8 ):
self.reset_velocity( "z" )
self.fix_z( -0.6 )
wait_z = True
def find_drum( self , direction , drop_ball = False ):
self.velocity_xy( 0.2 )
count_found = 0
while( self.ok_state() ):
self.sleep( 0.06 )
self.vision_drum.analysis_all( "mat" , "sevinar" , 5 )
if( self.vision_drum.have_object() ):
count_found += 1
if( count_found == 3 ):
break
else:
count_found = 0
self.reset_velocity( "xy" )
self.reset_target( "xy" )
value_x = 0
value_y = 0
ever_change = False
count_found = 0
while( self.ok_state() ):
self.sleep( 0.06 )
self.vision_drum.analysis_all( "mat" , "sevinar" , 5 )
if( not self.vision_drum.result['forward'] ): value_x = 0.1
elif( not self.vision_drum.result['backward'] ): value_x = -0.1
else: value_x = 0
if( direction == 1 ): # direction == 1 is left
if( not self.vision_drum.result['left'] and not ever_change):
direction *= -1
ever_change = True
else:
return False
else:
if( not self.vision_drum.result['right'] and not ever_change):
direction *= -1
ever_change = True
else:
return False
self.vision_drum.analysis_all( "blue" , "center" , 5 )
if( self.vision_drum.have_object() ):
count_found += 1
if( count_found == 3 ):
return self.play_drum( drop_ball )
else:
count_found = 0
self.velocity( { 'x' : value_x , 'y' : 0.1 * direction } )
def find_gate( self, distance_forward, time_forward, distance_survey, time_survey, direct):
self.velocity_xy( 0.2 )
self.over_move( distance_forward , time_forward , True )
finish_gate = False
count_found = 0
while( self.ok_state() and not( finish_gate )):
self.sleep( 0.06 )
self.vision_gate.analysis_all( "gate" , "sevinar" , 5 )
if( self.vision_gate.have_object() ):
if( self.vision_gate.num_object() == 2 ):
self.reset_velocity( "xy" )
finish_gate = self.play_gate( direct )
else:
count_found += 1
if( count_found == 5 ):
self.reset_velocity( "xy" )
finish_gate = self.play_gate( direct )
else:
count_found = 0
if( self.over_move( distance_forward , time_forward ) ):
break
self.reset_velocity( "xy" )
self.wait_state( "xy" , 0.15 , 5 )
count_found = 0
self.over_move( distance_survey , time_survey , True )
if( direct == 0 ):
return True
while( self.ok_state() and not( finish_gate ) ):
self.sleep( 0.06 )
self.vision_gate.analysis_all( "gate" , "sevinar" , 5 )
if( self.vision_gate.have_object() ):
if( self.vision_gate.num_object() == 2 ):
self.reset_velocity( "xy" )
finish_gate = self.play_gate( direct )
else:
count_found += 1
if( count_found == 5 ):
self.reset_velocity( "xy" )
finish_gate = self.play_gate( direct )
else:
count_found = 0
if( self.over_move( distance_survey , distance_forward ) ):
break
return finish_gate
def play_gate( self , direction ): # direction = 1 is right an -1 is left
self.vision_gate.analysis_all( "gate" , "sevinar" , 5 )
current_fix_velocity = False
count_finish = 0
if( self.vision_gate.num_object() == 2 ):
while( self.ok_state() ):
self.sleep( 0.06 )
self.vision_gate.analysis_all( "gate" , "sevinar" , 5 )
if( self.vision_gate.num_object() == 2 ):
count_finish = 0
if( abs( self.vision_gate.center_x() ) < 0.2 ):
if( not current_fix_velocity ):
self.velocity_xy( 0.1 , 0 )
current_fix_velocity = True
else:
if( current_fix_velocity ):
self.reset_velocity( "xy" )
current_fix_velocity = False
self.echo_vision( self.vision_gate.echo_special() )
if( self.vision_gate.distance_x() > 1 ):
if( current_fix_velocity ):
self.reset_velocity( "xy" )
return True
else:
count_finish += 1
if( count_finish == 3 ):
if( current_fix_velocity ):
self.reset_velocity( "xy" )
return True
else:
while( self.ok_state() ):
self.sleep( 0.06 )
self.vision_gate.analysis_all( "gate" , "sevinar" , 5 )
if( self.vision_gate.num_object() == 2 ):
return self.play_gate()
elif( self.vision_gate.num_object() == 1 ):
count_finish = 0
self.velocity( { 'y' : direction * 0.1 } )
else:
count_finish += 1
if( count_finish == 4 ):
return True
class MissionQualification(StandardMission):
def __init__(self, name):
self.name = name
StandardMission.__init__(self, self.name, "/mission/qualification",
self.callback)
self.vision = VisionCollector("qualification")
self.state = False
print("MISSION QUALIFICATION FINISHED SETUP")
def callback(self, message):
result = False
if (message.data and self.state):
self.echo(self.name, "Now mission qualification have running")
return False
elif (message.data):
self.state = True
else:
self.state = False
return False
# This function will call by switch we must to reset data target
self.reset_target("xy")
self.reset_target("yaw")
self.reset_velocity("xy")
self.fix_z(-0.5)
self.echo(self.name, "START MISSION QUALIFICATION")
# This situation we don't know what picture we have to find but we known we have start
# direct we will go forward until find the picture
self.type_pier = 0 # -1 : 0 : 1 is single left : double pier : single right
self.collect_state()
self.velocity_xy(0.3, 0)
count_have_object = 0
start_time = time.time()
while (self.ok_state()):
self.sleep(0.05)
if ((time.time() - start_time) < 5):
self.echo(self.name,
"Now time is " + str(time.time() - start_time))
continue
self.vision.analysis_all("qualification", "sevinar", 5)
self.echo_vision(self.vision.echo_data())
if (self.vision.have_object()):
count_have_object += 1
if (count_have_object == 6):
break
else:
count_have_object = 0
self.echo(
self.name, "distance : count_have_object are " +
str(self.distance()) + str(count_have_object))
if (self.distance() > 4.5):
self.reset_velocity("xy")
self.velocity_xy(0.1, 0)
# we want to ensure we find object 3 round and next we will move follow data
self.reset_velocity("xy")
self.fix_z(-0.3)
self.vision.analysis_all("qualification", "servinar", 5)
# we assign data of run_type is
# 1 <== Found only single pier # 2 <== Found double pier <All GATE>
# 3 <== Solution of last move form found single pier
if (self.vision.num_object() == 2): self.run_type = 2
elif (self.vision.num_object() == 1): self.run_type = 1
else: self.run_type = 0
self.echo(
self.name,
"We will start do it on function type : " + str(self.run_type))
while (self.ok_state()):
if (self.run_type == 1): self.type_1()
elif (self.run_type == 2): self.type_2()
elif (self.run_type == 3): self.type_3()
else: break
self.echo(self.name, "Finished run callback alway response TRUE")
return True
def type_1(self):
if (self.type_pier == 0):
self.vision.analysis_all("qualification", "sevinar", 5)
self.echo_vision(self.vision.echo_specific())
if (self.vision.num_object() == 1):
if (self.vision.center_x() < 0):
self.echo(self.name, "I decide that pier is left pier")
self.type_pier = -1
else:
self.echo(self.name, "I decide that pier is right pier")
self.type_pier = 1
elif (self.vision.num_object() == 2):
self.type_pier = 0
self.echo(self.name, "What I found 2 pier why?")
self.run_type = 2
return 0
count_not_single = 0
count_not_found = 0
while (self.ok_state()):
self.sleep(0.05)
self.vision.analysis_all("qualification", "sevinar", 5)
self.echo_vision(self.vision.echo_data())
if (self.vision.num_object() == 1):
count_not_single = 0
count_not_found = 0
if (abs(self.vision.center_x()) < 0.3):
self.velocity({'x': 0.10})
self.echo(
self.name, "type_1 object : " + str(self.type_pier) +
" We decide to move forward")
elif (self.vision.center_x() < 0):
self.velocity({'y': 0.1})
self.echo(
self.name, "type_1 object : " + str(self.type_pier) +
" We decide to move left")
elif (self.vision.center_x() > 0):
self.velocity({'y': -0.1})
self.echo(
self.name, "type_1 object : " + str(self.type_pier) +
" We decide to move right")
else:
self.echo(self.name, "BUG ON LINE 127")
self.run_type = 0
break
self.echo_vision(self.vision.echo_specific())
if (self.vision.distance_x() > 0.1):
self.echo(
self.name,
"Now over distance we have to chance to last move")
self.run_type = 3
break
elif (self.vision.num_object() == 2):
self.velocity(
{'y': math.copysign(0.1,
self.vision.center_x() * -1)})
self.run_type = 2
self.type_pier = 0
break
else:
count_not_found += 1
self.echo(self.name,
"type_1 not_found pier : " + str(count_not_found))
if (count_not_found == 5):
self.run_type = 0
self.relative_xy(5, math.copysign(1.5, self.type_pier))
self.echo(
self.name, "We decide to last move side is " +
str(math.copysign(1.5, -1 * self.type_pier)))
break
def type_2(self):
count_not_found = 0
count_not_doulbe = 0
current_fix_velocity = False
while (self.ok_state()):
self.sleep(0.1)
self.vision.analysis_all("qualification", "sevinar", 5)
self.echo_vision(self.vision.echo_data())
if (self.vision.num_object() == 2):
count_not_doulbe = 0
count_not_found = 0
if (abs(self.vision.center_x()) < 0.10):
if (not current_fix_velocity):
self.velocity_xy(0.2, 0)
current_fix_velocity = True
self.echo(self.name,
"type_2 we move direct by fix velocity")
elif (self.vision.center_x() < 0):
if (current_fix_velocity):
current_fix_velocity = False
self.reset_velocity("xy")
self.velocity({'y': 0.1})
self.echo(self.name, "type_2 we move left")
elif (self.vision.center_x() > 0):
if (current_fix_velocity):
current_fix_velocity = False
self.reset_velocity("xy")
self.velocity({'y': -0.1})
self.echo(self.name, "type_2 we move right")
self.echo_vision(self.vision.echo_specific())
if (self.vision.distance_x() > 1.0):
self.echo(
self.name,
"Now over distance we decide to only move forward")
self.run_type = 0
if (not current_fix_velocity):
self.velocity_xy(0.3, 0)
self.echo(self.name,
"We order contant velocity x is 0.3")
current_fix_velocity = True
break
elif (self.vision.num_object() == 1):
if (current_fix_velocity):
current_fix_velocity = False
self.reset_velocity("xy")
count_not_doulbe += 1
self.echo(
self.name, "Type 2 we found only single pier : " +
str(count_not_doulbe))
if (count_not_doulbe == 5):
self.echo(self.name,
"We change to mode type 1 single pier")
self.run_type = 1
self.type_pier = 0
break
else:
count_not_found += 1
self.echo(self.name,
"Type 2 we not found pier : " + str(count_not_found))
if (count_not_found == 5):
self.echo(self.name, "Last move")
self.run_type = 0
self.type_pier = 0
if (not current_fix_velocity):
self.velocity_xy(0.3, 0)
self.echo(self.name,
"I don't know what should I do FORWARD!")
current_fix_velocity = True
break
def type_3(self):
self.echo(
self.name, "We move in last mode type_3 and type peir is " +
str(self.type_pier))
# this step we will move side to don't find pier and
# will move a little time befor move forward
count_not_found = 0
while (self.ok_state()):
self.sleep(0.1)
self.vision.analysis_all("qualification", "sevinar", 5)
self.echo(
self.name, "Type 3 and move slide to don't have object : " +
str(count_not_found))
self.velocity({'y': math.copysign(0.10, self.type_pier)})
if (self.vision.have_object()):
count_not_found = 0
else:
count_not_found += 1
if (count_not_found == 5):
break
start_time = time.time()
self.echo(self.name, "Move side 3 second")
while (self.ok_state()):
self.sleep(0.1)
diff_time = time.time() - start_time
self.velocity({'y': math.copysign(0.10, self.type_pier)})
self.echo(self.name, "Type3 now move time is " + str(diff_time))
if (diff_time > 5): break
self.echo(self.name, "Time out move forward")
self.velocity_xy(0.3, 0)
self.run_type = 0
class MissionQualification( StandardMission ):
def __init__( self , name ):
self.name = name
StandardMission.__init__( self , self.name , "/mission/qualification" , self.callback )
self.vision = VisionCollector("qualification")
print("QUALIFICATION FINISHED SETUP")
def callback( self , message): # Divide to 2 part move to center and direction
result = False
self.temp_pier = 0
self.reset_target( "xy" )
self.reset_target( "yaw" )
self.fix_z( -0.3 )
self.echo( self.name , "START MISSION QUALIFICATION")
self.collect_state()
self.velocity_xy( 0.2 , 0 )
while( not rospy.is_shutdown() ):
self.sleep( 0.1 )
self.vision.analysis_all( "qualification" , "sevinar" , 5 )
if( self.vision.have_object() ):
self.echo( self.name , "Now have object ")
break
else:
self.echo( self.name , "Never found and distance is " + str( self.distance()) )
self.reset_velocity("xy")
self.run_type = 1
while( not rospy.is_shutdown() and not result ):
if( self.run_type == 0 ): break
elif( self.run_type == 1 ): self.move_forward()
elif( self.run_type == 2 ): self.move_center()
elif( self.run_type == 3 ): result = self.last_move()
elif( self.run_type == 4 ): result = self.relative_move()
return result
def move_forward( self ):
self.velocity_xy( 0.2 , 0 )
count_have_object = 0
count_out_center = 0
self.run_type = 0
while( not rospy.is_shutdown() ):
self.sleep(0.05)
self.vision.analysis_all( "qualification" , "sevinar" , 5 )
self.echo_vision( self.vision.echo_data() )
if( self.vision.num_object() == 2):
count_have_object = 0
if( self.vision.center_x() < 0.2 ):
self.echo( self.name , "MODE_FORWARD Move forward ok center")
count_out_center = 0
else:
count_out_center += 1
self.echo( self.name , "MODE_FORWARD Out center : " +
str( count_out_center ) )
if( count_out_center == 5 ):
self.run_type = 2
break
if( self.vision.area() > 0.5 ):
self.run_type = 3
break
elif( self.vision.num_object() == 1 ):
self.run_type = 4
break
else:
count_have_object += 1
self.echo( self.name , "MODE_FORWARD Count don't have object is " +
str( count_have_object ))
if( count_have_object == 5 ):
self.run_type = 3
break
self.reset_velocity( "xy" )
def move_center( self ):
count_have_object = 0
while( not rospy.is_shutdown() ):
self.sleep(0.05)
self.vision.analysis_all( "qualification" , "sevinar" , 5 )
self.echo_vision( self.vision.echo_data() )
if( self.vision.num_object() == 2 ):
count_have_object = 0
if( self.vision.center_x() < 0.2 ):
self.run_type = 1
break
elif( self.vision.center_x() < 0 ):
self.velocity( {'y' , 0.1 } )
self.echo( self.name , "Now we move Left")
elif( self.vision.center_x() > 0 ):
self.velocity( {'y' , -0.1 } )
self.echo( self.name , "Now we move Right")
elif( self.vision.num_object() == 1 ):
self.run_type = 4
break
else:
count_have_object += 1
self.echo( self.name , "Count don't have object is " + str( count_have_object))
if( count_have_object == 5 ):
self.run_type = 3
break
def relative_move( self ):
self.vision.analysis_all( "qualification" , "servinar" , 5 )
if( self.vision.center_x() < 0 ):
self.echo( self.name , "Last move relative RIGHT")
self.relative_xy( 3 , -1 )
else:
self.echo( self.name , "Last move relative LEFT")
self.relative_xy( 3 , 1 )
return True
def last_move( self ):
self.echo( self.name , "Last move forward by velocity_xy 0.3")
self.velocity_xy( 0.3 , 0 )
return True
