def make_mov(parameters, particles): pm = [ parameters[0] / 1000., parameters[1] / 1000., parameters[2], parameters[3] ] stm_driver('go_to_global_point', pm) stamp = time.time() while not stm_driver('is_point_was_reached'): time.sleep(0.3) if (time.time() - stamp) > 30: return False particles = pf.particles_move(particles, parameters[:-1]) lidar_data = lidar_sense() particles = pf.particles_sense(particles, lidar_data) main_robot = pf.calculate_main(particles) send_message(str(main_robot)) return True
def movements(): stm = multiprocessing.Process(target=stmDriver.stm_loop, args=(input_command_queue, reply_to_fsm_queue)) stm.start() init_vk() parameters = [0, 0, 0] particles = [pf.Robot() for i in range(pf.particle_number)] main_robot = pf.calculate_main(particles) send_message(str(main_robot)) stm_driver('set_coordinates_without_movement', parameters) ax = plt.subplot(111) plot = ax.plot([], [], 'ro')[0] plt.axis([0, 2000, 0, 3000]) plt.show() cord = [0, 0] plot.set_data(cord[0], cord[1]) # make movement parameters = [1000, 0, 0, 4] make_mov(parameters, parameters) cord = [main_robot.x, main_robot.y] plot.set_data(cord[0], cord[1]) print main_robot