def homeBots():
homing_seq = sharedData[3]
if homing_seq:
# print('home', homing_seq)
destinations = json.loads(homing_seq)
arrageBot(robotData, destinations)
sharedData[3] = ""
def initialize():
destX = set()
destY = set()
# for i in range(BOT_COUNT):
# robots_data.append(
# robot(
# (random.randint(5, ARENA_DIM-5 ),random.randint(5, ARENA_DIM-5 )), #TODO the corner bug should be resolved at the client
# 0,
# (random.randint(5, ARENA_DIM-5 ),random.randint(5, ARENA_DIM-5 )),
# 0
# )
# )
arrageBot(robots_data, [])
def initialize():
global robots_data
destX = set()
destY = set()
for i in range(BOT_COUNT):
robots_data.append(
robot(
(random.randint(5, ARENA_DIM-5 ),random.randint(5, ARENA_DIM-5 )), #TODO the corner bug should be resolved at the client
0,
(random.randint(5, ARENA_DIM-5 ),random.randint(5, ARENA_DIM-5 )),
0
)
)
arrageBot(robots_data, [])
def on_message(client, userdata, message):
global BOT_COUNT
# newBotDecode = BotPositionArr.FromString(message.payload)
decrypted = aesDecrypt(message.payload).decode('utf-8')
try:
messageString = decrypted.split(';')
if message.topic == TOPIC_COM:
if messageString[1] == 'get_servers':
print('client requests server name')
client.publish(
TOPIC_COM,
aesEncryptString('server_name_response;' + str(SWARM_ID) +
';' + swarm_name))
if message.topic == TOPIC_SEVER_COM:
if messageString[1] == 'connection_req':
BOT_COUNT = len(robotData)
print('client requests connection', {
'bot_count': BOT_COUNT,
'areana_dim': ARENA_DIM
})
client.publish(
TOPIC_SEVER_COM,
aesEncryptString('server_response;success;' +
json.dumps({
'bot_count': BOT_COUNT,
'areana_dim': ARENA_DIM
})),
qos=2)
if messageString[1] == 'set_dest':
print("Destination reset")
destinations = json.loads(messageString[2])
remapDes(destinations)
print(destinations)
arrageBot(robotData, destinations)
if messageString[1] == 'ping':
client.publish(TOPIC_SEVER_COM, aesEncryptString('ping;'))
if messageString[1] == 'battStat':
client.publish(TOPIC_SEVER_COM,
aesEncryptString('battStat;' + battStat()))
except Exception as e:
# pass
print("message format error", e)
def on_message(client, userdata, message):
# newBotDecode = BotPositionArr.FromString(message.payload)
# messageString = message.payload.decode('utf-8').split(';')
try:
if message.topic == TOPIC_SEVER_COM:
if messageString[1] == 'connection_req':
print('client requests connection')
client.publish(TOPIC_SEVER_COM, 'server_response;success;'+ json.dumps({'bot_count':BOT_COUNT, 'areana_dim':ARENA_DIM}))
if messageString[1] == 'set_dest':
print("Destination reset")
destinations = json.loads(messageString[2])
print(destinations)
arrageBot(robots_data , destinations)
if messageString[1] == 'ping':
client.publish(TOPIC_SEVER_COM, 'ping')
if messageString[1] == 'battStat':
client.publish(TOPIC_SEVER_COM, 'battStat;' + battStat())
except :
print("message format error")
def destinationCalculation(robots, broadcastPos, frame, client):
# check homing sequence
homeBots()
# create a array to store protobuf information
newBotPosArr = BotPositionArr()
# need to optimize
# print(robots)
# print(broadcastPos)
global robot, desReachedFlag
robots_data = []
keys = []
for key, robot_i in robots.items():
keys.append(key)
robots_data.append(robot(robot_i[0], 0, robot_i[4], 0))
# print(robots_data)
result = movements.action(robots_data)
# print('fin',result)
# count the bots who have reached the destination
countDesReach = 0
for i, robot_i in enumerate(result):
# calculate the direction
F = robot_i[0] * 150000 # resultant force
# F = min(0.5, F)
Dir = robot_i[1] # relustant force direction
dx = F * math.cos((Dir / 180 * math.pi))
dy = F * math.sin((Dir / 180 * math.pi))
# print(dx,dy)
# add the destination circle
frame = cv2.circle(frame, tuple(robots_data[i].des_pos), 1,
(0, 255, 0), 2)
# print(frame.shape)
#print((int(robots_data[i].init_pos[0] + dx), int(robots_data[i].init_pos[1] + dy)))
frame = cv2.line(
frame,
(int(robots_data[i].init_pos[0]), int(robots_data[i].init_pos[1])),
tuple(robots_data[i].des_pos), (0, 255, 0), 2)
frame = cv2.line(
frame,
(int(robots_data[i].init_pos[0]), int(robots_data[i].init_pos[1])),
(int(robots_data[i].init_pos[0] + dx),
int(robots_data[i].init_pos[1] + dy)), (0, 0, 255), 2)
# calculate the broadcast positions
broadcastPos[keys[i]] = positions(
robots[keys[i]][0], robots[keys[i]][3],
[robots_data[i].init_pos[0] + dx, robots_data[i].init_pos[1] + dy],
0)
# prepare data to send through mqtt
newBot = BotPosition()
newBot.bot_id = i
newBot.x_cod = robots_data[i].init_pos[0] / img_x * 30
newBot.y_cod = robots_data[i].init_pos[1] / img_y * 30
newBot.angle = 0
newBotPosArr.positions.append(newBot)
# print(distanceTwoPoints((int(robots_data[i].init_pos[0]), int(robots_data[i].init_pos[1])), tuple(robots_data[i].des_pos)))
if 40 < distanceTwoPoints(
(int(robots_data[i].init_pos[0]), int(robots_data[i].init_pos[1])),
tuple(robots_data[i].des_pos)):
countDesReach += 1
# find if the destination reached
if (countDesReach == 0):
if desReachedFlag:
# print('All the bots reached the destinations')
for i, robot_i in enumerate(result):
broadcastPos[keys[i]] = positions(
robots[keys[i]][0], robots[keys[i]][3], [
robots_data[i].init_pos[0],
robots_data[i].init_pos[1] + 5
], 0)
else:
temp = []
for i, robot_i in enumerate(result):
broadcastPos[keys[i]] = positions(
robots[keys[i]][0], robots[keys[i]][3],
[robots_data[i].des_pos[0], robots_data[i].des_pos[1]], 0)
for key in robotData:
temp.append({
'x': int(robotData[key][0][0]),
'y': int(robotData[key][0][1])
})
arrageBot(robotData, temp)
desReachedFlag = True
else:
desReachedFlag = False
# publishing data to mqtt
data = newBotPosArr.SerializeToString()
# print(data)
client.publish(TOPIC_SEVER_BOT_POS, aesEncrypt(data))
return broadcastPos