RoboRally/roboflut/roboflut.py

180 lines
5.3 KiB
Python
Raw Normal View History

import sys
import socket
import time
import numpy as np
import pygame
from threading import Thread, Lock
import scipy.integrate
single_robot = True
class Server:
def __init__(self):
self.ip = '127.0.0.1'
self.recv_port = 1337 # port for receiving data
self.send_port = 1338 # port for sending data
self.recv_addr = socket.getaddrinfo(self.ip, self.recv_port)[0][-1]
if not single_robot:
self.robots = []
else:
self.rob = Robot()
self.receiver = Thread(target=self.remote_control)
self.receiver.start()
def remote_control(self):
print("setting up socket communication ...")
recv_socket = socket.socket()
recv_socket.bind(self.recv_addr)
while True:
print("waiting for connections on {} ...".format(self.recv_addr))
recv_socket.listen(2)
(comm_socket, address) = recv_socket.accept() # this blocks until someone connects to the socket
print("starting thread for handling communication with {}".format(address))
Thread(target=self.handle_communication, args=(comm_socket, address)).start()
def handle_communication(self, socket, address):
print("connected!")
listening = True
if not single_robot:
rob = Robot()
self.robots.append(rob)
try:
while listening:
# expected data: '(u1, u2)'\n"
# where ui = control for motor i
# ui \in [-1.0, 1.0]
data = socket.recv(4096)
data_str = data.decode()
#print("Data received: {}".format(data_str))
#print("processing data = {}".format(data_str))
l = data_str.strip('()\n').split(',')
#print("l = {}".format(l))
u1 = float(l[0])
#print("u1 = {}".format(u1))
u2 = float(l[1])
#print("u2 = {}".format(u2))
if single_robot:
self.rob.set_control(u1, u2)
else:
rob.set_control(u1, u2)
except ValueError:
print("ValueError: Data has wrong format.")
print("Data received: {}".format(data_str))
except IndexError:
print("IndexError: Data has wrong format.")
print("Data received: {}".format(data_str))
except Exception as e:
print("Some other error occured")
print("Exception: {}".format(e))
finally:
socket.close()
if not single_robot:
self.robots.remove(rob)
class Robot:
def __init__(self):
self.x = np.array([0.0, 0.0, 0.0])
self.u1 = 0.0
self.u2 = 0.0
self.r = 0.3
self.R = 0.5
self.d = 0.2
self.mutex = Lock()
self.omega_max = 50.0
self.integrator = scipy.integrate.ode(self.f)
self.integrator.set_integrator('dopri5')
self.simulator = Thread(target=self.simulate)
self.simulator.start()
def set_control(self, u1, u2):
self.mutex.acquire()
try:
self.u1 = u1
self.u2 = u2
finally:
self.mutex.release()
def simulate(self):
t0 = time.time()
t = time.time()-t0
while True:
self.mutex.acquire()
try:
self.integrator.set_f_params(self.omega_max * np.array([self.u1, self.u2]))
finally:
self.mutex.release()
self.integrator.set_initial_value(self.x, t)
dt = time.time() - t0 - t
self.x = self.integrator.integrate(self.integrator.t + dt)
t = time.time() - t0
def f(self, t, x, u):
r = self.r
d = self.d
R = self.R
theta = x[2]
omegar = u[0]
omegal = u[1]
dx = np.zeros((3,1))
dx[0] = (r/2.0 * np.cos(theta) - r * d/(2.0 * R) * np.sin(theta)) * omegar + \
(r/2.0 * np.cos(theta) + r * d/(2.0 * R) * np.sin(theta)) * omegal
dx[1] = (r/2.0 * np.sin(theta) + r * d/(2.0 * R) * np.cos(theta)) * omegar + \
(r/2.0 * np.sin(theta) - r * d/(2.0 * R) * np.cos(theta)) * omegal
dx[2] = r/(2.0 * R) * (omegar - omegal)
return dx
def main(args):
# wait for connections of socket
s = Server()
background_colour = (255, 255, 255)
(width, height) = (640, 480)
screen = pygame.display.set_mode((width, height))
pygame.display.set_caption('Roboflut')
screen.fill(background_colour)
pygame.display.flip()
running = True
while running:
r = s.rob
#for r in s.robots:
x = int(r.x[0] + width/2.0)
y = int(r.x[1] + height / 2.0)
phi = r.x[2]
phases = ( phi + np.pi) % (2 * np.pi ) - np.pi
if x < 0 or x > width or y < 0 or y > height:
r.x[0] = 0.0
r.x[1] = 0.0
c = int((phases + 2.0 * np.pi) / (4.0 * np.pi) * 255)
#color = (c, c, c)
color = (0,0,0)
screen.set_at((x, y), color)
pygame.display.flip()
if __name__ == '__main__':
main(sys.argv)