class StrategyTestDFSDisplayEV3(Strategy):
mouse = None
isVisited = []
path = []
isBack = False
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()
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':self.mouse.canGoUp(), 'down':self.mouse.canGoDown(), 'left':self.mouse.canGoLeft(), 'right':self.mouse.canGoRight()}
self.network.sendStringData(sendData)
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
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')
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)