class StrategyTestRendezvous(Strategy): mouse = None isVisited = [] path = [] network = None neighbors_states = {} # group states including self topological_neighbors = [] switchGoal = False stop_condition = False whoami = -1 dx = [] dy = [] isBack = False iterations = 0 num_bots = -1 # starting_pose = () starting_pose = () drive = (None, None) # mazeMap = None # define number of robots def __init__(self, mouse, initPoint, num_bots): # print("INTIA") self.mouse = mouse self.num_bots = num_bots self.isVisited = [[0 for i in range(self.mouse.mazeMap.width)] for j in range(self.mouse.mazeMap.height)] # print(self.isVisited) self.isVisited[self.mouse.x][self.mouse.y] = 1 # print("made it past initialization") for i in range(1, self.num_bots + 1): if initPoint[str(i)] == (self.mouse.x, self.mouse.y): self.whoami = i self.neighbors_states[i] = { 'robot': i, 'x': initPoint[str(i)][0], 'y': initPoint[str(i)][1], 'direction': 'UP' } # print(self.neighbors_states[i]) # print("whoami:%s"%self.whoami) self.starting_pose = initPoint[str(self.whoami)] self.network = NetworkInterface() self.network.initSocket() self.network.startReceiveThread() # print("network start receive thread") def checkFinished(self): return self.isBack def distance_to_near_neigh(self): dx_temp = 0 dy_temp = 0 follow_him = -1 distance = 100 # some big number temp_distance = 0 for bots in range(1, self.num_bots + 1): if bots == self.whoami: continue else: dx_temp = self.neighbors_states[bots]['x'] - self.mouse.x dy_temp = self.neighbors_states[bots]['y'] - self.mouse.y temp_distance = (dx_temp * dx_temp + dy_temp * dy_temp)**(1 / 2) if temp_distance < distance: distance = temp_distance follow_him = bots return distance, follow_him def distance_to_far_neigh(self): dx_temp = 0 dy_temp = 0 follow_him = -1 distance = 0 # some small number temp_distance = 0 cost = 5 for bots in range(1, self.num_bots + 1): if bots == self.whoami: continue else: dx_temp = self.neighbors_states[bots]['x'] - self.mouse.x dy_temp = self.neighbors_states[bots]['y'] - self.mouse.y temp_distance = (dx_temp * dx_temp + dy_temp * dy_temp)**(1 / 2) if temp_distance > distance: distance = temp_distance follow_him = bots return distance, follow_him def distance_to_you(self): distance = -1 if abs(dx_temp) < abs(self.dx): self.dx = dx_temp if self.dx < 0: x_Dir = "LEFT" else: x_Dir = "RIGHT" elif abs(dy_temp) < abs(self.dy): self.dy = dx_temp if self.dy < 0: y_Dir = "UP" # opposite to intuition else: y_Dir = "DOWN" if abs(self.dx) < abs(self.dy): return x_Dir # elif abs(self.dx) == abs(self.dy):return y_Dir # just make up defualt when tie else: return y_Dir def Centroid(self): area = 0 centroid = () weights = 0 dx = 0 dy = 0 dx_temp = 0 dy_temp = 0 weighted_x = 0 weighted_y = 0 for bot in self.neighbors_states: dx_temp = abs(self.neighbors_states[bot]['x'] - self.mouse.x) dy_temp = abs(self.neighbors_states[bot]['y'] - self.mouse.y) if dx_temp > dx: dx = dx_temp if dy_temp > dy: dy = dy_temp weighted_x = math.floor(((dx / dy) * (self.mouse.x + dx)) / 2) weighted_y = math.floor(((dy / dx) * (self.mouse.y + dy)) / 2) centroid = (weighted_x, weighted_y) return centroid def GroupCentroid(self): group_centroid = () sumCx = 0 sumCy = 0 sumAc = 0 lstP = self.neighbors_states.copy() # copy the neighbors_states for i in range(1, self.num_bots + 1): sumCx += lstP[i]['x'] sumCy += lstP[i]['y'] group_centroid = (math.ceil(sumCx / self.num_bots), math.ceil(sumCy / self.num_bots)) print("group_centroid check:", group_centroid) return group_centroid def BestMove(self, utility, maze): direction = ['Up', 'Down', 'Left', 'Right'] return (direction_best, movement_best, state_next_best, reward_max) def check_greatest_distance(self): x_Dir = None y_Dir = None cost = 2 shortest_path_list_x = [] shortest_path_list_y = [] # print("I'm in") for bots in self.neighbors_states: if bots != self.whoami: dx_temp = self.neighbors_states[bots]['x'] - self.mouse.x dy_temp = self.neighbors_states[bots]['y'] - self.mouse.y if dy_temp < 0 and self.mouse.direction is not 'DOWN': dy_temp *= cost elif dy_temp > 0 and self.mouse.direction is not 'UP': dy_temp *= cost if dx_temp < 0 and self.mouse.direction is not 'LEFT': dx_temp *= cost elif dx_temp > 0 and self.mouse.direction is not 'RIGHT': dx_temp *= cost shortest_path_list_x.append(dx_temp) shortest_path_list_y.append(dy_temp) return shortest_path_list_x, shortest_path_list_y def distance_to_wall(self, cell, direction): check = True open_distance = 0 move_dir = {'U': [0, -1], 'D': [0, 1], 'L': [-1, 0], 'R': [1, 0]} current_cell = [cell.x, cell.y ] # are we modifying current cell?? if so make a copy dir = direction[0] # print(dir) height = self.mouse.mazeMap.height width = self.mouse.mazeMap.width while check: if current_cell[0] >= 0 and current_cell[1] >= 0 and current_cell[ 0] < width and current_cell[1] < height: if self.mouse.mazeMap.getCell( current_cell[0], current_cell[1]).getIsThereWall(dir): # print("in the conditional") open_distance += 1 current_cell[0] += move_dir[dir][0] # print("current_cell_0:",current_cell[0]) current_cell[1] += move_dir[dir][1] else: check = False else: check = False return open_distance def cost(self, goal): direction_list = {'U': [0, -1], 'D': [0, 1], 'L': [-1, 0], 'R': [1, 0]} my_dir = 'U' # moves = [] state = (self.mouse.x, self.mouse.y) cost = 100 # some very high cost isGoal = False gamma = 0.01 # cost is lower when distance is longer expense = 0 hasBeen = [[0 for i in range(self.mouse.mazeMap.width)] for j in range(self.mouse.mazeMap.height)] takeAction = [] hasBeen[state[0]][state[1]] = 1 # has been at initial location open = [(expense, cost, state, my_dir) ] # some constants a start point and an end point while len(open) < 5 and len(open) > 0: # give me 20 good points item = open.pop(0) # pop appended item expense = item[0] cost = item[1] state = item[2] my_dir = item[3] counter = 0 for d in direction_list: if self.mouse.mazeMap.getCell( state[0], state[1]).getIsThereWall( d ) == False: # while mouse can move in some direction counter += 1 delta = direction_list[d] next_state = (state[0] + delta[0], state[1] + delta[1]) if hasBeen[next_state[0]][ next_state[1]] == 0: # hasn't been next_cost = cost + 1 if my_dir is d else 2 # expense = self.priority(next_state, d, goal) takeAction.append([expense, next_state, d]) hasBeen[next_state[0]][next_state[1]] = 1 open.append((expense, next_cost, next_state, d)) takeAction.sort() open.sort() # sort based on low expense return takeAction # return after only one iteration def dist(self, xy): x, y = xy dx_temp = 0 dy_temp = 0 distance = 0 # some small number dx_temp = x - self.mouse.x dy_temp = y - self.mouse.y distance = (dx_temp * dx_temp + dy_temp * dy_temp)**(1 / 2) return distance def priority(self, state, d, goal): goal_x, goal_y = goal x, y = state[0], state[1] alpha = 10 # weight for going in a straight line beta = 0.001 # weight for going towards gradient zeta = 1 epsilon = 0.1 energy = ((self.mouse.x - x)**2 + (self.mouse.y - y)**2)**(1 / 2) straight_line = 0 shortest_distance = 0 gradient = 0 expense = 0 cell = self.mouse.mazeMap.getCell(x, y) straight_line = self.distance_to_wall(cell, d) # print("straight line: ", straight_line) # gradient = (((x - self.GroupCentroid()[0])**2 + (y-self.GroupCentroid()[1])**2)**(1/2)) gradient = (((x - goal_x)**2 + (y - goal_y)**2)**(1 / 2)) expense = gradient / 2 return expense def follow_it(self, near_bot): cost = 0 state = (self.neighbors_states[near_bot]['x'], self.neighbors_states[near_bot]['y']) direction = self.neighbors_states[near_bot]['direction'] action = [(cost, state, direction)] return action def go(self): self.iterations += 1 self.mouse.senseWalls() self.mouse.getCurrentCell().getWhichIsWall() sendData = { 'robot': self.whoami, 'x': self.mouse.x, 'y': self.mouse.y, 'up': not self.mouse.canGoUp(), 'down': not self.mouse.canGoDown(), 'left': not self.mouse.canGoLeft(), 'right': not self.mouse.canGoRight(), 'direction': self.mouse.direction } # print(sendData) self.network.sendStringData(sendData) recvData = self.network.retrieveData() while recvData: otherMap = recvData cell = self.mouse.mazeMap.getCell(otherMap['x'], otherMap['y']) self.neighbors_states[otherMap['robot']] = { 'robot': otherMap['robot'], 'x': otherMap['x'], 'y': otherMap['y'], 'direction': self.mouse.direction } # update neighbors_states as received if otherMap['up']: self.mouse.mazeMap.setCellUpAsWall(cell) if otherMap['down']: self.mouse.mazeMap.setCellDownAsWall(cell) if otherMap['left']: self.mouse.mazeMap.setCellLeftAsWall(cell) if otherMap['right']: self.mouse.mazeMap.setCellRightAsWall(cell) recvData = self.network.retrieveData() print('-----------------------------') group_centroid = () distance = 0 goal = () action = () cell = self.mouse.mazeMap.getCell(self.mouse.x, self.mouse.y) direction = self.mouse.direction group_centroid = self.GroupCentroid() print("group centroid: ", group_centroid) far_distance, far_bot = self.distance_to_far_neigh() print("far distance:", far_distance) self.switchGoal = False moved = False threshold = 1 distance, near_bot = self.distance_to_near_neigh( ) # goal begins as near neighbor goal_1 = (None, None) head = True if near_bot > self.whoami else False goal = (self.neighbors_states[near_bot]['x'], self.neighbors_states[near_bot]['y']) print("far_distance", far_distance) print("far bot", far_bot) if far_distance < threshold: print("distance to far neighbor:", far_distance) self.isBack = True if (self.mouse.x, self.mouse.y) == goal: if not head: self.switchGoal = True action = self.follow_it(near_bot) else: far_distance, far_bot = self.distance_to_far_neigh() group_centroid = self.group_centroid close_group = (self.neighbors_states[far_bot]['x'], self.neighbors_states[far_bot]['y']) cent = self.dist(group_centroid) far = self.dist(close_group) goal = close_group if far < cent else group_centroids # goal = self.group_centroid # try group centroid else: switchGoal = False action = self.cost(goal) # if self.isBack: # print("RENDEZVOUS") # self.checkFinished() # check if finished for moves in action: # best action loop print(moves) x, y = moves[1] direction = moves[2] if moved == True: break print("x,y", (x, y)) print("self : ", self.switchGoal) print("head : ", head) if (x, y) == goal: if not head: self.switchGoal = True # x, y = (self.neighbors_states[near_bot]['x'], self.neighbors_states[near_bot]['y']) if self.isVisited[x][ y] == 0 or self.switchGoal: # may be some confusion here if not continuos print("in") print("self.isVisited[x][y]: ", self.isVisited[x][y] == 0) print(self.mouse.getCurrentCell().getWhichIsWall()) if self.mouse.x < x and self.mouse.canGoRight(): self.path.append([self.mouse.x, self.mouse.y]) self.isVisited[self.mouse.x + 1][self.mouse.y] = 1 self.mouse.goRight() moved = True self.neighbors_states[self.whoami] = { 'robot': self.whoami, 'x': self.mouse.x, 'y': self.mouse.y, 'direction': 'RIGHT' } print("RIGHT") elif self.mouse.x > x and self.mouse.canGoLeft(): self.path.append([self.mouse.x, self.mouse.y]) self.isVisited[self.mouse.x - 1][self.mouse.y] = 1 self.mouse.goLeft() moved = True self.neighbors_states[self.whoami] = { 'robot': self.whoami, 'x': self.mouse.x, 'y': self.mouse.y, 'direction': 'LEFT' } print("LEFT") elif self.mouse.y > y and self.mouse.canGoUp(): self.path.append([self.mouse.x, self.mouse.y]) self.isVisited[self.mouse.x][self.mouse.y - 1] = 1 self.mouse.goUp() moved = True self.neighbors_states[self.whoami] = { 'robot': self.whoami, 'x': self.mouse.x, 'y': self.mouse.y, 'direction': 'UP' } print("UP") elif self.mouse.y < y and self.mouse.canGoDown(): self.path.append([self.mouse.x, self.mouse.y]) self.isVisited[self.mouse.x][self.mouse.y + 1] = 1 self.mouse.goDown() moved = True self.neighbors_states[self.whoami] = { 'robot': self.whoami, 'x': self.mouse.x, 'y': self.mouse.y, 'direction': 'DOWN' } print("DOWN") sleep(0.01) if moved == False: #backtrack if unable to move by best action if len(self.path) != 0: x, y = self.path.pop() if x < self.mouse.x: self.mouse.goLeft() self.neighbors_states[self.whoami] = { 'robot': self.whoami, 'x': self.mouse.x, 'y': self.mouse.y, 'direction': 'LEFT' } print("LEFT") moved = True elif x > self.mouse.x: self.mouse.goRight() self.neighbors_states[self.whoami] = { 'robot': self.whoami, 'x': self.mouse.x, 'y': self.mouse.y, 'direction': 'RIGHT' } moved = True print("RIGHT") elif y < self.mouse.y: self.mouse.goUp() self.neighbors_states[self.whoami] = { 'robot': self.whoami, 'x': self.mouse.x, 'y': self.mouse.y, 'direction': 'UP' } moved = True print("UP") elif y > self.mouse.y: self.mouse.goDown() self.neighbors_states[self.whoami] = { 'robot': self.whoami, 'x': self.mouse.x, 'y': self.mouse.y, 'direction': 'DOWN' } moved = True print("DOWN") else: self.isBack = True sleep(0.05)
class StrategyTestMultiDFS(Strategy): mouse = None isVisited = [] path = [] isBack = False network = None def __init__(self, mouse): self.mouse = mouse self.isVisited = [[0 for i in range(self.mouse.mazeMap.width)] for j in range(self.mouse.mazeMap.height)] self.isVisited[self.mouse.x][self.mouse.y] = 1 self.network = NetworkInterface() self.network.initSocket() self.network.startReceiveThread() def checkFinished(self): return self.isBack def go(self): self.mouse.senseWalls() print(self.mouse.getCurrentCell().getWhichIsWall()) sendData = { 'x': self.mouse.x, 'y': self.mouse.y, 'up': not self.mouse.canGoUp(), 'down': not self.mouse.canGoDown(), 'left': not self.mouse.canGoLeft(), 'right': not self.mouse.canGoRight() } print(self.network.sendStringData(sendData)) recvData = self.network.retrieveData() print("recvData%s: " % recvData) while recvData: otherMap = recvData cell = self.mouse.mazeMap.getCell(otherMap['x'], otherMap['y']) self.isVisited[otherMap['x']][otherMap['y']] = 1 if otherMap['up']: self.mouse.mazeMap.setCellUpAsWall(cell) if otherMap['down']: self.mouse.mazeMap.setCellDownAsWall(cell) if otherMap['left']: self.mouse.mazeMap.setCellLeftAsWall(cell) if otherMap['right']: self.mouse.mazeMap.setCellRightAsWall(cell) recvData = self.network.retrieveData() if self.mouse.canGoLeft() and not self.isVisited[self.mouse.x - 1][self.mouse.y]: self.path.append([self.mouse.x, self.mouse.y]) self.isVisited[self.mouse.x - 1][self.mouse.y] = 1 self.mouse.goLeft() elif self.mouse.canGoUp( ) and not self.isVisited[self.mouse.x][self.mouse.y - 1]: self.path.append([self.mouse.x, self.mouse.y]) self.isVisited[self.mouse.x][self.mouse.y - 1] = 1 self.mouse.goUp() elif self.mouse.canGoRight() and not self.isVisited[self.mouse.x + 1][self.mouse.y]: self.path.append([self.mouse.x, self.mouse.y]) self.isVisited[self.mouse.x + 1][self.mouse.y] = 1 self.mouse.goRight() elif self.mouse.canGoDown( ) and not self.isVisited[self.mouse.x][self.mouse.y + 1]: self.path.append([self.mouse.x, self.mouse.y]) self.isVisited[self.mouse.x][self.mouse.y + 1] = 1 self.mouse.goDown() else: if len(self.path) != 0: x, y = self.path.pop() if x < self.mouse.x: self.mouse.goLeft() elif x > self.mouse.x: self.mouse.goRight() elif y < self.mouse.y: self.mouse.goUp() elif y > self.mouse.y: self.mouse.goDown() else: self.isBack = True sleep(0.5)
class Strategy_SRSS(Strategy): network = None controlNet = None send_data_history = None local_id = 0 local_task_id = 0 local_energy_level = 100 local_direction = [1, 0] # Direction vector: not necessary to be normalized local_coordinate = [0, 0] local_task_destination = [0, 0] local_step_size = 1 local_go_interval = 0.5 local_round = 0 local_stage = 'start' local_step = 0 local_negotiation = 1 local_queue = [] # [3, 1, 2] means the priority: robot-3 > robot-1 > robot-2 local_negotiation_result = False # If all the queues are the same, set as True, otherwise, False global_num_robots = 1 global_num_tasks = 1 global_min_require_robots = 1 global_group_num_robots = 1 global_energy_level = {} # {1: 100, 2: 99, 3: 85, ...} global_negotiation_queue = {} # {'1': [3, 1, 2], '2': [3, 2, 1], '3': [3, 1, 2], ...} global_agreement = {} # {'1': True, '2': False, '3': True, ...} global_task_duration = 10 global_task_radius = 200 global_task_coordinate = [400, 400] global_robots_coordinate = {} local_debugger = None def __init__(self, id, \ coordinate=[50, 50], \ direction=[1, 1], \ step_size=1, \ go_interval=0.5, \ num_robots=1, \ controlNet='172.16.0.254'): self.local_id = id self.local_coordinate = coordinate self.local_direction = direction self.local_step_size = step_size self.local_go_interval = go_interval self.global_num_robots = num_robots self.controlNet = controlNet self.network = NetworkInterface(port=19999) self.network.initSocket() self.network.startReceiveThread() # Debugger tool: self.local_debugger = COREDebuggerVirtual((controlNet, 12888)) def checkFinished(self): return (math.fabs(self.local_task_destination[0] - self.local_coordinate[0]) < 5 and \ math.fabs(self.local_task_destination[1] - self.local_coordinate[1]) < 5) def go(self): if self.local_stage == 'start': self.local_round = self.local_round + 1 self.local_stage = 'selection' self.selection() elif self.local_stage == 'selection': self.local_stage = 'formation' self.formation() elif self.local_stage == 'formation': self.local_stage = 'routing' self.routing() elif self.local_stage == 'routing': if self.checkFinished(): self.local_debugger.send_to_monitor('I finished my task!') self.local_stage = 'end' else: self.routing() elif self.local_stage == 'end': self.broadcast_coordinate() # default stage is 'end' # if new tasks are released: local_stage -> 'start' else: print('Unknown state.') time.sleep(self.local_go_interval) def global_condition_func(self, recv_data): # TODO: # Check if there is a task released. # Set self.local_stage -> 'start' # If some task is executing, put it into a place to store pass def message_communication(self, send_data, condition_func, time_out=10): # input: send_data is a dictionary # output: recv_data is also a dictionary # new task release: trigger a new round of <selection-formation-routing> while True: time_start = time.time() self.network.sendStringData(send_data) # self.local_debugger.send_to_monitor('send: ' + str(send_data)) while time.time() - time_start < time_out: try: recv_data = self.network.retrieveData() if recv_data != None: # self.local_debugger.send_to_monitor('recv: ' + str(recv_data)) # if new task is released self.global_condition_func(recv_data) if condition_func(recv_data) == True: self.send_data_history = send_data return recv_data else: continue else: continue except Exception as e: raise e self.network.sendStringData(self.send_data_history) def get_basic_status(self): status_dict = { \ 'id': self.local_id, 'round': self.local_round, 'stage': self.local_stage } return status_dict def selection(self): self.selection_step1() is_negotiation = self.selection_step2() is_agreement = self.selection_step3() if is_agreement == False: while is_negotiation: self.local_negotiation = self.local_negotiation + 1 self.selection_step1() is_negotiation = self.selection_step2() is_agreement = self.selection_step3() if is_agreement == True: break else: continue self.local_negotiation = 1 self.selection_execution() # After this step, we get (self.local_task_id, self.global_group_num_robots) self.local_debugger.send_to_monitor('selection: ' + str((self.local_task_id, self.global_group_num_robots))) self.global_energy_level = {} self.global_agreement = {} # clear energy level data for future use. def selection_execution(self): n = self.global_num_robots p = [self.global_energy_level[self.local_queue[0]]] * n k = self.global_num_tasks M = [[0 for i in range(k)] for j in range(n)] D = [[0 for i in range(k)] for j in range(n)] energy_sum = [] partition_plan = [] energy_level_queue = [self.global_energy_level[self.local_queue[i]] for i in range(n)] for i in range(1, n): p[i] = p[i-1] + energy_level_queue[i] for i in range(n): M[i][0] = p[i] for i in range(k): M[0][i] = energy_level_queue[i] for i in range(1, n): for j in range(1, k): M[i][j] = float('inf') for x in range(i): s = max(M[x][j-1], p[i]-p[x]) if M[i][j] > s: M[i][j] = s D[i][j] = x partition_plan = [] while k > 1: partition_plan.append(D[n-1][k-1]) n = D[n-1][k-1] + 1 k = k - 1 partition_plan.reverse() myindex_in_queue = 0 for i in range(self.global_num_robots): if self.local_id == self.local_queue[i]: myindex_in_queue = i break self.local_task_id = 0 for i in range(self.global_num_tasks - 1): if myindex_in_queue > partition_plan[i]: self.local_task_id = self.local_task_id + 1 # If only one task if len(partition_plan) == 0: self.global_group_num_robots = self.global_num_robots else: if self.local_task_id == 0: my_group_num = partition_plan[0] + 1 elif self.local_task_id == self.global_num_tasks - 1: my_group_num = self.global_num_robots - partition_plan[-1] - 1 else: my_group_num = partition_plan[self.local_task_id] - partition_plan[self.local_task_id - 1] self.global_group_num_robots = my_group_num def check_recv_all_energy(self, recv_data): try: recv_id = recv_data['id'] self.global_energy_level[recv_id] = recv_data['energy'] if len(self.global_energy_level) == self.global_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_all_queue(self, recv_data): try: recv_id = recv_data['id'] self.global_negotiation_queue[recv_id] = recv_data['queue'] if len(self.global_negotiation_queue) == self.global_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_all_agreement(self, recv_data): try: recv_id = recv_data['id'] self.global_agreement[recv_id] = recv_data['end'] if len(self.global_agreement) == self.global_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e # Step1: Exchange energy level def selection_step1(self): send_data = self.get_basic_status() send_data['energy'] = self.local_energy_level self.message_communication(send_data, condition_func=self.check_recv_all_energy, time_out=3) if self.local_negotiation == 1: self.local_queue = [i[0] for i in sorted(self.global_energy_level.items(), key=lambda x:x[1])] elif self.local_negotiation == 2: self.local_queue = sorted(self.global_energy_level.iteritems(), key=lambda x:(x[1], x[0]), reverse = True) # Step2: Exchange priority queue def selection_step2(self): send_data = self.get_basic_status() send_data['queue'] = self.local_queue self.message_communication(send_data, condition_func=self.check_recv_all_queue, time_out=3) for key in self.global_negotiation_queue.keys(): if self.local_queue == self.global_negotiation_queue[key]: self.local_negotiation_result = True else: self.local_negotiation_result = False self.global_negotiation_queue = {} return self.local_negotiation_result # Step3: Agreement def selection_step3(self): send_data = self.get_basic_status() send_data['end'] = self.local_negotiation_result self.message_communication(send_data, condition_func=self.check_recv_all_agreement, time_out=3) is_agreement = True for value in self.global_agreement.values(): if value == False: is_agreement = False break self.global_negotiation_queue = {} return is_agreement def check_recv_mygroup_energy(self, recv_data): try: recv_id = recv_data['id'] recv_task_id = recv_data['task_id'] # throw out the packet that has different task_id if recv_task_id != self.local_task_id: return self.global_energy_level[recv_id] = recv_data['energy'] if len(self.global_energy_level) == self.global_group_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_mygroup_queue(self, recv_data): try: recv_id = recv_data['id'] recv_task_id = recv_data['task_id'] # throw out the packet that has different task_id if recv_task_id != self.local_task_id: return self.global_negotiation_queue[recv_id] = recv_data['queue'] if len(self.global_negotiation_queue) == self.global_group_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_mygroup_agreement(self, recv_data): try: recv_id = recv_data['id'] recv_task_id = recv_data['task_id'] # throw out the packet that has different task_id if recv_task_id != self.local_task_id: return self.local_debugger.send_to_monitor('recv (id, task_id): ' + str((recv_id, recv_task_id))) self.global_agreement[recv_id] = recv_data['end'] if len(self.global_agreement) == self.global_group_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def formation(self): self.formation_step1() is_negotiation = self.formation_step2() is_agreement = self.formation_step3() if is_agreement == False: while is_negotiation: self.local_negotiation = self.local_negotiation + 1 self.formation_step1() is_negotiation = self.formation_step2() is_agreement = self.formation_step3() if is_agreement == True: break else: continue self.local_negotiation = 1 self.formation_execution() # After this step, we get (self.local_task_id, self.global_group_num_robots) self.global_energy_level = {} # clear energy level data for future use. self.global_agreement = {} def formation_execution(self): myindex_in_queue = 0 for i in range(self.global_num_robots): if self.local_id == self.local_queue[i]: myindex_in_queue = i break theta = (2 * math.pi) / self.global_group_num_robots * myindex_in_queue self.local_task_destination[0] = self.global_task_coordinate[0] + self.global_task_radius * math.cos(theta) self.local_task_destination[1] = self.global_task_coordinate[1] + self.global_task_radius * math.sin(theta) self.local_direction[0] = self.local_task_destination[0] - self.local_coordinate[0] self.local_direction[1] = self.local_task_destination[1] - self.local_coordinate[1] self.local_debugger.send_to_monitor('destination: ' + str(self.local_task_destination)) def formation_step1(self): send_data = self.get_basic_status() send_data['energy'] = self.local_energy_level send_data['task_id'] = self.local_task_id self.message_communication(send_data, condition_func=self.check_recv_mygroup_energy, time_out=3) if self.local_negotiation == 1: self.local_queue = [i[0] for i in sorted(self.global_energy_level.items(), key=lambda x:x[1])] elif self.local_negotiation == 2: self.local_queue = sorted(self.global_energy_level.iteritems(), key=lambda x:(x[1], x[0]), reverse = True) def formation_step2(self): send_data = self.get_basic_status() send_data['queue'] = self.local_queue send_data['task_id'] = self.local_task_id self.message_communication(send_data, condition_func=self.check_recv_mygroup_queue, time_out=3) for key in self.global_negotiation_queue.keys(): if self.local_queue == self.global_negotiation_queue[key]: self.local_negotiation_result = True else: self.local_negotiation_result = False self.global_negotiation_queue = {} return self.local_negotiation_result def formation_step3(self): send_data = self.get_basic_status() send_data['end'] = self.local_negotiation_result send_data['task_id'] = self.local_task_id self.message_communication(send_data, condition_func=self.check_recv_mygroup_agreement, time_out=3) is_agreement = True for value in self.global_agreement.values(): if value == False: is_agreement = False break self.global_negotiation_queue = {} return is_agreement def check_recv_robots_coordinates(self, recv_data): try: recv_id = recv_data['id'] recv_coordinate = recv_data['coordinate'] # throw out the packet that has different task_id self.global_robots_coordinate[recv_id] = recv_coordinate if len(self.global_robots_coordinate) == self.global_group_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def routing(self): is_collision = self.routing_step1() if is_collision == True: is_negotiation = self.routing_step2() is_agreement = self.routing_step3() if is_agreement == False: while is_negotiation: self.local_negotiation = self.local_negotiation + 1 self.routing_step1() is_negotiation = self.routing_step2() is_agreement = self.routing_step3() if is_agreement == True: break else: continue self.local_negotiation = 1 self.routing_execution() # After this step, we get (self.local_task_id, self.global_group_num_robots) self.global_energy_level = {} # clear energy level data for future use. self.global_agreement = {} else: self.walk_one_step() def get_cross(p1, p2, p): return (p2[0] - p1[0]) * (p[1] - p1[1]) -(p[0] - p1[0]) * (p2[1] - p1[1]) def inside_point(p1, p2, p3, p4, p): return get_cross(p1, p2, p) * \ get_cross(p3, p4, p) >= 0 \ and get_cross(p2, p3, p) * \ get_cross(p4, p1, p) >= 0 \
class Strategy_SRSS(Strategy): network = None controlNet = None send_data_history = None time_start = None # For task release simulation -> Future work: real dynamic change. local_id = 0 local_task_id = 0 local_energy_level = 100 local_direction = [1, 0] # Direction vector: not necessary to be normalized local_robot_radius = 10 # not in __init__ local_coordinate = [0, 0] local_task_destination = [0, 0] local_step_size = 1 local_go_interval = 0.5 local_round = 0 local_stage = 'start' local_step = 0 local_negotiation = 1 local_has_gone = 0 # How many steps you go local_queue = [] # [3, 1, 2] means the priority: robot-3 > robot-1 > robot-2 local_negotiation_result = False # If all the queues are the same, set as True, otherwise, False local_collision_queue = [] # Only contains local collision queue global_num_robots = 1 global_num_tasks = 0 global_min_require_robots = 1 global_group_num_robots = 1 global_energy_level = {} # {1: 100, 2: 99, 3: 85, ...} global_negotiation_queue = {} # {'1': [3, 1, 2], '2': [3, 2, 1], '3': [3, 1, 2], ...} global_agreement = {} # {'1': True, '2': False, '3': True, ...} global_task_list = [] # [{'duration': 100, 'raduis': 20, 'coordinate': [100, 100]}, {'duration': 150, 'raduis': 30, 'coordinate': [200, 150]}, ...] global_new_task_list = [] # Same as global_robots_task_distance = {} # Used in formation: the distance matrix - robots vs. location of the task global_robots_coordinate = {} # {1: [xx, yy], ...} global_robots_polygon = {} # {1: [(x1, y1), (x2, y2), (x3, y3), (x4, y4)], ...} global_collision_queue = {} # {1: [3, 1], 2: [], 3: [3, 4], ...} global_robots_task_id = {} # {1: 2, 2:1, 3:1, ...} Only used in routing - first priority global_deadlock = {} global_is_finish_task = {} # {1: True, 4: False, 8: False} Only consider about my group global_time_step = 0 # For better logging and collecting data local_debugger = None def __init__(self, id, \ coordinate=[50, 50], \ direction=[1, 1], \ step_size=1, \ robot_radius=10, \ energy_level=100, \ go_interval=0.5, \ num_robots=1, \ controlNet='172.16.0.254'): self.local_id = id self.local_coordinate = coordinate self.local_direction = direction self.local_step_size = step_size self.local_robot_radius = robot_radius self.local_energy_level = energy_level self.local_go_interval = go_interval self.global_num_robots = num_robots self.controlNet = controlNet self.network = NetworkInterface(port=19999) self.network.initSocket() self.network.startReceiveThread() # Debugger tool: self.local_debugger = COREDebuggerVirtual((controlNet, 12888), path='/home/rick/Documents/research/SRSS/log', filename=str(self.local_id)) self.time_start = time.time() self.local_debugger.log_local('Start.', tag='Status') self.local_debugger.send_to_monitor({'id': self.local_id, 'coordinate': self.local_coordinate}, tag='Status') def checkFinished(self): return (math.fabs(self.local_task_destination[0] - self.local_coordinate[0]) < self.local_step_size and \ math.fabs(self.local_task_destination[1] - self.local_coordinate[1]) < self.local_step_size) def go(self): self.local_debugger.log_local('%.2f' % self.local_energy_level, tag='Battery Energy') if self.local_stage == 'start': # Wait until new task arrives if self.global_num_tasks > 0: self.local_stage = 'selection' else: self.local_energy_level = self.local_energy_level - 0.04 time.sleep(1) # self.global_condition_func() elif self.local_stage == 'selection': self.global_is_finish_task = {} self.local_round = self.local_round + 1 self.local_debugger.log_local('Selection Start.', tag='Status') self.selection() self.local_debugger.log_local('Selection End: Do Task %d.' % self.local_task_id, tag='Status') self.local_stage = 'formation' elif self.local_stage == 'formation': self.local_debugger.log_local('Formation Start.', tag='Status') self.formation() # Formation End printed in formation step3 self.local_stage = 'routing' elif self.local_stage == 'routing': self.global_time_step = self.global_time_step + 1 self.local_debugger.log_local('Routing Start.', tag='Status') if self.checkFinished(): self.local_debugger.send_to_monitor('I finished my task!') self.local_debugger.log_local('Task Finished.', tag='Status') self.local_stage = 'end' else: if not self.check_new_tasks(): self.broadcast_coordinate() # should broadcast original coodinate and polygon before routing. else: self.reallocate_tasks() return if not self.check_new_tasks(): self.broadcast_polygon() else: self.reallocate_tasks() return if not self.check_new_tasks(): self.routing() else: self.reallocate_tasks() return elif self.local_stage == 'end': self.global_time_step = self.global_time_step + 1 if not self.check_new_tasks(): self.broadcast_coordinate() else: self.reallocate_tasks() return if not self.check_new_tasks(): self.broadcast_polygon() else: self.reallocate_tasks() return if not self.check_new_tasks(): self.routing() else: self.reallocate_tasks() return # default stage is 'end' # if new tasks are released: local_stage -> 'start' else: print('Unknown state.') time.sleep(self.local_go_interval) self.reallocate_tasks() def reallocate_tasks(self): if self.local_stage != 'selection' and self.local_stage != 'formation': num_tasks_now = sum([i['start'] <= time.time()-self.time_start for i in self.global_task_list]) if self.global_num_tasks != num_tasks_now: self.global_num_tasks = num_tasks_now self.local_stage = 'selection' self.local_debugger.log_local('A New Task Released.', tag='Task') self.local_debugger.send_to_monitor('A New Task Released.', tag='Task') if self.local_id == 1: if self.global_num_tasks != 0: self.local_debugger.send_to_monitor({'new_task': True, 'coordinate': [int(self.global_task_list[self.global_num_tasks-1]['coordinate'][0]), \ int(self.global_task_list[self.global_num_tasks-1]['coordinate'][1])]}, tag='Status') def check_new_tasks(self): if self.local_stage != 'selection' and self.local_stage != 'formation': num_tasks_now = sum([i['start'] <= time.time()-self.time_start for i in self.global_task_list]) if self.global_num_tasks != num_tasks_now: return True else: return False else: return False def check_recv_is_finish_task(self): if sum([i == True for i in self.global_is_finish_task.values()]) == self.global_group_num_robots: return True else: return False def global_condition_func(self, recv_data): pass def message_communication(self, send_data, condition_func, time_out=10): # input: send_data is a dictionary # output: recv_data is also a dictionary # new task release: trigger a new round of <selection-formation-routing> while True: self.local_energy_level = self.local_energy_level - 0.01 # This is for preventing them to stuck here if everyone finished task if self.check_new_tasks(): return time_start = time.time() self.network.sendStringData(send_data) # self.local_debugger.send_to_monitor('send: ' + str(send_data)) while time.time() - time_start < time_out: try: recv_data = self.network.retrieveData() if recv_data != None: # self.local_debugger.send_to_monitor('recv: ' + str(recv_data)) # if new task is released self.global_condition_func(recv_data) if condition_func(recv_data) == True: self.send_data_history = send_data return recv_data else: continue else: continue except Exception as e: raise e self.network.sendStringData(self.send_data_history) def get_basic_status(self): status_dict = { \ 'id': self.local_id, 'round': self.local_round, 'stage': self.local_stage } return status_dict def selection(self): self.selection_step1() is_negotiation = self.selection_step2() is_agreement = self.selection_step3() if is_agreement == False: while is_negotiation: self.local_negotiation = self.local_negotiation + 1 self.selection_step1() is_negotiation = self.selection_step2() is_agreement = self.selection_step3() if is_agreement == True: break else: continue self.local_negotiation = 1 self.selection_execution() # After this step, we get (self.local_task_id, self.global_group_num_robots) self.local_debugger.send_to_monitor('selection: ' + str((self.local_task_id, self.global_group_num_robots))) self.global_energy_level = {} self.global_agreement = {} # clear energy level data for future use. def selection_execution(self): n = self.global_num_robots p = [self.global_energy_level[self.local_queue[0]]] * n k = self.global_num_tasks M = [[0 for i in range(k)] for j in range(n)] D = [[0 for i in range(k)] for j in range(n)] energy_sum = [] partition_plan = [] energy_level_queue = [self.global_energy_level[self.local_queue[i]] for i in range(n)] for i in range(1, n): p[i] = p[i-1] + energy_level_queue[i] for i in range(n): M[i][0] = p[i] for i in range(k): M[0][i] = energy_level_queue[i] for i in range(1, n): for j in range(1, k): M[i][j] = float('inf') for x in range(i): s = max(M[x][j-1], p[i]-p[x]) if M[i][j] > s: M[i][j] = s D[i][j] = x partition_plan = [] while k > 1: partition_plan.append(D[n-1][k-1]) n = D[n-1][k-1] + 1 k = k - 1 partition_plan.reverse() myindex_in_queue = 0 for i in range(self.global_num_robots): if self.local_id == self.local_queue[i]: myindex_in_queue = i break self.local_task_id = 0 for i in range(self.global_num_tasks - 1): if myindex_in_queue > partition_plan[i]: self.local_task_id = self.local_task_id + 1 # If only one task if len(partition_plan) == 0: self.global_group_num_robots = self.global_num_robots else: if self.local_task_id == 0: my_group_num = partition_plan[0] + 1 elif self.local_task_id == self.global_num_tasks - 1: my_group_num = self.global_num_robots - partition_plan[-1] - 1 else: my_group_num = partition_plan[self.local_task_id] - partition_plan[self.local_task_id - 1] self.global_group_num_robots = my_group_num def check_recv_all_energy(self, recv_data): try: recv_id = recv_data['id'] self.global_energy_level[recv_id] = recv_data['energy'] if len(self.global_energy_level) == self.global_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_all_queue(self, recv_data): try: recv_id = recv_data['id'] self.global_negotiation_queue[recv_id] = recv_data['queue'] if len(self.global_negotiation_queue) == self.global_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_all_agreement(self, recv_data): try: recv_id = recv_data['id'] self.global_agreement[recv_id] = recv_data['end'] if len(self.global_agreement) == self.global_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e # Step1: Exchange energy level def selection_step1(self): send_data = self.get_basic_status() send_data['energy'] = self.local_energy_level self.message_communication(send_data, condition_func=self.check_recv_all_energy, time_out=0.01) if self.local_negotiation == 1: self.local_queue = [i[0] for i in sorted(self.global_energy_level.items(), key=lambda x:x[1])] elif self.local_negotiation == 2: self.local_queue = [i[0] for i in sorted(self.global_energy_level.items(), key=lambda x:(x[1], x[0]))] # Step2: Exchange priority queue def selection_step2(self): send_data = self.get_basic_status() send_data['queue'] = self.local_queue self.message_communication(send_data, condition_func=self.check_recv_all_queue, time_out=0.01) self.local_negotiation_result = False for key in self.global_negotiation_queue.keys(): if self.local_queue == self.global_negotiation_queue[key]: continue else: self.local_negotiation_result = True break self.global_negotiation_queue = {} return self.local_negotiation_result # Step3: Agreement def selection_step3(self): send_data = self.get_basic_status() send_data['end'] = self.local_negotiation_result self.message_communication(send_data, condition_func=self.check_recv_all_agreement, time_out=0.01) is_agreement = True for value in self.global_agreement.values(): if value == True: is_agreement = False break self.global_agreement = {} return is_agreement def check_recv_mygroup_energy(self, recv_data): try: recv_id = recv_data['id'] recv_task_id = recv_data['task_id'] # throw out the packet that has different task_id if recv_task_id != self.local_task_id: return self.global_energy_level[recv_id] = recv_data['energy'] if len(self.global_energy_level) == self.global_group_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_mygroup_queue(self, recv_data): try: recv_id = recv_data['id'] recv_task_id = recv_data['task_id'] # throw out the packet that has different task_id if recv_task_id != self.local_task_id: return self.global_negotiation_queue[recv_id] = recv_data['queue'] if len(self.global_negotiation_queue) == self.global_group_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_mygroup_agreement(self, recv_data): try: recv_id = recv_data['id'] recv_task_id = recv_data['task_id'] # throw out the packet that has different task_id if recv_task_id != self.local_task_id: return self.global_agreement[recv_id] = recv_data['end'] if len(self.global_agreement) == self.global_group_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_mygroup_distance(self, recv_data): try: recv_id = recv_data['id'] recv_task_id = recv_data['task_id'] # throw out the packet that has different task_id if recv_task_id != self.local_task_id: return self.global_robots_task_distance[recv_id] = recv_data['dist'] if len(self.global_robots_task_distance) == self.global_group_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def formation(self): self.formation_step1() is_negotiation = self.formation_step2() is_agreement = self.formation_step3() if is_agreement == False: while is_negotiation: self.local_negotiation = self.local_negotiation + 1 self.formation_step1() is_negotiation = self.formation_step2() is_agreement = self.formation_step3() if is_agreement == True: break else: continue self.local_negotiation = 1 self.formation_execution() # After this step, we get (self.local_task_id, self.global_group_num_robots) self.global_energy_level = {} # clear energy level data for future use. self.global_agreement = {} self.global_robots_task_distance = {} def formation_execution(self): # Computing the distances to the different task locations dist_vector = [] for i in range(self.global_group_num_robots): theta = (2 * math.pi) / self.global_group_num_robots * i task_coordinate = np.array(self.global_task_list[self.local_task_id]['coordinate']) task_radius = self.global_task_list[self.local_task_id]['radius'] task_destination = task_coordinate + np.array([task_radius * math.cos(theta), task_radius * math.sin(theta)]) task_dist = np.linalg.norm(task_destination - np.array(self.local_coordinate)) dist_vector.append(task_dist) # Send the distance vector to others send_data = self.get_basic_status() send_data['dist'] = dist_vector send_data['task_id'] = self.local_task_id self.message_communication(send_data, condition_func=self.check_recv_mygroup_distance, time_out=0.01) dist_matrix = self.global_robots_task_distance # Everyone look at the distance matrix and use the consensus queue to sequentially choose task. myindex_in_queue = 0 for i in self.local_queue: task_chosen = dist_matrix[i].index(min(dist_matrix[i])) # task_chosen = dist_matrix[i].index(max(dist_matrix[i])) if i == self.local_id: myindex_in_queue = task_chosen break for j in dist_matrix: dist_matrix[j][task_chosen] = float('inf') # dist_matrix[j][task_chosen] = float('-inf') theta = (2 * math.pi) / self.global_group_num_robots * myindex_in_queue my_task_coordinate = self.global_task_list[self.local_task_id]['coordinate'] my_task_radius = self.global_task_list[self.local_task_id]['radius'] self.local_task_destination[0] = my_task_coordinate[0] + my_task_radius * math.cos(theta) self.local_task_destination[1] = my_task_coordinate[1] + my_task_radius * math.sin(theta) self.local_direction[0] = self.local_task_destination[0] - self.local_coordinate[0] self.local_direction[1] = self.local_task_destination[1] - self.local_coordinate[1] self.local_debugger.send_to_monitor('formation: index = ' + str(myindex_in_queue)) self.local_debugger.log_local('Formation End: To Position %d.' % myindex_in_queue, tag='Status') def formation_step1(self): send_data = self.get_basic_status() send_data['energy'] = self.local_energy_level send_data['task_id'] = self.local_task_id self.message_communication(send_data, condition_func=self.check_recv_mygroup_energy, time_out=0.01) if self.local_negotiation == 1: self.local_queue = [i[0] for i in sorted(self.global_energy_level.items(), key=lambda x:x[1])] # self.local_queue = [i[0] for i in sorted(self.global_energy_level.items(), key=lambda x:x[1], reverse=True)] elif self.local_negotiation == 2: self.local_queue = [i[0] for i in sorted(self.global_energy_level.items(), key=lambda x:(x[1], x[0]))] # self.local_queue = [i[0] for i in sorted(self.global_energy_level.items(), key=lambda x:(x[1], x[0]), reverse=True)] def formation_step2(self): send_data = self.get_basic_status() send_data['queue'] = self.local_queue send_data['task_id'] = self.local_task_id self.message_communication(send_data, condition_func=self.check_recv_mygroup_queue, time_out=0.01) self.local_negotiation_result = False for key in self.global_negotiation_queue.keys(): if self.local_queue == self.global_negotiation_queue[key]: continue else: self.local_negotiation_result = True break self.global_negotiation_queue = {} return self.local_negotiation_result def formation_step3(self): send_data = self.get_basic_status() send_data['end'] = self.local_negotiation_result send_data['task_id'] = self.local_task_id self.message_communication(send_data, condition_func=self.check_recv_mygroup_agreement, time_out=0.01) is_agreement = True for value in self.global_agreement.values(): if value == True: is_agreement = False break self.global_agreement = {} return is_agreement def check_recv_robots_coordinates(self, recv_data): try: recv_id = recv_data['id'] recv_coordinate = recv_data['coordinate'] recv_task_id = recv_data['task_id'] recv_is_finished = recv_data['is_finish'] # throw out the packet that has different task_id self.global_robots_coordinate[recv_id] = recv_coordinate if recv_task_id == self.local_task_id: self.global_is_finish_task[recv_id] = recv_data['is_finish'] if len(self.global_robots_coordinate) == self.global_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_robots_polygons(self, recv_data): try: recv_id = recv_data['id'] recv_polygon = recv_data['polygon'] # throw out the packet that has different task_id self.global_robots_polygon[recv_id] = recv_polygon if len(self.global_robots_polygon) == self.global_num_robots: return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_collision_queue(self, recv_data): try: recv_id = recv_data['id'] recv_collision = recv_data['collision_queue'] # throw out the packet that has different task_id self.global_collision_queue[recv_id] = recv_collision if len(self.global_collision_queue) == self.global_num_robots: return True else: #self.local_debugger.send_to_monitor('collision_queue: %d %d' % (len(self.global_collision_queue), self.global_num_robots)) return False except KeyError: pass except Exception as e: raise e def check_recv_local_collision_task_id(self, recv_data): try: recv_id = recv_data['id'] recv_task_id = recv_data['task_id'] # throw out the packet that has different task_id if recv_id in self.local_collision_queue: self.global_robots_task_id[recv_id] = recv_task_id if len(self.global_robots_task_id) == len(self.local_collision_queue): return True else: #self.local_debugger.send_to_monitor('collision_taskid: %d %d' % (len(self.global_robots_task_id), len(self.local_collision_queue))) return False except KeyError: pass except Exception as e: raise e def check_recv_local_collision_energy(self, recv_data): try: recv_id = recv_data['id'] # throw out the packet that has different task_id if recv_id in self.local_collision_queue: self.global_energy_level[recv_id] = recv_data['energy'] if len(self.global_energy_level) == len(self.local_collision_queue): return True else: #self.local_debugger.send_to_monitor('collision_taskid: %d %d' % (len(self.global_robots_task_id), len(self.local_collision_queue))) return False except KeyError: pass except Exception as e: raise e def check_recv_local_collision_queue(self, recv_data): try: recv_id = recv_data['id'] if recv_id in self.local_collision_queue: self.global_negotiation_queue[recv_id] = recv_data['queue'] if len(self.global_negotiation_queue) == len(self.local_collision_queue): return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_local_collision_agreement(self, recv_data): try: recv_id = recv_data['id'] if recv_id in self.local_collision_queue: self.global_agreement[recv_id] = recv_data['end'] if len(self.global_agreement) == len(self.local_collision_queue): return True else: return False except KeyError: pass except Exception as e: raise e def check_recv_deadlock(self, recv_data): try: recv_id = recv_data['id'] if recv_id in self.local_collision_queue: self.global_deadlock[recv_id] = recv_data['deadlock'] if len(self.global_deadlock) == len(self.local_collision_queue): return True else: return False except KeyError: pass except Exception as e: raise e def routing(self): self.routing_step1() is_collision = self.routing_step2() # Generate a local collision queue with in-group consensus if is_collision == True: self.routing_step3() # Generate a priority queue is_negotiation = self.routing_step4() is_agreement = self.routing_step5() if is_agreement == False: while is_negotiation: self.local_negotiation = self.local_negotiation + 1 self.routing_step3() is_negotiation = self.routing_step4() is_agreement = self.routing_step5() if is_agreement == True: break else: continue self.local_negotiation = 1 self.routing_execution() self.global_collision_queue = {} self.global_robots_task_id = {} self.global_energy_level = {} self.global_negotiation_queue = {} self.global_agreement = {} self.global_deadlock = {} else: self.walk_one_step() def rectangle_transform(self, rectangle, direction, local_coordinate): vertices = np.transpose(np.array([[rectangle[0][0], rectangle[0][1]], \ [rectangle[1][0], rectangle[1][1]], \ [rectangle[2][0], rectangle[2][1]], \ [rectangle[3][0], rectangle[3][1]]])) rotation_matrix = np.array([[math.cos(direction), - math.sin(direction)], \ [math.sin(direction), math.cos(direction)]]) new_vertices = np.matmul(rotation_matrix, vertices) update_coordinate = np.transpose(new_vertices + [[local_coordinate[0]], [local_coordinate[1]]]) return update_coordinate def vector_transform(self, vector, direction, local_coordinate): vertices = np.transpose(np.array([[vector[0][0], vector[0][1]], \ [vector[1][0], vector[1][1]]])) rotation_matrix = np.array([[math.cos(direction), - math.sin(direction)], \ [math.sin(direction), math.cos(direction)]]) new_vertices = np.matmul(rotation_matrix, vertices) update_coordinate = np.transpose(new_vertices + [[local_coordinate[0]], [local_coordinate[1]]]) return update_coordinate # Step1: Collision Detection def routing_step1(self): if self.local_direction[0] != 0: direction_angle = math.atan(self.local_direction[1] / self.local_direction[0]) else: direction_angle = 0 my_raw_rectangle = [[-self.local_robot_radius, self.local_robot_radius], [-self.local_robot_radius, -self.local_robot_radius], [self.local_robot_radius + self.local_step_size, -self.local_robot_radius], [self.local_robot_radius + self.local_step_size, self.local_robot_radius]] my_rectangle = self.rectangle_transform(my_raw_rectangle, direction_angle, self.local_coordinate) p1 = Polygon([ (my_rectangle[0][0], my_rectangle[0][1]), (my_rectangle[1][0], my_rectangle[1][1]), (my_rectangle[2][0], my_rectangle[2][1]), (my_rectangle[3][0], my_rectangle[3][1])]) self.local_queue = [] for index in range(self.global_num_robots): i = index + 1 if i != self.local_id: p2 = Polygon(self.global_robots_polygon[i]) if p1.intersects(p2): self.local_queue.append(i) if len(self.local_queue) > 0: self.local_queue.append(self.local_id) # After this step, only get the "potential" collision queue # Generate local collision queue: unique 'set' but is not unique 'queue' def routing_step2(self): send_data = self.get_basic_status() send_data['collision_queue'] = self.local_queue self.global_collision_queue = {} self.message_communication(send_data, condition_func=self.check_recv_collision_queue, time_out=0.01) u = unionfind(self.global_collision_queue.values()) u.createtree() collision_queues = u.printree() self.local_collision_queue = [] for queue in collision_queues: if self.local_id in queue: self.local_collision_queue = queue break # After this step, get the final local consensus collision queue if len(self.local_collision_queue) > 0: return True else: return False # Communicate task id as the first priority def routing_step3(self): send_data = self.get_basic_status() send_data['task_id'] = self.local_task_id self.global_robots_task_id = {} # TODO: check_recv_local_collision_task_id not exists self.message_communication(send_data, condition_func=self.check_recv_local_collision_task_id, time_out=0.01) send_data = self.get_basic_status() send_data['energy'] = self.local_energy_level self.global_energy_level = {} # TODO: check_recv_local_collision_task_id not exists self.message_communication(send_data, condition_func=self.check_recv_local_collision_energy, time_out=0.01) taskid_energy = [(i, self.global_robots_task_id[i], self.global_energy_level[i]) for i in self.local_collision_queue] if self.local_negotiation == 1: self.local_queue = [i[0] for i in sorted(taskid_energy, key=lambda x:(x[1], x[2]))] # # # self.local_queue = [i[0] for i in sorted(taskid_energy, key=lambda x:(x[1], x[2], x[0]))] # self.local_queue = [i[0] for i in sorted(taskid_energy, key=lambda x:(x[1], -x[2]))] elif self.local_negotiation == 2: self.local_queue = [i[0] for i in sorted(taskid_energy, key=lambda x:(x[2], x[0]))] # self.local_queue = [i[0] for i in sorted(taskid_energy, key=lambda x:(x[2], x[0]), reverse=True)] # Exchange Priority queue: Exactly same as selection part - step2 def routing_step4(self): send_data = self.get_basic_status() send_data['queue'] = self.local_queue self.message_communication(send_data, condition_func=self.check_recv_local_collision_queue, time_out=0.01) self.local_negotiation_result = False for key in self.global_negotiation_queue.keys(): if self.local_queue == self.global_negotiation_queue[key]: continue else: self.local_negotiation_result = True break self.global_negotiation_queue = {} return self.local_negotiation_result # Agreement: Also the same as selection - step3 def routing_step5(self): send_data = self.get_basic_status() send_data['end'] = self.local_negotiation_result self.message_communication(send_data, condition_func=self.check_recv_local_collision_agreement, time_out=0.01) is_agreement = True for value in self.global_agreement.values(): if value == True: is_agreement = False break self.global_agreement = {} return is_agreement def routing_execution(self): # Loop until deadlock dismisses while True: self.global_deadlock = {} if self.local_id == self.local_queue[0]: dist_queue = {} for robot_id in self.local_queue: if robot_id != self.local_id: dist = np.linalg.norm([self.global_robots_coordinate[robot_id][0][1] - self.local_coordinate[1], \ self.global_robots_coordinate[robot_id][0][0] - self.local_coordinate[0]]) dist_queue[robot_id] = dist ordered_queue = [i[0] for i in sorted(dist_queue.items(), key=lambda x:x[1])] is_deadlock = True if not self.checkFinished(): closest_robot_coordinate = np.array(self.global_robots_coordinate[ordered_queue[0]][0]) my_coordinate = np.array(self.local_coordinate) my_direction = np.array(self.local_direction) # theta = boundary tangent angle not to collide theta = math.atan(2 * self.local_robot_radius / (np.linalg.norm(closest_robot_coordinate - my_coordinate) - self.local_robot_radius * 2)) # theta1 = acos(a`b/(|a||b|)) relative_direction = closest_robot_coordinate - my_coordinate theta1 = math.acos(np.inner(my_direction, relative_direction) / (np.linalg.norm(my_direction) * np.linalg.norm(relative_direction))) if self.local_direction[0] != 0: my_angle = math.atan(self.local_direction[1] / self.local_direction[0]) else: my_angle = math.pi / 2 if theta1 < theta: # new_angle = my_angle + theta - theta1 + 10 * math.pi / 180 new_angle = my_angle + theta - theta1 + 10 * math.pi / 180 + 7 else: new_angle = my_angle self.local_direction = [math.cos(new_angle), math.sin(new_angle)] is_deadlock = False else: # I have finish. I will not move is_deadlock = True send_data = self.get_basic_status() send_data['deadlock'] = is_deadlock self.message_communication(send_data, condition_func=self.check_recv_deadlock, time_out=0.01) if is_deadlock == False: self.walk_one_step() break else: # shift my local_queue: put myself to the last self.local_queue = self.local_queue[1:] + self.local_queue[0:1] else: # I am not the first one in queue send_data = self.get_basic_status() send_data['deadlock'] = False self.message_communication(send_data, condition_func=self.check_recv_deadlock, time_out=0.01) # The first one in queue has deadlock -> shift if self.global_deadlock[self.local_queue[0]] == True: self.local_queue = self.local_queue[1:] + self.local_queue[0:1] else: # deadlock released, keep in place self.local_debugger.log_local('Routing %d: Walk: %d' % (self.global_time_step, self.local_has_gone), tag='Status') self.local_energy_level = self.local_energy_level - 0.04 break def broadcast_coordinate(self): # Coordinates of current and next L2norm = math.sqrt(self.local_direction[0] * self.local_direction[0] + self.local_direction[1] * self.local_direction[1]) if L2norm != 0 and not self.checkFinished(): cur_next_coordinate = [[self.local_coordinate[0], self.local_coordinate[1]], \ [self.local_coordinate[0] + self.local_step_size * self.local_direction[0] / L2norm, \ self.local_coordinate[1] + self.local_step_size * self.local_direction[1] / L2norm]] else: cur_next_coordinate = [[self.local_coordinate[0], self.local_coordinate[1]], \ [self.local_coordinate[0], self.local_coordinate[1]]] send_data = self.get_basic_status() send_data['coordinate'] = cur_next_coordinate send_data['task_id'] = self.local_task_id send_data['is_finish'] = self.checkFinished() self.global_robots_coordinate = {} self.message_communication(send_data, condition_func=self.check_recv_robots_coordinates, time_out=0.01) def broadcast_polygon(self): # Polygon send_data = self.get_basic_status() if self.local_direction[0] != 0: direction_angle = math.atan(self.local_direction[1] / self.local_direction[0]) else: direction_angle = 0 my_raw_rectangle = [[-self.local_robot_radius, self.local_robot_radius], [-self.local_robot_radius, -self.local_robot_radius], [self.local_robot_radius + self.local_step_size, -self.local_robot_radius], [self.local_robot_radius + self.local_step_size, self.local_robot_radius]] my_rectangle = self.rectangle_transform(my_raw_rectangle, direction_angle, self.local_coordinate) polygon = [ (my_rectangle[0][0], my_rectangle[0][1]), (my_rectangle[1][0], my_rectangle[1][1]), (my_rectangle[2][0], my_rectangle[2][1]), (my_rectangle[3][0], my_rectangle[3][1])] send_data['polygon'] = polygon self.global_robots_polygon = {} self.message_communication(send_data, condition_func=self.check_recv_robots_polygons, time_out=0.01) def walk_one_step(self): if not self.checkFinished(): L2norm = math.sqrt(self.local_direction[0] * self.local_direction[0] + self.local_direction[1] * self.local_direction[1]) if L2norm != 0: self.local_coordinate[0] = self.local_coordinate[0] + self.local_step_size * self.local_direction[0] / L2norm self.local_coordinate[1] = self.local_coordinate[1] + self.local_step_size * self.local_direction[1] / L2norm # Return to the task direction even if you change direction in routing in last step self.local_direction[0] = self.local_task_destination[0] - self.local_coordinate[0] self.local_direction[1] = self.local_task_destination[1] - self.local_coordinate[1] self.local_debugger.send_to_monitor({'id': self.local_id, 'coordinate': self.local_coordinate}, tag='Status') self.local_debugger.log_local({'id': self.local_id, 'coordinate': self.local_coordinate}, tag='Status') # # show in the core # core_cmd = "coresendmsg -a %s node number=%s xpos=%s ypos=%s" % (self.controlNet, \ # self.local_id, \ # str(int(self.local_coordinate[0])), \ # str(int(self.local_coordinate[1]))) # os.system(core_cmd) self.local_energy_level = self.local_energy_level - 0.1 self.local_has_gone = self.local_has_gone + 1 else: # If direction vector is 0-vector, keep in place self.local_energy_level = self.local_energy_level - 0.04 pass self.local_debugger.log_local('Routing %d: Walk: %d' % (self.global_time_step, self.local_has_gone), tag='Status')
#!/usr/bin/env python3 from task import NetworkInterface from map import Map from map_painter import MapPainter mazeMap = Map(8, 8, 40, 40) mapPainter = MapPainter(mazeMap) mapPainter.createWindow() mapPainter.drawMap() lastCell = mazeMap.getCell(2, 7) mapPainter.putRobotInCell(lastCell, 'yellow') network = NetworkInterface() network.initSocket() network.startReceiveThread() while True: recvData = network.retrieveData() if recvData: otherMap = recvData cell = mazeMap.getCell(otherMap['x'], otherMap['y']) if otherMap['up']: mazeMap.setCellUpAsWall(cell) if otherMap['down']: mazeMap.setCellDownAsWall(cell) if otherMap['left']: mazeMap.setCellLeftAsWall(cell) if otherMap['right']: mazeMap.setCellRightAsWall(cell) mapPainter.drawCell(cell, 'grey') mapPainter.putRobotInCell(lastCell) mapPainter.putRobotInCell(cell, 'yellow') lastCell = cell print('(' + str(otherMap['x']) + ', ' + str(otherMap['y']) + ') up:' + str(otherMap['up']) + ',down:' + str(otherMap['down']) +
class StrategyJaxRendezvous(Strategy): mouse = None network = None finished = False numNeighbors = 0 neighborInfo = {} centroid = [] group = [] groupCentroid = [] weights = [] visited = [] path = [] leader = 0 stayPut = False backTrack = False maxPathLength = 16 timeStep = 0 #TODO find better way to eliminated following if through walls #TODO parameterize timeStep, weighting, and max path length def __init__(self, mouse, numNeighbors, initLocations): self.weights = [0, 0, 0, 0] self.centroid = [-1, -1] self.groupCentroid = [-1, -1] self.stop = [-1, -1] self.mouse = mouse self.leader = self.mouse.id self.numNeighbors = numNeighbors for key, value in initLocations.items(): if key is not self.mouse.id: self.neighborInfo[key] = { 'x': value[0], 'y': value[1], 'direction': 'UP' } if key < self.leader: self.leader = key self.visited = [[-1 for i in range(self.mouse.mazeMap.width)] for j in range(self.mouse.mazeMap.height)] self.visited[self.mouse.x][self.mouse.y] = self.mouse.id self.network = NetworkInterface() self.network.initSocket() self.network.startReceiveThread() def checkFinished(self): x = 0 y = 0 self.finished = True if len(self.neighborInfo) is not self.numNeighbors: self.finished = False return self.finished for value in self.neighborInfo.values(): x = value['x'] - self.mouse.x y = value['y'] - self.mouse.y if math.sqrt((x * x) + (y * y)) < 1.0: self.finished = False break self.finished = False return self.finished def isAtCentroid(self): x = self.mouse.x - self.centroid[0] y = self.mouse.y - self.centroid[1] if math.sqrt((x * x) + (y * y)) is 0.0: return True else: return False #determines if mouse is surrounded by walls #if it is it will move in that direction def checkFreedom(self): freedom = [] if self.mouse.canGoLeft(): freedom.append('left') if self.mouse.canGoRight(): freedom.append('right') if self.mouse.canGoUp(): freedom.append('up') if self.mouse.canGoDown(): freedom.append('down') return freedom #returns len(group) def refineGroup(self, threshold): if len(self.group) is 0: return 0 withinThreshold = True distSqNeighbors = 0 for item in self.group: x = self.neighborInfo[item]['x'] - self.mouse.x y = self.neighborInfo[item]['y'] - self.mouse.y distSqNeighbors = (x * x) + (y * y) if distSqNeighbors > threshold * threshold: self.group.remove(item) return len(self.group) #returns true if all bots within threshold def checkGroupDist(self, distance): withinThreshold = True distSqNeighbors = 0 for item in self.group: x = self.neighborInfo[item]['x'] - self.mouse.x y = self.neighborInfo[item]['y'] - self.mouse.y distSqNeighbors = (x * x) + (y * y) if distSqNeighbors > distance * distance: withinThreshold = False break return withinThreshold #returns max distance squared of group member to group centroid def calcMaxDistFromGroupCentroid(self): if len(self.group) is 0 or self.groupCentroid is [-1, -1]: return sys.maxsize x = self.groupCentroid[0] - self.mouse.x y = self.groupCentroid[1] - self.mouse.y distSq = (x * x) + (y * y) maxDist = distSq for item in self.group: x = self.groupCentroid[0] - self.neighborInfo[item]['x'] y = self.groupCentroid[1] - self.neighborInfo[item]['y'] distSq = (x * x) + (y * y) if distSq > maxDist: distSq = maxDist return maxDist #returns max distance squared of member to centroid def calcMaxDistFromCentroid(self): if len(self.neighborInfo) < self.numNeighbors or self.centroid is [ -1, -1 ]: return sys.maxsize x = self.centroid[0] - self.mouse.x y = self.centroid[1] - self.mouse.y distSq = (x * x) + (y * y) maxDist = distSq for value in self.neighborInfo.values(): x = self.centroid[0] - value['x'] y = self.centroid[1] - value['y'] distSq = (x * x) + (y * y) if distSq > maxDist: distSq = maxDist return maxDist #returns true if closest to centroid def isClosestToGroupCentroid(self): if len(self.group) is 0 or self.groupCentroid is [-1, -1]: return sys.maxsize x = self.groupCentroid[0] - self.mouse.x y = self.groupCentroid[1] - self.mouse.y distSq = (x * x) + (y * y) currentValue = distSq isClosest = True for item in self.group: x = self.groupCentroid[0] - self.neighborInfo[item]['x'] y = self.groupCentroid[1] - self.neighborInfo[item]['y'] distSq = (x * x) + (y * y) if distSq < currentValue: isClosest = False break return isClosest #returns true if closest to group centroid def isClosestToCentroid(self): if len(self.neighborInfo) < self.numNeighbors or self.centroid is [ -1, -1 ]: return False x = self.centroid[0] - self.mouse.x y = self.centroid[1] - self.mouse.y distSq = (x * x) + (y * y) currentValue = distSq isClosest = True for value in self.neighborInfo.values(): x = self.centroid[0] - value[item]['x'] y = self.centroid[1] - value[item]['y'] distSq = (x * x) + (y * y) if distSq <= currentValue: isClosest = False break return isClosest #returns true if closest to group centroid and all within threshold def checkClosenessToGroupCentroid(self, threshold): if len(self.group) is 0 or self.groupCentroid is [-1, -1]: return sys.maxsize x = self.groupCentroid[0] - self.mouse.x y = self.groupCentroid[1] - self.mouse.y distSq = (x * x) + (y * y) currentValue = distSq if currentValue > threshold * threshold: return False isClosest = True for item in self.group: x = self.groupCentroid[0] - self.neighborInfo[item]['x'] y = self.groupCentroid[1] - self.neighborInfo[item]['y'] distSq = (x * x) + (y * y) if distSq < currentValue or distSq > threshold * threshold: isClosest = False break return isClosest #returns true if closest to centroid and all within threshold def checkClosenessToCentroid(self, threshold): if len(self.neighborInfo) < self.numNeighbors or self.centroid is [ -1, -1 ]: return False x = self.centroid[0] - self.mouse.x y = self.centroid[1] - self.mouse.y distSq = (x * x) + (y * y) currentValue = distSq if currentValue > threshold * threshold: return False isClosest = True for value in self.neighborInfo.values(): x = self.centroid[0] - value['x'] y = self.centroid[1] - value['y'] distSq = (x * x) + (y * y) if distSq <= currentValue or distSq > threshold * threshold: isClosest = False break return isClosest #returns true if all bots within threshold and removes bots outside maxDist def refineAndCheckGroupDist(self, threshold, distance): withinThreshold = True distSqNeighbors = 0 for item in self.group: x = self.neighborInfo[item]['x'] - self.mouse.x y = self.neighborInfo[item]['y'] - self.mouse.y distSqNeighbors = (x * x) + (y * y) if distSqNeighbors > threshold * threshold: self.group.remove(item) if distSqNeighbors > distance * distance: withinThreshold = False return withinThreshold #calculates global and if following anyone group centroids def calcCentroid(self): self.centroid = [self.mouse.x, self.mouse.y] for key, value in self.neighborInfo.items(): self.centroid[0] += value['x'] self.centroid[1] += value['y'] self.centroid[0] = math.floor(self.centroid[0] / (self.numNeighbors + 1)) self.centroid[1] = math.floor(self.centroid[1] / (self.numNeighbors + 1)) def weightByXY(self, x, y, multiplier): if multiplier is 0: return xDir = x - self.mouse.x yDir = y - self.mouse.y distSq = (xDir * xDir) + (yDir * yDir) if distSq is 0: return xDir /= distSq yDir /= distSq if xDir < 0: self.weights[0] += abs(xDir) * multiplier else: self.weights[1] += xDir * multiplier if yDir > 0: self.weights[2] += yDir * multiplier else: self.weights[3] += abs(yDir) * multiplier def weightByCentroid(self, multiplier): if multiplier is 0: return x = self.centroid[0] - self.mouse.x y = self.centroid[1] - self.mouse.y distSq = (x * x) + (y * y) if distSq is 0: return x /= distSq y /= distSq if x < 0: self.weights[0] += abs(x) * multiplier else: self.weights[1] += x * multiplier if y > 0: self.weights[2] += y * multiplier else: self.weights[3] += abs(y) * multiplier def calcGroupCentroid(self): if len(self.group) is 0: return self.groupCentroid = [self.mouse.x, self.mouse.y] for id in self.group: self.groupCentroid[0] += self.neighborInfo[id]['x'] self.groupCentroid[1] += self.neighborInfo[id]['y'] self.groupCentroid[0] = math.floor(self.groupCentroid[0] / (len(self.group) + 1)) self.groupCentroid[1] = math.floor(self.groupCentroid[1] / (len(self.group) + 1)) def weightByGroup(self, multiplier): if multiplier is 0: return x = self.groupCentroid[0] - self.mouse.x y = self.groupCentroid[1] - self.mouse.y distSq = (x * x) + (y * y) if distSq is 0: return x /= distSq y /= distSq if x < 0: self.weights[0] += abs(x) * multiplier else: self.weights[1] += x * multiplier if y > 0: self.weights[2] += y * multiplier else: self.weights[3] += abs(y) * multiplier #calculates weights based on neighbors distances def weightByNeighbor(self, multiplier): if multiplier is 0: return x = 0 y = 0 for key, value in self.neighborInfo.items(): x = value['x'] - self.mouse.x y = value['y'] - self.mouse.y #dist squared distSq = (x * x) + (y * y) if distSq is 0: continue x /= distSq y /= distSq if x < 0: self.weights[0] += abs(x) * multiplier else: self.weights[1] += x * multiplier if y > 0: self.weights[2] += y * multiplier else: self.weights[3] += abs(y) * multiplier def go(self): self.mouse.senseWalls() print(self.mouse.getCurrentCell().getWhichIsWall()) sendData = { 'id': self.mouse.id, 'direction': self.mouse.direction, 'x': self.mouse.x, 'y': self.mouse.y, 'up': not self.mouse.canGoUp(), 'down': not self.mouse.canGoDown(), 'left': not self.mouse.canGoLeft(), 'right': not self.mouse.canGoRight() } self.network.sendStringData(sendData) recvData = self.network.retrieveData() numPackets = 0 while recvData: otherMap = recvData cell = self.mouse.mazeMap.getCell(otherMap['x'], otherMap['y']) if self.visited[otherMap['x']][ otherMap['y']] is not self.numNeighbors + 1: self.visited[otherMap['x']][otherMap['y']] = otherMap['id'] if otherMap['id'] in self.neighborInfo: if otherMap['id'] < self.leader: self.leader = otherMap['id'] self.neighborInfo[otherMap['id']] = { 'x': otherMap['x'], 'y': otherMap['y'], 'direction': otherMap['direction'] } numPackets += 1 if otherMap['up']: self.mouse.mazeMap.setCellUpAsWall(cell) if otherMap['down']: self.mouse.mazeMap.setCellDownAsWall(cell) if otherMap['left']: self.mouse.mazeMap.setCellLeftAsWall(cell) if otherMap['right']: self.mouse.mazeMap.setCellRightAsWall(cell) recvData = self.network.retrieveData() #determine if tracking leader still viable option if self.stayPut: if self.mouse.x is self.neighborInfo[self.leader]['x'] and \ self.mouse.y is self.neighborInfo[self.leader]['y']: print('staying put') return else: print('cant stay put') self.stayPut = False #make sure neighbors are still around and initialize weights previousWeights = self.weights self.weights = [0, 0, 0, 0] groupWeight = 0 centroidWeight = 0 #not used at all currently neighborWeight = 0 groupSize = self.refineGroup(8) #calculate weights and centroids self.calcCentroid() self.calcGroupCentroid() #NOTE weight multipliers could be optimized or irradicated targetLeader = False if groupSize is self.numNeighbors: targetLeader = True if self.mouse.id is self.leader: self.centroid = [self.mouse.x, self.mouse.y] print('targeting leader') return else: x = self.neighborInfo[self.leader]['x'] y = self.neighborInfo[self.leader]['y'] self.weightByXY(x, y, 1) if self.path.count([x, y]): self.backTrack = True elif groupSize > 0: groupWeight = 1 neighborWeight = 2 self.weightByGroup(groupWeight) self.weightByNeighbor(neighborWeight) else: neighborWeight = 1 self.weightByNeighbor(neighborWeight) #determine freedom freedom = [] freedom = self.checkFreedom() #determine if pause is good idea if len(self.neighborInfo) is self.numNeighbors and \ (self.isAtCentroid() and len(self.group) is self.numNeighbors): return #sort weighted directions ranks = [('left', self.weights[0]), ('right', self.weights[1]), ('down', self.weights[2]), ('up', self.weights[3])] ranks = sorted(ranks, key=itemgetter(1)) #attempt to move moved = False for d in range(4): if self.backTrack: break direction = ranks[3 - d][0] if freedom.count(direction) is 0: continue if direction is 'left': prevVisitor = self.visited[self.mouse.x - 1][self.mouse.y] if prevVisitor is self.numNeighbors + 1: continue if prevVisitor is not self.mouse.id: self.path.append([self.mouse.x, self.mouse.y]) self.visited[self.mouse.x - 1][self.mouse.y] = self.mouse.id self.mouse.goLeft() moved = True elif direction is 'up': prevVisitor = self.visited[self.mouse.x][self.mouse.y - 1] if prevVisitor is self.numNeighbors + 1: continue if prevVisitor is not self.mouse.id: self.path.append([self.mouse.x, self.mouse.y]) self.visited[self.mouse.x][self.mouse.y - 1] = self.mouse.id self.mouse.goUp() moved = True elif direction is 'right': prevVisitor = self.visited[self.mouse.x + 1][self.mouse.y] if prevVisitor is self.numNeighbors + 1: continue if prevVisitor is not self.mouse.id: self.path.append([self.mouse.x, self.mouse.y]) self.visited[self.mouse.x + 1][self.mouse.y] = self.mouse.id self.mouse.goRight() moved = True elif direction is 'down': prevVisitor = self.visited[self.mouse.x][self.mouse.y + 1] if prevVisitor is self.numNeighbors + 1: continue if prevVisitor is not self.mouse.id: self.path.append([self.mouse.x, self.mouse.y]) self.visited[self.mouse.x][self.mouse.y + 1] = self.mouse.id self.mouse.goDown() moved = True if moved: if prevVisitor is not self.mouse.id and prevVisitor is not -1 and \ prevVisitor is not self.numNeighbors + 1 and self.group.count(prevVisitor) is 0: self.group.append(prevVisitor) break #backtrack if necessary, but can only go back 16 spaces if not moved and len(self.path) != 0: self.backTrack = False xp, yp = self.path.pop() if self.visited[xp][yp] is self.numNeighbors + 1: xp, yp = self.path.pop() if xp < self.mouse.x: self.mouse.goLeft() elif xp > self.mouse.x: self.mouse.goRight() elif yp < self.mouse.y: self.mouse.goUp() elif yp > self.mouse.y: self.mouse.goDown() #keep path length at max if len(self.path) > self.maxPathLength: self.path = self.path[1:] #check to see if rendezvous at leader complete - will break if one robot leaves if len(self.group) is self.numNeighbors and self.calcMaxDistFromCentroid() <= 2 and \ (targetLeader and self.mouse.x is self.neighborInfo[self.leader]['x'] and self.mouse.y is self.neighborInfo[self.leader]['y']): print('staying put') self.stayPut = True sleep(self.timeStep)