class MissionPick(StandardMission): def __init__(self, name): self.name = name StandardMission.__init__(self, self.name, "/mission/pick", self.callback) self.vision = VisionCollector("drum") self.echo(self.name, "PICK GOLF SETUP FINISHED") self.state = False def callback(self, request): result = False self.state = request.data if (not self.ok_state()): return False self.fix_z(-0.5) self.reset_target("xy") self.wait_state("z", 0.1, 5) found_object = self.step_01() if (not found_object): self.echo(self.name, "Don't found object mission aborted") return False else: self.step_03() result = True return result def step_01(self): # find golf in a drum found_golf = False mode_control = 1 # mode control is survey around drum # 1 is left 2 is right 3 is up 4 is down value_x = 0 value_y = 0 count_change_mode = 0 count_found_golf = 0 limit_mode = 10 self.echo(self.name, "We command to hold for pick golf") self.hold_golf() self.echo(self.name, "We command finish") while (self.ok_state()): self.sleep(0.1) if (mode_control == 1): self.vision.analysis_all("blue", "left-right", 5) self.echo_vision(self.vision.echo_special()) if (self.vision.result["cx_1"] < -0.5): value_y = 0.1 elif (self.vision.result["cx_1"] > 0.0): value_y = -0.1 else: value_y = 0 if (abs(self.vision.result["cy_1"]) < 0.5): value_x = 0 else: value_x = math.copysign(0.05, self.vision.result["cy_1"]) elif (mode_control == 2): self.vision.analysis_all("blue", "left-right", 5) self.echo_vision(self.vision.echo_special()) if (self.vision.result["cx_2"] > 0.5): value_y = -0.05 elif (self.vision.result["cx_2"] < 0.0): value_y = 0.05 else: value_y = 0 if (abs(self.vision.result["cy_2"]) < 0.5): value_x = 0 else: value_x = math.copysign(0.05, self.vision.result["cy_2"]) self.echo( self.name + " Mode " + str(mode_control), "We command velocity x : y --> " + str(value_x) + " : " + str(value_y)) self.velocity({'x': value_x, 'y': value_y}) self.sleep(0.1) self.vision.analysis_all("golf", "sevinar", 5) self.echo_vision(self.vision.echo_data()) # This part will discus about data from vision if (self.vision.have_object()): count_found_golf = 0 self.echo(self.name, "Find golf move to center of golf") while (self.ok_state() and abs(count_found_golf) < 15): self.sleep(0.1) self.vision.analysis_all("golf", "sevinar", 5) if (self.vision.have_object()): count_found_golf += 1 self.velocity({ 'x': math.copysign(0.1, self.vision.center_y()), 'y': math.copysign(0.1, -1 * self.vision.center_x()) }) else: count_found_golf -= 0 self.echo(self.name, "Found golf is " + str(count_found_golf)) if (count_found_golf == 15): return True if (value_x == 0 and value_y == 0): count_change_mode += 1 self.echo(self.name, "count_change_mode " + str(count_change_mode)) if (count_change_mode == limit_mode): mode_control += 1 count_change_mode = 0 if (mode_control > 2): return False print("Finish loop") def step_02(self): # this part we will pick a ball self.echo(self.name, "We will start to pick ball") self.fix_z(-0.8) count_ok_depth = 0 value_x = 0 value_y = 0 self.free_xy(True) while (self.ok_state()): self.vision.analysis_all("golf", "sevinar", 5) self.echo_vision(self.vision.echo_data()) if (self.vision.center_x() < -0.5): value_y = -0.1 elif (self.vision.center_x() > -0.1): value_y = 0.1 else: value_y = 0 if (self.vision.center_y() < 0.1): value_x = 0.1 elif (self.vision.center_y() > 0.5): value_x = -0.1 else: value_x = 0 self.echo(self.name, " vel x : y is" + str(value_x) + " : " + str(value_y)) self.velocity({'x': value_x, 'y': value_y}) if (self.check_position("z", 0.15)): count_ok_depth += 1 else: count_ok_depth = 0 if (value_x == 0 and value_y == 0): self.echo(self.name, "Target on lock") if (count_ok_depth > 5): self.echo(self.name, "We will move down now ") start_time = time.time() diff_time = 0 self.velocity_z(-0.1) while (self.ok_state() and diff_time < 10): self.sleep(0.1) self.reset_target("xy") diff_time = time.time() - start_time self.echo(self.name, "Now time is " + diff_time) self.reset_velocity("z") self.fix_z(-0.5) self.free_xy(False) break self.echo(self.name, "Finish pick golf it ok?") def step_03(self): self.echo(self.name, "We will start to pick ball") current_fix_velocity = False value_x = 0 value_y = 0 self.fix_z(-0.8) self.wait_state("z", 0.1, 5) self.free_xy(True) while (self.ok_state()): self.sleep(0.05) self.vision.analysis_all("golf", "sevinar", 5) self.echo_vision(self.vision.echo_data()) if (self.vision.have_object()): if (self.vision.center_x() > -0.4): value_y = -0.2 else: value_y = 0.1 if (self.vision.center_y() < 0.3): value_x = -0.15 else: value_x = 0.10 self.echo( self.name, "Vel x : y are " + str(value_x) + " : " + str(value_y)) self.velocity({'x': value_x, 'y': value_y}) else: if (not current_fix_velocity): self.velocity_z(-0.03) current_fix_velocity = True value_y = 0.0 value_x = 0.0 self.velocity({'y': value_y, 'x': value_x}) self.echo( self.name, "Vel x : y are " + str(value_x) + " : " + str(value_y)) self.target_state()
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 MissionAll(StandardMission): def __init__(self, name): self.name = name StandardMission.__init__(self, self.name, "/mission/all", self.callback) self.gate = VisionCollector("gate") self.run_gate = False self.drum = VisionCollector("drum") self.run_drum = False self.flare = VisionCollector("flare") self.run_flare = False self.mission_gate = rospy.ServiceProxy("/mission/gate", TwoBool) self.mission_flare = rospy.ServiceProxy("/mission/flare", TwoBool) self.mission_drum = rospy.ServiceProxy("/mission/drop", TwoBool) self.mission_golf = rospy.ServiceProxy("/mission/pick", TwoBool) self.start_time = time.time() self.start_yaw = 0 self.over = self.over_distance self.type_over = "distance" # self.over = self.over_time # self.type_over = "time" self.echo(self.name, "FINISHED SETUP ALL MISSION") def callback(self, message): if (self.state and message.data): self.echo(self.name, "Now mission will run please close befor try again") return False elif (message.data): self.state = True self.main_play() else: self.echo(self.name, "Switch call to stop run mission") self.state = False if (self.run_gate): self.run_gate = self.gate(False) self.echo(self.name, "Call to close gate") if (self.run_drum): self.run_drum = self.drun(False) self.echo(self.name, "Call to close drum") if (self.run_flare): self.run_flare = self.flare(False) self.echo(self.name, "Call to close flare") return False self.echo(self.name, "End Callback") def main_play(self): # This function will call by switch we must reset data target self.reset_velocity("xy") self.reset_target("xy") self.reset_target("yaw") self.fix_z(-0.8) self.collect_state() self.start_yaw = self.save_state[5] self.echo(self.name, "START ALL MISSION at yaw is " + str(self.start_yaw)) self.wait_state("z", 0.1, 5) self.velocity_xy(0.2, 0) self.over_time(10, True) while (self.ok_state() and (not self.over_time(10))): self.sleep(0.1) self.reset_velocity("xy") result = self.gate(True) if (self.ok_state()): self.fix_z(-1.4) self.fix_yaw(self.start_yaw - (math.pi / 2)) self.wait_state("yaw", 0.1, 5) self.survey_mode(5, 6, 1, 4, "flare", self.flare) if (self.ok_state()): self.fix_z(0.5) self.fix_yaw(self.start_yaw) self.wait_state("z", 0.1, 5) self.wait_state("yaw", 0.1, 5) result = self.survey_mode(4, 10, 1, 6) if (self.ok_state()): self.fix_z(-0.5) self.wait_state("z", 0.1, 5) self.velocity_xy(-0.1, 0) self.over_time(15, True) while (self.ok_state()): self.sleep(0.1) if (self.over_time(15)): break self.reset_target("xy") self.velocity_xy(0.1, 0) while (self.ok_state()): self.sleep(0.1) self.drum.analysis_all("blue", "side", 5) if (self.drum.have_object()): self.mission_golf(True) break # WARNING ! survey mode will use after rotation # step is move forward move slide move forward and move inverse slide def survey_mode(self, first_forward, first_slide, forward, slide, mission, service): self.echo(self.name, "We start survey for mission " + str(mission)) type_movement = 1 # 1 is forward , 2 is first_slide , 3 is forward , 4 is slide current_fix_velocity = False count_have_object = 0 limit_value = first_forward while (self.ok_state()): # This part of connect to vision and call service for make make mission if (mission == "gate"): self.gate.analysis_all("gate", "sevinar", 5) if (self.gate.have_object()): count_have_object += 1 else: count_have_object = 0 elif (mission == "flare"): self.flare.analysis_all("flare", "far", 5) if (self.flare.have_object()): count_have_object += 1 else: self.flare.analysis_all("flare", "near", 5) if (self.flare.have_object()): count_have_object += 1 else: count_have_object = 0 elif (mission == "drum"): self.drum.analysis_all("blue", "side", 5) if (self.drum.have_object()): count_have_object += 1 else: count_have_object = 0 else: self.echo(self.name, "Don't have this mode vision aborted") self.state = False if (count_have_object > 0): if (current_fix_velocity): self.reset_velocity("xy") self.reset_target("xy") current_fix_velocity = False if (self.type_over == "distance"): limit_value -= self.distance() elif (self.type_over == "time"): limit_value -= time.time() - self.start_time self.echo( self.name, "Now survey for mission " + str(mission) + " Count " + str(count_have_object)) if (count_have_object == 5): # This is part about send service to individual mission to do task self.echo(self.name, "We send process to mission " + str(mission)) result = service(True) count_have_object = 0 if (result): return True continue # This part of connect to control for survey if (type_movement == 1): if (not current_fix_velocity): self.over(limit_value, True) self.velocity_xy(0.10, 0) current_fix_velocity = True if (self.over(limit_value)): type_movement = 2 self.reset_velocity("xy") current_fix_velocity = False elif (type_movement == 2): if (not current_fix_velocity): self.over(limit_value, True) self.velocity_xy(0, math.copysign(0.1, first_slide)) current_fix_velocity = True if (self.over(limit_value)): type_movement = 3 self.reset_velocity("xy") current_fix_velocity = False elif (type_movement == 3): if (not current_fix_velocity): self.over(limit_value, True) self.velocity_xy(0.1, 0) current_fix_velocity = True if (self.over(limit_value)): type_movement = 4 self.reset_velocity("xy") current_fix_velocity = False first_forward = forward elif (type_movement == 4): if (not current_fix_velocity): self.over(limit_value, True) self.velocity_xy(0, math.copysign(0.1, slide)) current_fix_velocity = True if (self.over(limit_value)): type_movement = 1 self.reset_velocity("xy") current_fix_velocity = False first_slide = math.copysign(slide, first_slide) else: self.echo(self.name, "Dont'have this mode type_movement aborted") self.state = False return False def over_time(self, limit_value, setup=False): if (setup): self.start_time = time.time() return False elif (abs(limit_value) < time.time() - self.start_time): print("For check difftime " + str(time.time() - self.start_time) + " and limit time is " + str(limit_value)) return True else: print("For check difftime " + str(time.time() - self.start_time) + " and limit time is " + str(limit_value)) return False def over_distance(self, limit_value, setup=False): if (setup): self.collect_state() return False elif (self.distance() > limit_value): print("For check distance " + str(self.distance()) + " and limit is " + str(limit_value)) return True else: print("For check distance " + str(self.distance()) + " and limit is " + str(limit_value)) return False
class MissionGate(StandardMission): def __init__(self, name): self.name = name StandardMission.__init__(self, self.name, "/mission/gate", self.callback) self.vision = VisionCollector("gate") self.echo(self.name, "GATE SETUP FINISHED") self.state = False def callback(self, request): # Divide to 2 part move to center and direction result = False self.state = request.data if (not self.ok_state()): return False self.echo(self.name, "START MISSION GATE") count = 0 count += self.step_01() if (count != 0): result = self.step_02(count) self.fix_z(-0.5) return result def step_01(self): result = 0 count_unfound = 0 while (self.ok_state()): self.sleep(0.1) self.vision.analysis_all("gate", "sevinar", 5) self.echo_vision(self.vision.echo_data()) if (count_unfound == 5): break if (not self.vision.have_object()): count_unfound += 1 continue count_unfound = 0 if (self.vision.result['pos'] == 0): temp_center_x = self.vision.center_x() if (abs(temp_center_x) < 0.1): self.echo(self.name, "Found Pos : 0 and Move Forward") self.velocity({'x': 0.2}) elif (temp_center_x < 0): self.echo(self.name, "Found Pos : 0 and Move Left") self.velocity({'y': 0.1}) else: self.echo(self.name, "Found Pos : 0 and Move Right") self.velocity({'y': -0.1}) if (self.vision.area() > 0.6): self.collect_state() self.echo(self.name, "Area are over let play direct") self.velocity_xy(0.2, 0) self.sleep() self.relative_z(-0.3) result = 1 break elif (self.vision.result['pos'] == -1): self.echo(self.name, "Found Pos : -1 and Move Left") self.velocity({'y': 0.2}) elif (self.vision.result['pos'] == 1): self.echo(self.name, "Found Pos : 1 and Move Right") self.velocity({'y': -0.2}) return result def step_02(self, num_type): self.echo(self.name, "Welcome to step 2 check movement") if (num_type == 1): # have fix velocity while (self.ok_state()): self.sleep() distance = self.distance() self.echo(self.name, "Now distance " + str(distance)) if (distance > 2.5): self.reset_velocity("xy") self.relative_z(0.3) break count_ok = 0 while (not rospy.is_shutdown()): self.sleep() if (self.check_position("z", 0.15)): count_ok += 1 if (count_ok == 5): break else: count_ok = 0 self.echo(self.name, "Count ok depth is " + str(count_ok)) return True elif (num_type == 2): # non fix velocity count_ok = 0 while (self.ok_state()): self.sleep() if (self.check_position("xy", 0.1)): count_ok += 1 if (count_ok == 5): break else: count_ok = 0 self.echo(self.name, "Count ok xy is " + str(count_ok)) count_ok = 0 while (not rospy.is_shutdown()): self.sleep() if (self.check_position("z", 0.15)): count_ok += 1 if (count_ok == 5): break else: count_ok = 0 self.echo(self.name, "Count ok depth is " + str(count_ok)) return True else: return False
class MissionGate(StandardMission): def __init__(self, name): self.name = name StandardMission.__init__(self, self.name, "/mission/flare", self.callback) self.vision = VisionCollector("flare") print("FLARE FINISHED SETUP") def callback(self, message): # Divide to 2 part move to center and direction result = False self.echo(self.name, "START MISSION FLARE") result = self.step_01() if (result): result = self.step_02() return result def step_01(self): # play in far mode and try to find in near mode self.echo(self.name, "PLAY STEP ONE TO FIND FLAR IN FAR MODE") count_unfound = 0 while (not rospy.is_shutdown()): self.sleep(0.1) self.vision.analysis_all("flare", "far", 5) self.echo_vision(self.vision.echo_data()) if (self.vision.have_object()): count_unfound = 0 if (abs(self.vision.center_x()) < 0.3): self.velocity({'x': 0.1}) self.echo(self.name, "FARMODE Move FORWARD") elif (self.vision.center_x() < 0): self.velocity({'y': 0.08}) self.echo(self.name, "FARMODE MOVE LEFT") elif (self.vision.center_x() > 0): self.velocity({'y': -0.08}) self.echo(self.name, "FARMODE MOVE RIGHT") else: self.echo( self.name, "\tERROR IN LINE 59 =====================================" ) else: count_unfound += 1 self.echo(self.name, "WARNING UNFOUND : " + str(count_unfound)) if (count_unfound == 5): break self.vision.analysis_all("flare", "near", 5) if (self.vision.have_object()): self.echo(self.name, "FOUND IN NEAR MODE") self.reset_target("xy") break if (count_unfound == 5): return False else: return True def step_02(self): # play in near mode self.echo(self.name, "PLAY STEP TWO TO FIND FLAR AND TARGET DASH") count_unfound = 0 # this variable is about picture count_ok = 0 # this variable is about position haved_reset_target = False while (not rospy.is_shutdown()): self.sleep(0.1) self.vision.analysis_all("flare", "near", 5) self.echo_vision(self.vision.echo_data()) if (self.vision.have_object()): count_unfound = 0 if (abs(self.vision.center_x()) < 0.5): if (not haved_reset_target): self.reset_target("xy") self.echo(self.name, "NEARMODE Renew target") self.echo( self.name, "NEARMODE That center we will move direct if we can") if (self.check_position("xy", 0.05)): count_ok += 1 if (count_ok == 5): self.velocity_xy(0.2, 0) self.echo(self.name, "NEARMODE GO DIRECT") break else: count_ok = 0 elif (self.vision.center_x() < 0): self.velocity({'y': 0.05}) self.echo(self.name, "NEARMODE MOVE LEFT") elif (self.vision.center_x() > 0): self.velocity({'y': -0.05}) self.echo(self.name, "NEARMODE MOVE RIGHT") else: self.echo( self.name, "\tERROR IN LINE 112 =====================================" ) else: count_unfound += 1 if (count_unfound == 5): break while (not rospy.is_shutdown() and count_unfound < 5): self.sleep(0.1) self.vision.analysis_all("flare", "near", 5) self.echo_vision(self.vision.echo_data()) if (self.vision.have_object()): count_unfound = 0 else: count_unfound += 1 self.fix_z(-0.1) return True
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 MissionDrum(StandardMission): def __init__(self, name): self.name = name StandardMission.__init__(self, self.name, "/mission/drop", self.callback) self.vision = VisionCollector("drum") self.state = False self.echo(self.name, "DROP BALL SEtuP FINISHER") def callback(self, request): result = False self.state = request.data if (not self.ok_state()): return False self.echo(self.name, "START MISSION DROP BALL") self.wait_state("yaw", 0.1, 5) self.step_01() return True def step_01(self): count_Time = False self.echo(self.name, "START AT STEP 01") value_x = 0 value_y = 0 start_time = 0 ever_drop = False self.velocity_z(-0.03) self.wait_state("z", 0.1, 5) current_fix_z = False while (self.ok_state()): self.vision.analysis_all("blue", "left-right", 5) self.echo_vision(self.vision.echo_special()) self.echo_vision(self.vision.echo_data()) if (abs(self.vision.center_x()) < 0.15): value_y = 0 else: value_y = math.copysign(0.1, -1 * self.vision.center_x()) if (abs(self.vision.center_y()) < 0.15): value_x = 0 else: value_x = math.copysign(0.1, self.vision.center_y()) # if( not self.vision.result["forward"] ): # value_x = -0.2 # elif( not self.vision.result["backward"] ): # value_x = 0.2 # else: # value_x = 0 self.echo( self.name, "I command velocity x : y are " + str(value_x) + " : " + str(value_y)) if (not count_Time): self.target_state() self.echo(self.name, "Node depth is " + str(self.temp_state[2])) if (self.temp_state[2] < -1.3): count_Time = True start_time = time.time() self.free_xy(True) else: diff_time = time.time() - start_time self.echo(self.name, "Count time for drop " + str(diff_time)) if (diff_time > 5): break elif (diff_time > 2 and not ever_drop): self.echo(self.name, "We have to command drop ball") self.fire_golf() else: continue self.velocity({'x': value_x, 'y': value_y}) self.reset_velocity("z") self.fix_z(-0.5) self.wait_state("z", 0.1, 5) self.free_xy(False) self.reset_target("xy")
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