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
