def __init__(self): print "Initialized new supervisor" self.time_limit = 3 self.state_machine = msm.motion_state_machine() self.bumper = bump.Bumper((0.161, 0, 0)) self.start_time = time.time() self.vac=vacuum() self.hand = HandHandler() self.handler=jsonHandler() (self.origMap,self.workOrder)=self.handler.readInFile("RandomTestB.json") self.items_to_stow = len(self.workOrder[0]) ##Load bin Selector self.binSelect=binSelector() ##Initialize bin Selector self.binSelect.initialize("RandomTestB.json")
class Supervisor: def __init__(self): print "Initialized new supervisor" self.time_limit = 3 self.state_machine = msm.motion_state_machine() self.bumper = bump.Bumper((0.161, 0, 0)) self.start_time = time.time() self.vac=vacuum() self.hand = HandHandler() self.handler=jsonHandler() (self.origMap,self.workOrder)=self.handler.readInFile("RandomTestB.json") self.items_to_stow = len(self.workOrder[0]) ##Load bin Selector self.binSelect=binSelector() ##Initialize bin Selector self.binSelect.initialize("RandomTestB.json") # Potentially more to load here with everything else going on? def load_config_file(self, file_name): self.orig_config = file_name # Need to integrate JSON functionality into supervisor def done(self): if ((time.time() - self.start_time) > 60*self.time_limit): # Should be 20, but we're testing return True elif (self.items_to_stow == 0): return True return False # Didn't meet any of our end conditions def compete(self): print "Competing" print "Going to the tote" self.state_machine.move_to_bin_from_config(2) # If time is up or we've finished all items, break # Otherwise, loop while(not self.done()): # Go to the bin # Get Perception Data print "Getting Perception Data ..." # Send perception data to bump function pos = (random.uniform(-0.114, 0.013), random.uniform(-0.127, 0.127), random.uniform(-0.15, 0)) print "Bumping to coordinates", pos self.bumper.bumpRight(pos) # Pick up item # TODO: For demo, close the hand print "Picking up item ..." self.vac.on() self.hand.closeHand() time.sleep(1) # Based on item, pick a shelf to go to self.current_item = self.workOrder[0][0] # TODO: Add in Craig's bin picker instead of picking random shelf (self.target_bin,self.target_shelf) = self.binSelect.chooseBin(self.current_item); print "Going to shelf", self.target_shelf # Go to that shelf self.state_machine.move_to_shelf(self.target_shelf) # Figure out if need to bump print "Getting perception data to figure out any bumps" # Bump to that location # TODO: For demo, pick a random coordinate set and bump to there print "Bumping to coordinates ..." self.bumper.bumpRight((0.1, 0, 0)) # Drop item # TODO: For Demo, open the hand print "Dropping item ..." self.hand.openHand() self.vac.off() time.sleep(1) self.bumper.bumpRight((-0.1, 0, 0)) # Update JSON file and count self.binSelect.addtoBin(self.current_item,self.target_bin) self.workOrder[0].remove(self.current_item) self.origMap[self.target_bin].append(self.current_item) print "Updating JSON File" self.items_to_stow = self.items_to_stow - 1 if self.target_shelf >= 1 and self.target_shelf <= 3: self.state_machine.move_to_bin_from_config(1) if self.target_shelf >= 4 and self.target_shelf <= 6: self.state_machine.move_to_bin_from_config(2) if self.target_shelf >= 7 and self.target_shelf <= 9: self.state_machine.move_to_bin_from_config(3) if self.target_shelf >= 10 and self.target_shelf <= 12: self.state_machine.move_to_bin_from_config(4)