def receive_send(self):
time_t = time.time()
while True:
current_pos = None
data = self.client_socket.recv(2048)
log_file.write(data+'\n')
log_file.flush()
if (data):
print ('Received %s from RPi' % (data))
split_data = data.split("|")
global exp, t_s, area, steps, numCycle, currentMap, exp, fsp
if (split_data[0] == 'EXPLORE'):
t_s = time.time()
exp = Exploration(sim=False)
current_pos = exp.robot.center
visited[tuple(current_pos)] = 1
update(exp.currentMap, exp.exploredArea, exp.robot.center, exp.robot.head,
START, GOAL, 0)
elif (split_data[0] == 'WAYPOINT'):
global waypoint
waypoint = map(int, split_data[1:])
waypoint[0] = 19 - waypoint[0]
elif (split_data[0] == 'COMPUTE'):
print 'Time 0: %s s' % (time.time() - time_t)
sensors = map(float, split_data[1:])
current_pos = exp.robot.center
current = exp.moveStep(sensors)
currentMap = exp.currentMap
if (not current[1]):
time_t = time.time()
move = combineMovement(current[0])
elapsedTime = round(time.time()-t_s, 2)
update(exp.currentMap, exp.exploredArea, exp.robot.center, exp.robot.head,
START, GOAL, elapsedTime)
steps += 1
current_pos = tuple(exp.robot.center)
if (current_pos in visited):
visited[current_pos] += 1
if (visited[current_pos] > 3):
neighbour = exp.getExploredNeighbour()
if (neighbour):
neighbour = np.asarray(neighbour)
fsp = FastestPath(currentMap, exp.robot.center, neighbour,
exp.robot.direction, None, sim=False)
fastestPath(fsp, neighbour, exp.exploredArea, None)
move.extend(combineMovement(fsp.movement))
exp.robot.phase = 2
exp.robot.center = neighbour
exp.robot.head = fsp.robot.head
exp.robot.direction = fsp.robot.direction
currentMap = exp.currentMap
if (np.array_equal(exp.robot.center, START) and exp.exploredArea > 50):
numCycle += 1
if (numCycle > 1 and steps > 4):
neighbour = exp.getExploredNeighbour()
if (neighbour):
neighbour = np.asarray(neighbour)
fsp = FastestPath(currentMap, exp.robot.center, neighbour,
exp.robot.direction, None, sim=False)
fastestPath(fsp, neighbour, exp.exploredArea, None)
move.extend(combineMovement(fsp.movement))
exp.robot.phase = 2
exp.robot.center = neighbour
exp.robot.head = fsp.robot.head
exp.robot.direction = fsp.robot.direction
currentMap = exp.currentMap
print 'Time 1: %s s' % (time.time() - time_t)
time_t = time.time()
global mdfCounter
if mdfCounter == 3:
get_msg = output_formatter('MOVEMENT', [str(exp.robot.descriptor_1()),
str(exp.robot.descriptor_2())] + move + ['S'])
mdfCounter = 0
else:
get_msg = output_formatter('MOVEMENT', [str(0), str(0)] + move + ['S'])
mdfCounter += 1
print 'Time 2: %s s' % (time.time() - time_t)
else:
move = combineMovement(current[0])
get_msg = output_formatter('MOVEMENT', [str(exp.robot.descriptor_1()),
str(exp.robot.descriptor_2())] + move)
self.client_socket.send(get_msg)
print ('Sent %s to RPi' % (get_msg))
log_file.write('Robot Center: %s\n' % (str(exp.robot.center)))
log_file.write('Sent %s to RPi\n\n' % (get_msg))
log_file.flush()
time.sleep(2)
update(exp.currentMap, exp.exploredArea, exp.robot.center, exp.robot.head,
START, GOAL, elapsedTime)
logger('Exploration Done !')
logger("Map Descriptor 1 --> "+str(exp.robot.descriptor_1()))
logger("Map Descriptor 2 --> "+str(exp.robot.descriptor_2()))
fsp = FastestPath(currentMap, exp.robot.center, START, exp.robot.direction,
None, sim=False)
logger('Fastest Path Started !')
fastestPath(fsp, START, exp.exploredArea, None)
move = combineMovement(fsp.movement)
currentMap = exp.currentMap
global direction
if (fsp.robot.direction == WEST):
calibrate_move = ['A', 'L', 'O']
else:
calibrate_move = ['L', 'O']
direction = NORTH
get_msg = output_formatter('DONE', [str(exp.robot.descriptor_1()),
str(exp.robot.descriptor_2())] + ['N'] + move +
calibrate_move)
self.client_socket.send(get_msg)
time.sleep(1)
get_msg = output_formatter('MOVEMENT', [str(exp.robot.descriptor_1()),
str(exp.robot.descriptor_2())] + ['N'] + move +
calibrate_move)
self.client_socket.send(get_msg)
print ('Sent %s to RPi' % (get_msg))
time_t = time.time()
log_file.write('Robot Center: %s\n' % (str(exp.robot.center)))
log_file.write('Sent %s to RPi\n\n' % (get_msg))
log_file.flush()
elif (split_data[0] == 'FASTEST'):
print currentMap
fsp = FastestPath(currentMap, START, GOAL, direction, waypoint, sim=False)
current_pos = fsp.robot.center
fastestPath(fsp, GOAL, 300, waypoint)
# move = fsp.movement
move = combineMovement(fsp.movement)
get_msg = output_formatter('FASTEST', ['N'] + move + ['C'])
self.client_socket.send(get_msg)
print ('Sent %s to RPi' % (get_msg))
elif (split_data[0] == 'MANUAL'):
manual_movement = split_data[1:]
for move in manual_movement:
exp.robot.moveBot(move)
def receive_send(self):
while True:
current_pos = None
data = self.client_socket.recv(1024)
log_file.write(data + '\n')
log_file.flush()
if (data):
print('Received %s from RPi' % (data))
split_data = data.split("|")
global exp, t_s, area, steps, numCycle, currentMap, exp, fsp
if (split_data[0] == 'EXPLORE'):
t_s = time.time()
exp = Exploration(sim=False)
current_pos = exp.robot.center
visited[tuple(current_pos)] = 1
update(exp.currentMap, exp.exploredArea, exp.robot.center,
exp.robot.head, START, GOAL, 0)
elif (split_data[0] == 'WAYPOINT'):
global waypoint
waypoint = map(int, split_data[1:])
waypoint[0] = 19 - waypoint[0]
elif (split_data[0] == 'COMPUTE'):
sensors = map(float, split_data[1:])
current_pos = exp.robot.center
current = exp.moveStep(sensors)
if (not current[1]) and (numCycle == 1):
move = current[0]
currentMap = exp.currentMap
elapsedTime = round(time.time() - t_s, 2)
update(exp.currentMap, exp.exploredArea,
exp.robot.center, exp.robot.head, START, GOAL,
elapsedTime)
steps += 1
current_pos = tuple(exp.robot.center)
if (np.array_equal(exp.robot.center, START)
and exp.exploredArea > 50):
numCycle += 1
get_msg = output_formatter('MOVEMENT', [
str(exp.robot.descriptor_1()),
str(exp.robot.descriptor_2())
] + move + ['S'])
else:
move = current[0]
get_msg = output_formatter('MOVEMENT', [
str(exp.robot.descriptor_1()),
str(exp.robot.descriptor_2())
] + move)
self.client_socket.send(get_msg)
print('Sent %s to RPi' % (get_msg))
log_file.write('Robot Center: %s\n' %
(str(exp.robot.center)))
log_file.write('Sent %s to RPi\n\n' % (get_msg))
log_file.flush()
time.sleep(2)
update(exp.currentMap, exp.exploredArea,
exp.robot.center, exp.robot.head, START, GOAL,
elapsedTime)
logger('Exploration Done !')
logger("Map Descriptor 1 --> " +
str(exp.robot.descriptor_1()))
logger("Map Descriptor 2 --> " +
str(exp.robot.descriptor_2()))
fsp = FastestPath(currentMap,
exp.robot.center,
START,
exp.robot.direction,
None,
sim=False)
logger('Fastest Path Started !')
fastestPath(fsp, START, exp.exploredArea, None)
move = combineMovement(fsp.movement)
global direction
if (fsp.robot.direction == WEST):
calibrate_move = ['A', 'L', 'D', 'D']
else:
calibrate_move = ['L', 'D', 'D']
direction = NORTH
get_msg = output_formatter('DONE', [
str(exp.robot.descriptor_1()),
str(exp.robot.descriptor_2())
] + ['N'] + move + calibrate_move)
self.client_socket.send(get_msg)
time.sleep(1)
get_msg = output_formatter('MOVEMENT', [
str(exp.robot.descriptor_1()),
str(exp.robot.descriptor_2())
] + ['N'] + move + calibrate_move)
self.client_socket.send(get_msg)
print('Sent %s to RPi' % (get_msg))
log_file.write('Robot Center: %s\n' %
(str(exp.robot.center)))
log_file.write('Sent %s to RPi\n\n' % (get_msg))
log_file.flush()
elif (split_data[0] == 'FASTEST'):
fsp = FastestPath(currentMap,
START,
GOAL,
direction,
waypoint,
sim=False)
current_pos = fsp.robot.center
fastestPath(fsp, GOAL, 100, waypoint)
move = combineMovement(fsp.movement)
get_msg = output_formatter('FASTEST', move)
self.client_socket.send(get_msg)
print('Sent %s to RPi' % (get_msg))
elif (split_data[0] == 'MANUAL'):
manual_movement = split_data[1:]
for move in manual_movement:
exp.robot.moveBot(move)
curmap[center[0]-1:center[0]+2, center[1]-1:center[1]+2] = 5
curmap[head[0], head[1]] = 6
print curmap
curmap[center[0]-1:center[0]+2, center[1]-1:center[1]+2] = 1
currentMap = np.zeros([20, 15])
exp = Exploration(sim=False)
current_pos = exp.robot.center
markArea(exp.currentMap, exp.robot.center, exp.robot.head)
data = "COMPUTE|24|35|35|4|3|58"
split_data = data.split("|")
sensors = map(float, split_data[1:])
current_pos = exp.robot.center
current = exp.moveStep(sensors)
move = current[0]
currentPos = tuple(exp.robot.center)
markArea(exp.currentMap, exp.robot.center, exp.robot.head)
data = "COMPUTE|15|25|35|4|5|16"
split_data = data.split("|")
sensors = map(float, split_data[1:])
current_pos = exp.robot.center
current = exp.moveStep(sensors)
move = current[0]
currentPos = tuple(exp.robot.center)
markArea(exp.currentMap, exp.robot.center, exp.robot.head)
data = "COMPUTE|0|14|35|3|3|6"
split_data = data.split("|")