def __init__(self, const_throttle=None):
super(AutoDriftEnv, self).__init__()
self.const_throttle = const_throttle
# save the last n commands (throttle and steering)
self.n_commands = 2
# interface to the car
self.viewer = CarController()
# action space
if const_throttle is None:
# define independent bounds for steering and throttle
self.action_space = spaces.Box(low=np.array([-MAX_STEERING, -1]),
high=np.array([MAX_STEERING, 1]),
dtype=np.float32)
else:
# steering only
self.action_space = spaces.Box(low=np.array([-MAX_STEERING]),
high=np.array([MAX_STEERING]),
dtype=np.float32)
# use pixels as input
self.observation_space = spaces.Box(low=0,
high=255,
shape=INPUT_DIM,
dtype=np.uint8)
def car_control_open(self, *args):
'''
open car camera and connect the car controller.
:return:
'''
print(">>> loading.")
self.host_detector.detect_hosts()
self.stream.pull() # call pull to init car camera.
# check ESP 8266 connection
if self.controller is None:
controller_ip = None
print("Found hosts:", self.host_detector.hosts)
for host in self.host_detector.hosts:
if 'esp' in host or 'ESP' in host:
controller_ip = self.host_detector.hosts[host]
break
# TODO: seems ESP8266 can't be detect
# used static IP for debug.
# IP is pre-found from router configuration.
if controller_ip is None:
controller_ip = '192.168.43.213'
# debug end.
if controller_ip is not None:
self.controller = CarController(controller_ip)
self.controller.status = 'online'
# TODO: check connection
# self.controller.test()
else:
print(">>> ERROR: can't find ESP8266. Current hosts: ",
self.host_detector.hosts)
msg = QtWidgets.QMessageBox.warning(
self,
u"Warning",
u"请检测汽车芯片ESP8266是否正确连接路由",
buttons=QtWidgets.QMessageBox.Ok,
defaultButton=QtWidgets.QMessageBox.Ok)
else:
# had connected before.
self.controller.status = 'online'
# open car video stream
frame = self.stream.read_buffer()
if frame is not None:
if not self.timer_stream.isActive():
self.timer_stream.start(30)
self.timer_stream.timeout.connect(self.stream_show)
def main():
pygame.init()
size = width, height = 800, 800
d_surf = pygame.display.set_mode(size)
clock = pygame.time.Clock()
car_id = 'CAR_ID'
wheel_id = 'WHEEL_ID'
lot_id = 'LOT_ID'
car_start_pos = (400, 600)
car_length = 90
car_width = 50
car_angle = 0
wheel_angle = 0
image_manager = ImageManager()
image_manager.put(car_id, 'images/car.png', (64, 112))
image_manager.put(wheel_id, 'images/tyre.png', (8, 24))
image_manager.put(lot_id, 'images/lot.png', (800, 800))
car = Car(car_id, wheel_id, car_start_pos, car_length, car_width,
car_angle, wheel_angle)
car_controller = CarController(car)
car_drawer = CarDrawer(image_manager)
while True:
delta_ms = clock.tick(30)
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
elif event.type in (pygame.KEYUP, pygame.KEYDOWN):
car_controller.handle_event(event)
car_controller.update()
car.update()
d_surf.blit(image_manager.get(lot_id), (0, 0))
car_drawer.draw(d_surf, car)
pygame.display.update()
#window.debug_menu.enabled = False
window.size=(width,height)
#Light(type='ambient', color=(0.3,0.3,0.3,1), direction=(1,1,1))
#ground = Entity(model='plane',shader=lit_with_shadows_shader, texture='white_cube',scale=(32,1,32),texture_scale=(32,32), collider='mesh',color=color.white)
ground = Entity(model='plane', texture='road',scale=(1000,1,1000), texture_scale=(500,500), collider='box')#,shader=lit_with_shadows_shader)
#ground = Entity(model='plane', scale=(20,1,20),texture_scale=(200,200), texture='road', collider='box')
sun = DirectionalLight(y=10, rotation=(160,90,0))
#sun._light.show_frustum()
Sky(color=color.rgb(200,200, 220, a=255) )
#player = FirstPersonController(model='cube', y=1, origin_y=-.5)
player=CarController(y=10,x=2)
def update():
pass
app.run()
window.color = color.black
window.exit_button.enabled = False
window.fps_counter.enabled = True
window.cog_button.enabled = False
#window.debug_menu.enabled = False
if mode == "test":
window.position = (0, 20)
window.borderless = False
else:
window.size = (width, height)
#
if hosting == True:
window.title = "host"
player = CarController(y=0.1, x=10, z=10)
code = createMap(width=10, height=10, roads=10)
gameSettings[1] = code
displayMap(code)
elif hosting == False:
window.title = "client"
player = CarController(y=0.1, x=10, z=10)
#Light(type='ambient', color=(0.3,0.3,0.3,1), direction=(1,1,1))
#ground = Entity(model='plane',shader=lit_with_shadows_shader, texture='white_cube',scale=(32,1,32),texture_scale=(32,32), collider='mesh',color=color.white)
#ground = Entity(model='plane',texture="track", scale=(200,1,200), collider='box',shader=lit_with_shadows_shader)
#ground = Entity(model='plane', scale=(20,1,20),texture_scale=(200,200), texture='road', collider='box')
#my_scene = load_blender_scene('gmae')
ground = Entity(
model='plane',
def __init__(self, cv_bridge, debug_stream=None):
'''
Init ros node to receive images
'''
self.cv_bridge = cv_bridge
self.debug_stream = debug_stream
if self.debug_stream:
self.debug_stream.create_stream('rgb', 'debug/rgb')
self.debug_stream.create_stream('depth', 'debug/depth')
# ================ Initialize controlling models ================
self.image_queue = Queue.Queue(maxsize=config.IMAGE_QUEUE_SIZE)
self.depth_image_queue = Queue.Queue(maxsize=config.IMAGE_QUEUE_SIZE)
# Segmentation
seg_config = path.join(DATA_FOLDER, 'semantic_seg_ENet.conf.json')
self.semantic_segmentation = SemanticSegmentation(
seg_config, debug_stream=debug_stream)
# Depth processor
self.depth_processor = DepthProcessor(debug_stream=debug_stream)
# Sign detector
self.image_processor = SignDetector(cv_bridge)
# Car controlling
self.car_controller = CarController(self.semantic_segmentation,
debug_stream=debug_stream)
self.sign_detector = SignDetector(cv_bridge)
# Setup pub/sub
# WTF BUG!!! https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/
self.rgb_camera_sub = rospy.Subscriber(
config.TOPIC_GET_IMAGE,
CompressedImage,
callback=self.callback_rgb_image,
queue_size=1,
buff_size=2**24)
if config.USE_DEPTH_CAMERA:
self.depth_camera_sub = rospy.Subscriber(
config.TOPIC_GET_DEPTH_IMAGE,
CompressedImage,
callback=self.callback_depth_image,
queue_size=1,
buff_size=2**24)
# Setup workers
control_worker = Thread(target=self.thread_car_control,
args=(self.image_queue,
self.depth_image_queue))
control_worker.setDaemon(True)
control_worker.start()
sign_worker = Thread(target=self.sign_detector.processing_thread,
args=(self.image_queue, ))
sign_worker.setDaemon(True)
sign_worker.start()
connection_worker = Thread(target=self.thread_refresh_connection)
connection_worker.setDaemon(True)
connection_worker.start()
self.last_time_image_received = time.time()
# Push init images to force model loading
image_np = np.zeros([240, 320, 3], dtype=np.uint8)
depth_image_np = np.zeros([240, 320], dtype=np.uint8)
try:
for _ in range(10):
if self.image_queue.full():
break
self.image_queue.put_nowait(image_np)
for _ in range(10):
if self.depth_image_queue.full():
break
self.depth_image_queue.put_nowait(depth_image_np)
except:
pass
class ImageProcessor:
def __init__(self, cv_bridge, debug_stream=None):
'''
Init ros node to receive images
'''
self.cv_bridge = cv_bridge
self.debug_stream = debug_stream
if self.debug_stream:
self.debug_stream.create_stream('rgb', 'debug/rgb')
self.debug_stream.create_stream('depth', 'debug/depth')
# ================ Initialize controlling models ================
self.image_queue = Queue.Queue(maxsize=config.IMAGE_QUEUE_SIZE)
self.depth_image_queue = Queue.Queue(maxsize=config.IMAGE_QUEUE_SIZE)
# Segmentation
seg_config = path.join(DATA_FOLDER, 'semantic_seg_ENet.conf.json')
self.semantic_segmentation = SemanticSegmentation(
seg_config, debug_stream=debug_stream)
# Depth processor
self.depth_processor = DepthProcessor(debug_stream=debug_stream)
# Sign detector
self.image_processor = SignDetector(cv_bridge)
# Car controlling
self.car_controller = CarController(self.semantic_segmentation,
debug_stream=debug_stream)
self.sign_detector = SignDetector(cv_bridge)
# Setup pub/sub
# WTF BUG!!! https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/
self.rgb_camera_sub = rospy.Subscriber(
config.TOPIC_GET_IMAGE,
CompressedImage,
callback=self.callback_rgb_image,
queue_size=1,
buff_size=2**24)
if config.USE_DEPTH_CAMERA:
self.depth_camera_sub = rospy.Subscriber(
config.TOPIC_GET_DEPTH_IMAGE,
CompressedImage,
callback=self.callback_depth_image,
queue_size=1,
buff_size=2**24)
# Setup workers
control_worker = Thread(target=self.thread_car_control,
args=(self.image_queue,
self.depth_image_queue))
control_worker.setDaemon(True)
control_worker.start()
sign_worker = Thread(target=self.sign_detector.processing_thread,
args=(self.image_queue, ))
sign_worker.setDaemon(True)
sign_worker.start()
connection_worker = Thread(target=self.thread_refresh_connection)
connection_worker.setDaemon(True)
connection_worker.start()
self.last_time_image_received = time.time()
# Push init images to force model loading
image_np = np.zeros([240, 320, 3], dtype=np.uint8)
depth_image_np = np.zeros([240, 320], dtype=np.uint8)
try:
for _ in range(10):
if self.image_queue.full():
break
self.image_queue.put_nowait(image_np)
for _ in range(10):
if self.depth_image_queue.full():
break
self.depth_image_queue.put_nowait(depth_image_np)
except:
pass
def thread_refresh_connection(self):
'''
Monitor and restart image topic if we don't receive from image topic for a long time
This prevent car freezing when start simulator after start controling node.
This idea comes from https://github.com/RobotWebTools/rosbridge_suite/issues/298.
'''
while True:
time.sleep(3)
if time.time() - self.last_time_image_received > 3:
self.rgb_camera_sub.unregister()
# WTF BUG!!! https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/
self.rgb_camera_sub = rospy.Subscriber(
config.TOPIC_GET_IMAGE,
CompressedImage,
callback=self.callback_rgb_image,
queue_size=1,
buff_size=2**24)
if config.USE_DEPTH_CAMERA:
self.depth_camera_sub.unregister()
self.depth_camera_sub = rospy.Subscriber(
config.TOPIC_GET_DEPTH_IMAGE,
CompressedImage,
callback=self.callback_depth_image,
queue_size=1,
buff_size=2**24)
def thread_car_control(self, image_queue, depth_image_queue):
while True:
img = image_queue.get()
if config.USE_DEPTH_CAMERA:
depth_img = depth_image_queue.get()
self.car_controller.control(img, depth_img)
else:
self.car_controller.control(img)
def callback_rgb_image(self, data):
'''
Function to process rgb images
'''
try:
np_arr = np.fromstring(data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
# NOTE: image_np.shape = (240, 320, 3)
image_np = cv2.resize(image_np, (320, 240))
for _ in range(2):
if self.image_queue.full():
try:
self.image_queue.get_nowait()
except e:
print(e)
try:
self.image_queue.put_nowait(image_np)
except e:
print(e)
# self.car_controller.control(image_np)
self.last_time_image_received = time.time()
if self.debug_stream:
self.debug_stream.update_image('rgb', image_np)
except CvBridgeError as e:
print(e)
def callback_depth_image(self, data):
try:
np_arr = np.fromstring(data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.IMREAD_GRAYSCALE)
image_np = cv2.resize(image_np, (320, 240))
image = self.depth_processor.pre_processing_depth_img(image_np)
for _ in range(2):
if self.depth_image_queue.full():
try:
self.depth_image_queue.get_nowait()
except e:
print(e)
try:
self.depth_image_queue.put_nowait(image)
except e:
print(e)
except CvBridgeError as e:
print(e)
class AutoDriftEnv(gym.Env):
"""
Custom gym environment for the Autodrift Grand Prix.
For now, assume that camera image input is (80, 160, 3) (height, width, depth).
Command history is not supported yet, and with it jerk penalty is unsupported.
Frame skip is also not supported.
"""
metadata = {"render.modes": ["human", "rgb_array"]}
def __init__(self, const_throttle=None):
super(AutoDriftEnv, self).__init__()
self.const_throttle = const_throttle
# save the last n commands (throttle and steering)
self.n_commands = 2
# interface to the car
self.viewer = CarController()
# action space
if const_throttle is None:
# define independent bounds for steering and throttle
self.action_space = spaces.Box(low=np.array([-MAX_STEERING, -1]),
high=np.array([MAX_STEERING, 1]),
dtype=np.float32)
else:
# steering only
self.action_space = spaces.Box(low=np.array([-MAX_STEERING]),
high=np.array([MAX_STEERING]),
dtype=np.float32)
# use pixels as input
self.observation_space = spaces.Box(low=0,
high=255,
shape=INPUT_DIM,
dtype=np.uint8)
def step(self, action):
"""
:param action: (np.ndarray)
:return: (np.ndarray, float, bool, dict
"""
# action[0] = steering angle, action[1] = throttle
if self.const_throttle is None:
# variable throttle. convert throttle range -> [-1, 1] -> [0, 1] -> [MIN, MAX]
temp = (action[1] + 1) / 2
action[1] = (1 - temp) * MIN_THROTTLE + temp * MAX_THROTTLE
else:
# fixed throttle
action = np.concatenate([action, [self.const_throttle]])
# order the car
self.viewer.take_action(action)
observation, reward, done, info = self.viewer.observe()
logger.debug("Reward: {0}, isDone: {1}".format(reward, done))
return observation, reward, done, info
def reset(self):
print("One env timestep done. Resetting after 0.5s...")
# reset state of car controller, then sleep for 5s to replace car to start position manually
self.viewer.reset()
time.sleep(0.5)
print("Now resuming...")
observation, reward, done, info = self.viewer.observe()
return observation
def render(self, mode='rgb_array'):
if mode == 'rgb_array':
return self.viewer.image_array
return None
def close(self):
# nothing actually happens here
self.viewer.quit()
if len(sys.argv) > 1 and sys.argv[1] == '--debug':
logging.info('debug mode')
DEBUG = True
else:
logging.info('working mode')
# start the camera streaming server
camera = cam_streamer.CameraWrapper()
camera_thread = threading.Thread(target=camera.run)
camera_thread.start()
logging.info("Cam streamer started")
print("Cam streamer started")
# start the socket server to receive commands
cc = CarController()
logging.info("Car controller started")
print("Car controller started")
while True:
try:
logging.info('opening socket')
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind((HOST, PORT))
sock.listen(5)
logging.info('listening on ' + HOST + ':' + str(PORT))
reset = False
while reset == False:
conn, addr = sock.accept()
print('Connected by', addr)
while True:
data = conn.recv(1024)
class MainWindow(QMainWindow, Ui_MainWindow):
# define signals
signal_gesture_read = pyqtSignal(name="signal_gesture_read")
signal_prediction_show = pyqtSignal(name="signal_prediction_show")
signal_command_send = pyqtSignal(str, name='signal_command_send')
def __init__(self, parent=None):
super(MainWindow, self).__init__(parent)
self.setupUi(self)
# CNN
self.model = load_model()
if torch.cuda.is_available():
self.gpu = True
self.model.cuda()
self.prediction_frequency = 10 # each 10 images arise a prediction
self.prediction_count = 0
# camera
self.camera = cv2.VideoCapture()
self.CAM_NUM = 0
self.camera_x0 = 400
self.camera_y0 = 200
self.camera_height = 200
self.camera_width = 200
# car video stream
self.stream = CarStream()
# car controller
self.controller = None
self.host_detector = HostDetector()
# control
self.timer_camera = QtCore.QTimer()
self.timer_stream = QtCore.QTimer()
self.horizontalSlider.setValue(70)
self.threshold = self.horizontalSlider.value()
# SLOT
self.slot_init()
@log_decorator
def slot_init(self):
self.timer_camera.timeout.connect(self.camera_show)
self.pushButton_open_cam.clicked.connect(self.camera_open)
self.pushButton_close_cam.clicked.connect(self.camera_close)
self.pushButton_left.clicked.connect(self.box_left)
self.pushButton_right.clicked.connect(self.box_right)
self.pushButton_down.clicked.connect(self.box_down)
self.pushButton_up.clicked.connect(self.box_up)
self.pushButton_connect_car.clicked.connect(self.car_control_open)
self.pushButton_disconnect.clicked.connect(self.car_control_close)
self.horizontalSlider.valueChanged.connect(self.threshold_update)
self.signal_prediction_show.connect(self.prediction_show)
self.signal_gesture_read.connect(self.gesture_show)
self.signal_command_send.connect(self.command_send)
@log_decorator
def car_control_open(self, *args):
'''
open car camera and connect the car controller.
:return:
'''
print(">>> loading.")
self.host_detector.detect_hosts()
self.stream.pull() # call pull to init car camera.
# check ESP 8266 connection
if self.controller is None:
controller_ip = None
print("Found hosts:", self.host_detector.hosts)
for host in self.host_detector.hosts:
if 'esp' in host or 'ESP' in host:
controller_ip = self.host_detector.hosts[host]
break
# TODO: seems ESP8266 can't be detect
# used static IP for debug.
# IP is pre-found from router configuration.
if controller_ip is None:
controller_ip = '192.168.43.213'
# debug end.
if controller_ip is not None:
self.controller = CarController(controller_ip)
self.controller.status = 'online'
# TODO: check connection
# self.controller.test()
else:
print(">>> ERROR: can't find ESP8266. Current hosts: ",
self.host_detector.hosts)
msg = QtWidgets.QMessageBox.warning(
self,
u"Warning",
u"请检测汽车芯片ESP8266是否正确连接路由",
buttons=QtWidgets.QMessageBox.Ok,
defaultButton=QtWidgets.QMessageBox.Ok)
else:
# had connected before.
self.controller.status = 'online'
# open car video stream
frame = self.stream.read_buffer()
if frame is not None:
if not self.timer_stream.isActive():
self.timer_stream.start(30)
self.timer_stream.timeout.connect(self.stream_show)
@log_decorator
def car_control_close(self, *args):
'''
close car camera stream and the connection of car controller.
:return:
'''
# disconnect with stream
self.timer_stream.timeout.disconnect(self.stream_show)
self.label_car.setText("Not recieve car video stream yet.")
self.controller.status = 'idle'
# @log_decorator
def stream_show(self):
'''
show camera image, emit signal_gesture_read signal
:return:
'''
frame = self.stream.read_buffer()
frame = cv2.resize(frame, (640, 480))
show = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
showImage = QtGui.QImage(show.data, show.shape[1], show.shape[0],
QtGui.QImage.Format_RGB888)
self.label_car.setPixmap(QtGui.QPixmap.fromImage(showImage))
@log_decorator
def camera_open(self, *args):
'''
open laptop camera
:param args:
:return:
'''
if self.timer_camera.isActive() == False:
flag = self.camera.open(self.CAM_NUM)
if flag == False:
msg = QtWidgets.QMessageBox.warning(
self,
u"Warning",
u"请检测相机与电脑是否连接正确",
buttons=QtWidgets.QMessageBox.Ok,
defaultButton=QtWidgets.QMessageBox.Ok)
else:
if not self.timer_camera.isActive():
self.timer_camera.start(30)
@log_decorator
def camera_close(self, *args):
'''
close laptop camera
:param args:
:return:
'''
self.timer_camera.stop()
self.camera.release()
self.label_camera.clear()
self.label_camera.setText("Not recieve car video stream yet.")
def camera_show(self):
'''
show camera image, emit signal_gesture_read signal
:return:
'''
flag, frame = self.camera.read()
frame = cv2.flip(frame, 3)
frame = cv2.resize(frame, (640, 480))
self.image = frame
# notations
threshold_str = "Current threshlold: %d %%" % self.threshold
cv2.putText(frame, 'Driver Romm <( ^-^ )>', (FX, FY), FONT, SIZE,
(0, 255, 0), 1, 1)
cv2.putText(frame, threshold_str, (FX, FY + 2 * FH), FONT, SIZE,
(0, 255, 0), 1, 1)
cv2.rectangle(frame, (self.camera_x0, self.camera_y0),
(self.camera_x0 + self.camera_width,
self.camera_y0 + self.camera_height), (0, 255, 0), 1)
show = cv2.cvtColor(self.image, cv2.COLOR_BGR2RGB)
showImage = QtGui.QImage(show.data, show.shape[1], show.shape[0],
QtGui.QImage.Format_RGB888)
self.label_camera.setPixmap(QtGui.QPixmap.fromImage(showImage))
# call gesture filter
self.signal_gesture_read.emit()
# @log_decorator
def gesture_show(self):
'''
show gesture image after binary mask, emit signal_prediction_show signal
:return:
'''
roi = skinMask(self.image, self.camera_x0, self.camera_y0,
self.camera_width, self.camera_height)
showImage = QtGui.QImage(roi.data, roi.shape[1], roi.shape[0],
QtGui.QImage.Format_Grayscale8)
self.label_gesture.setPixmap(QtGui.QPixmap.fromImage(showImage))
# predict image
if self.prediction_count == self.prediction_frequency:
t = threading.Thread(target=models.predict_gesture,
args=[self.model, roi],
kwargs={"verbose": False})
t.start()
self.prediction_count = 0
else:
self.prediction_count += 1
self.signal_prediction_show.emit()
# @log_decorator
def prediction_show(self):
'''
show prediction, and emit signal with prediction to controller
:return:
'''
# draw probability plot
plot = models.update()
cmd = None
# write result
if len(global_vars.jsonarray) > 0:
prediction = max(global_vars.jsonarray,
key=lambda x: global_vars.jsonarray[x])
if global_vars.jsonarray[prediction] * 100 > self.threshold:
self.label_predict.setText(GESTURE_CLASSES[int(prediction)])
cmd = GESTURE_CLASSES[int(prediction)]
else:
self.label_predict.setText('nothing')
showImage = QtGui.QImage(plot.data, plot.shape[1], plot.shape[0],
QtGui.QImage.Format_RGB888)
self.label_probability.setPixmap(QtGui.QPixmap.fromImage(showImage))
# emit sigal
if cmd is not None and self.controller.status == 'online':
self.signal_command_send.emit(cmd)
@log_decorator
def keyPressEvent(self, event):
'''
rewrite keyboard event for car controller debug.
:param event:
:return:
'''
if self.controller is not None and self.controller.status == 'online':
if event.key() == Qt.Key_W:
# send move forward signal
self.signal_command_send.emit("go_ahead")
elif event.key() == Qt.Key_A:
# send turn left signal
self.signal_command_send.emit("turn_left")
elif event.key() == Qt.Key_D:
# send turn right signal
self.signal_command_send.emit("turn_right")
elif event.key() == Qt.Key_S:
# send back up signal:
self.signal_command_send.emit("back_up")
else:
print("Not connect to ESP8266 yet.")
if event.key() == Qt.Key_I:
# try to find ESP 8266 's IP
self.host_detector.detect_hosts()
elif event.key() == Qt.Key_P:
# print current detected IPs
print(self.host_detector.hosts)
# SLOT: update value
def box_left(self):
self.camera_x0 -= 5
def box_right(self):
self.camera_x0 += 5
def box_up(self):
self.camera_y0 -= 5
def box_down(self):
self.camera_y0 += 5
def threshold_update(self):
self.threshold = self.horizontalSlider.value()
def command_send(self, prediction):
self.controller.move(prediction)
from threading import Thread
from car_controller import CarController
import api
if __name__ == '__main__':
try:
controller = CarController()
thread: Thread = Thread(target=lambda: api.init(controller))
thread.setDaemon(True)
thread.start()
controller.follow_waypoints()
except InterruptedError as e:
# Shutdown on ctrl+c or window closed
print('Shutting down as per user request...')
exit(0)