def main_loop(self):
self.lsq_dat = defaultdict(lambda: (0,0,0,0))
with self.pose_dat_lock:
for robot_id in self.pose_dat.keys():
try:
self.lsq_dat[robot_id] = CustomMaths.lsq_circle_fit(np.array(self.pose_dat[robot_id]))
except TypeError:
continue
self.calculate_dist_per_step()
self.send_message()
self.pose_dat = defaultdict(deque)
self.lsq_dat = defaultdict(lambda: (0,0,0,0))
def main_loop(self):
self.lsq_dat = defaultdict(lambda: (0, 0, 0, 0))
with self.pose_dat_lock:
for robot_id in self.pose_dat.keys():
try:
self.lsq_dat[robot_id] = CustomMaths.lsq_circle_fit(
np.array(self.pose_dat[robot_id]))
except TypeError:
continue
self.calculate_dist_per_step()
self.send_message()
self.pose_dat = defaultdict(deque)
self.lsq_dat = defaultdict(lambda: (0, 0, 0, 0))
def main():
global serial_buffer
global pose_dat
rr_thread = threading.Thread(target=rr_monitor)
rr_lock.acquire()
rr_thread.start()
serial_thread = threading.Thread(target=serial_monitor)
serial_lock.acquire()
serial_thread.start()
with serial_buffer_lock:
serial_buffer = 'cmd reset'
time.sleep(1)
try:
new_direction = 1
with serial_buffer_lock:
serial_buffer = 'msg {0}{1}{2}{3}{4}{5}{6}{7}{8}{9}{10}{11}{12}{13}{14}{15}{16}{17}{18}{19}'.format(*[chr(96+(new_direction<<2)) for _ in ordered_ids])
last_direction = new_direction
while True:
for _ in range(30):
time.sleep(1)
lsq_dat = {}
for robot_id in ordered_ids:
lsq_dat[robot_id] = None
def get_per_robot_command(bot_id):
value = (96+(new_direction<<2))
if (lsq_dat[bot_id] is not None) and last_direction is not 0:
adjust = calculate_adjust_response(lsq_dat[bot_id], last_direction)
value += adjust
if adjust is 1:
history_dict[bot_id][0][last_direction][0] -= 10
history_dict[bot_id][0][last_direction][1] += 10
elif adjust is 2:
history_dict[bot_id][0][last_direction][0] += 10
history_dict[bot_id][0][last_direction][1] -= 10
return chr(value)
with pose_dat_lock:
for robot_id in pose_dat.keys():
try:
lsq_dat[robot_id] = CustomMaths.lsq_circle_fit(np.array(pose_dat[robot_id]))
except TypeError:
lsq_dat[robot_id] = (0,0,0,0)
calculate_dist_per_step(last_direction)
new_direction = get_rand_dir()
with serial_buffer_lock:
serial_buffer = 'msg {0}{1}{2}{3}{4}{5}{6}{7}{8}{9}{10}{11}{12}{13}{14}{15}{16}{17}{18}{19}'.format(*[get_per_robot_command(bot_id) for bot_id in ordered_ids])
last_direction = new_direction
pose_dat = defaultdict(deque)
for bot in ordered_ids:
print(bot,end='')
for dir in [1,2,3,4]:
print('\t',end='')
print(history_dict[bot][1][dir])
except KeyboardInterrupt:
for bot in ordered_ids:
print(bot)
print("\tAdjusts")
for dir in [1,2,3,4]:
print('\t',end='')
print(history_dict[bot][0][dir])
print("\tDists:")
for dir in [1,2,3,4]:
print('\t',end='')
print(history_dict[bot][1][dir])
rr_lock.release()
rr_thread.join()
serial_lock.release()
serial_thread.join()
sys.exit()
def main():
global serial_buffer
global pose_dat
rr_thread = threading.Thread(target=rr_monitor)
rr_lock.acquire()
rr_thread.start()
serial_thread = threading.Thread(target=serial_monitor)
serial_lock.acquire()
serial_thread.start()
with serial_buffer_lock:
serial_buffer = 'cmd reset'
time.sleep(1)
try:
new_direction = 1
with serial_buffer_lock:
serial_buffer = 'msg {0}{1}{2}{3}{4}{5}{6}{7}{8}{9}{10}{11}{12}{13}{14}{15}{16}{17}{18}{19}'.format(
*[chr(96 + (new_direction << 2)) for _ in ordered_ids])
last_direction = new_direction
while True:
for _ in range(30):
time.sleep(1)
lsq_dat = {}
for robot_id in ordered_ids:
lsq_dat[robot_id] = None
def get_per_robot_command(bot_id):
value = (96 + (new_direction << 2))
if (lsq_dat[bot_id] is not None) and last_direction is not 0:
adjust = calculate_adjust_response(lsq_dat[bot_id],
last_direction)
value += adjust
if adjust is 1:
history_dict[bot_id][0][last_direction][0] -= 10
history_dict[bot_id][0][last_direction][1] += 10
elif adjust is 2:
history_dict[bot_id][0][last_direction][0] += 10
history_dict[bot_id][0][last_direction][1] -= 10
return chr(value)
with pose_dat_lock:
for robot_id in pose_dat.keys():
try:
lsq_dat[robot_id] = CustomMaths.lsq_circle_fit(
np.array(pose_dat[robot_id]))
except TypeError:
lsq_dat[robot_id] = (0, 0, 0, 0)
calculate_dist_per_step(last_direction)
new_direction = get_rand_dir()
with serial_buffer_lock:
serial_buffer = 'msg {0}{1}{2}{3}{4}{5}{6}{7}{8}{9}{10}{11}{12}{13}{14}{15}{16}{17}{18}{19}'.format(
*[
get_per_robot_command(bot_id)
for bot_id in ordered_ids
])
last_direction = new_direction
pose_dat = defaultdict(deque)
for bot in ordered_ids:
print(bot, end='')
for dir in [1, 2, 3, 4]:
print('\t', end='')
print(history_dict[bot][1][dir])
except KeyboardInterrupt:
for bot in ordered_ids:
print(bot)
print("\tAdjusts")
for dir in [1, 2, 3, 4]:
print('\t', end='')
print(history_dict[bot][0][dir])
print("\tDists:")
for dir in [1, 2, 3, 4]:
print('\t', end='')
print(history_dict[bot][1][dir])
rr_lock.release()
rr_thread.join()
serial_lock.release()
serial_thread.join()
sys.exit()
