remote_control.py

import pygame
import sys
from pexpect import pxssh
from pygame.locals import *
import threading
import socket
import sys


ODROID_HOST_ADDRESS = "10.5.37.177"


class OdroidConnection(pxssh.pxssh):
	def __init__(self, hostname, command_program_path, username="root", password="autonom1718"):
		pxssh.pxssh.__init__(self)
		self.hostname = hostname
		self.username = username
		self.password = password
		self.command_program_path = command_program_path
		self.connected = False

	def __enter__(self):
		self.connect()
		return self

	def __exit__(self, exc_type, exc_val, exc_tb):
		self.disconnect()

	def connect(self):
		"""
		Connects to a node via ssh. This may take a few seconds (due to the underlying module).
		:return:
		"""
		try:
			self.login(self.hostname, self.username, self.password,
					   original_prompt="root@odroid:~#")
		except pxssh.ExceptionPxssh:
			print("ERROR: failed to connect to odroid!")
			raise
		self.connected = True
		print("Connected to: " + str(self.username) + "@" + str(self.hostname))
		self.start_command_program()

	def disconnect(self):
		if self.connected:
			self.sendline("q")
			self.logout()
			self.connected = False
			print("Disconnected from: " + str(self.username) + "@" + str(self.hostname))

	def start_command_program(self):
		self.sendline(self.command_program_path)

	def set_speed(self, speed):
		self.sendline("s " + str(speed))

	def set_angle(self, angle):
		self.sendline("a " + str(angle))


def center(src, dest):
	src.centerx = dest.centerx
	src.centery = dest.centery


class Button:
	def __init__(self, rect, label, onPressed=None, onReleased=None):
		self.rect = rect
		self.label = label
		self.onPressed = onPressed
		self.onReleased = onReleased
		self.pressed = False

	def press(self):
		if not self.pressed and self.onPressed is not None:
			self.onPressed()
		self.pressed = True

	def release(self):
		if self.pressed and self.onReleased is not None:
			self.onReleased()
		self.pressed = False

	def draw(self, surf):
		# fill
		if not self.pressed:
			pygame.draw.rect(surf, (150, 150, 150), self.rect)
		else:
			pygame.draw.rect(surf, (255, 100, 100), self.rect)
		# frame
		pygame.draw.rect(surf, (10, 10, 10), self.rect, 1)
		# label
		font = pygame.font.Font(None, 36)
		text = font.render(self.label, True, (10, 10, 10))
		text_rect = text.get_rect()
		center(text_rect, self.rect)
		surf.blit(text, text_rect)


def receive_speed():
	sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
	server_address = ('0.0.0.0', 10150)
	sock.bind(server_address)
	while True:
		data, address = sock.recvfrom(30)
		if data:
			split = data[4:].decode("utf-8").split("\n")
			speed = float(split[0])
			change = float(split[1])
			print("Speed: " + str(speed))
			print("Change: " + str(change))

if __name__ == '__main__':
	conn = OdroidConnection(ODROID_HOST_ADDRESS, "/root/repos/Hochautomatisiertes-Fahren/modules/catkin_ws/src/VeloxProtocolLib/cmake-build-debug/TerminalControl")
	conn.connect()

	speedReceiver = threading.Thread(target=receive_speed)
	speedReceiver.start()

	FPS = 60
	pygame.init()
	fpsClock = pygame.time.Clock()

	BACKGROUND_COLOR = (255, 255, 255)
	SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480
	screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
	background = pygame.Surface(screen.get_size())
	background = background.convert()
	background.fill(BACKGROUND_COLOR)
	background.blit(background, (0,0))

	clock = pygame.time.Clock()
	pygame.key.set_repeat(1, 40)

	BUTTON_WIDTH = 200
	BUTTON_HEIGHT = 100
	PADDING = 10
	buttonsSurface = pygame.Surface((3 * BUTTON_WIDTH + 2 * PADDING, 2 * BUTTON_HEIGHT + PADDING))
	buttonsSurface = buttonsSurface.convert()
	buttonsSurface.fill(BACKGROUND_COLOR)

	SPEED = 1
	ANGLE = 30
	ZERO_SPEED = 0
	ZERO_ANGLE = -4

	forwards = Button(
		pygame.Rect(1 * (BUTTON_WIDTH + PADDING), 0 * (BUTTON_HEIGHT + PADDING), BUTTON_WIDTH, BUTTON_HEIGHT),
		"FORWARDS",
		onPressed=lambda: conn.set_speed(SPEED),
		onReleased=lambda: conn.set_speed(ZERO_SPEED))

	backwards = Button(
		pygame.Rect(1 * (BUTTON_WIDTH + PADDING), 1 * (BUTTON_HEIGHT + PADDING), BUTTON_WIDTH, BUTTON_HEIGHT),
		"BACKWARDS",
		onPressed=lambda: conn.set_speed(ZERO_SPEED),
		onReleased=lambda: conn.set_speed(ZERO_SPEED))

	left = Button(
		pygame.Rect(0 * (BUTTON_WIDTH + PADDING), 1 * (BUTTON_HEIGHT + PADDING), BUTTON_WIDTH, BUTTON_HEIGHT),
		"LEFT",
		onPressed=lambda: conn.set_angle(-ANGLE),
		onReleased=lambda: conn.set_angle(ZERO_ANGLE))

	right = Button(
		pygame.Rect(2 * (BUTTON_WIDTH + PADDING), 1 * (BUTTON_HEIGHT + PADDING), BUTTON_WIDTH, BUTTON_HEIGHT),
		"RIGHT",
		onPressed=lambda: conn.set_angle(ANGLE),
		onReleased=lambda: conn.set_angle(ZERO_ANGLE))

	buttons = [forwards, backwards, left, right]
	buttons_rect = buttonsSurface.get_rect()
	center(buttons_rect, background.get_rect())

	while True:
		for event in pygame.event.get():
			if event.type == QUIT:
				conn.disconnect()
				pygame.quit()
				sys.exit()
			elif event.type == KEYDOWN:
				if event.key == K_UP:
					forwards.press()
				elif event.key == K_DOWN:
					backwards.press()
				elif event.key == K_LEFT:
					left.press()
				elif event.key == K_RIGHT:
					right.press()
			elif event.type == KEYUP:
				if event.key == K_UP:
					forwards.release()
				elif event.key == K_DOWN:
					backwards.release()
				elif event.key == K_LEFT:
					left.release()
				elif event.key == K_RIGHT:
					right.release()

		background.fill(BACKGROUND_COLOR)

		for button in buttons:
			button.draw(buttonsSurface)

		background.blit(buttonsSurface, buttons_rect)
		screen.blit(background, (0, 0))
		pygame.display.update()
		fpsClock.tick(FPS)