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()