diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..71b07f0151d15e5c8a07ed31816ccc98bcad65fa
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,5 @@
+# folder with images
+/Bilder/*
+
+# Python cache-files
+*.pyc
\ No newline at end of file
diff --git a/Klassendiagramm.png b/Klassendiagramm.png
new file mode 100644
index 0000000000000000000000000000000000000000..3dcc27a5c2cf26f14d8b891ae4180a41a76fbda6
Binary files /dev/null and b/Klassendiagramm.png differ
diff --git a/Lokalisierung_LEGO/.vscode/extensions.json b/Lokalisierung_LEGO/.vscode/extensions.json
new file mode 100644
index 0000000000000000000000000000000000000000..f8f1a4401370ad4181562d98b9cbf1795831c631
--- /dev/null
+++ b/Lokalisierung_LEGO/.vscode/extensions.json
@@ -0,0 +1,13 @@
+{
+ // See http://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
+ // Extension identifier format: ${publisher}.${name}. Example: vscode.csharp
+
+ // List of extensions which should be recommended for users of this workspace.
+ "recommendations": [
+ "lego-education.ev3-micropython"
+ ],
+ // List of extensions recommended by VS Code that should not be recommended for users of this workspace.
+ "unwantedRecommendations": [
+ "ms-python.python"
+ ]
+}
\ No newline at end of file
diff --git a/Lokalisierung_LEGO/.vscode/launch.json b/Lokalisierung_LEGO/.vscode/launch.json
new file mode 100644
index 0000000000000000000000000000000000000000..d933aeb1fa42c27e31946454216860b6d7e83f9d
--- /dev/null
+++ b/Lokalisierung_LEGO/.vscode/launch.json
@@ -0,0 +1,15 @@
+{
+ // Use IntelliSense to learn about possible attributes.
+ // Hover to view descriptions of existing attributes.
+ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+ "version": "0.2.0",
+ "configurations": [
+ {
+ "name": "Download and Run",
+ "type": "ev3devBrowser",
+ "request": "launch",
+ "program": "/home/robot/${workspaceRootFolderName}/main.py",
+ "interactiveTerminal": false
+ }
+ ]
+}
diff --git a/Lokalisierung_LEGO/.vscode/settings.json b/Lokalisierung_LEGO/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..343aaf987bc5720f3b0327c5ea28d21f56cd6d94
--- /dev/null
+++ b/Lokalisierung_LEGO/.vscode/settings.json
@@ -0,0 +1,7 @@
+// Place your settings in this file to overwrite default and user settings.
+{
+ "files.eol": "\n",
+ "debug.openDebug": "openOnSessionStart",
+ "python.linting.enabled": false,
+ "python.languageServer": "None"
+}
diff --git a/Lokalisierung_LEGO/main.py b/Lokalisierung_LEGO/main.py
new file mode 100644
index 0000000000000000000000000000000000000000..5a730ee9e5193737e3d4c662b7d553e3ea21d1e9
--- /dev/null
+++ b/Lokalisierung_LEGO/main.py
@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Tue May 9 12:16:54 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+
+Hi I'm Sir Ludwig. I'm a self driving robot.
+My job is it to follow a white line.
+I love my job!
+"""
+
+#%% imports
+import socket
+from datetime import datetime
+from ev3dev2.port import LegoPort
+from ev3dev2.sensor import INPUT_4, INPUT_1
+from ev3dev2.sensor.lego import ColorSensor
+from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent
+from ev3dev2.sensor.lego import GyroSensor
+#import sys
+
+STUD_MM = 8
+use_gyro = 0 # 0 = ohne Gyrosensor, 1 = mit Gyrosensor
+
+#HOST = "10.128.41.108"
+#HOST = "10.128.41.90"
+#HOST = "192.168.178.81"
+HOST = "192.168.0.100"
+PORT = 50007
+
+#%% main programm
+
+print("Hello! My name is Sir Ludwig and I love to follow the line <3")
+
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+s.connect((HOST,PORT))
+
+tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
+max_speed = 170 / 60 # rps
+d = 120 # in mm
+radumfang = (3.141592 * 54) # in mm
+white_value = 54
+black_value = 13
+desired_light = (white_value + black_value) / 2
+t = 4.5 # in ms
+
+cs = ColorSensor(INPUT_4)
+
+gs = GyroSensor(INPUT_1)
+gs.mode = 'GYRO-RATE' # Rotationsgeschwindigkeit
+
+fehler = 0
+fehler_alt = 0
+s_r = 0
+s_r_alt = 0
+s_l = 0
+s_l_alt = 0
+
+counter_links = 0
+counter_rechts = 0
+counter_geradeaus = 0
+counter = 0
+w = 0
+v = 0
+
+time_last_send = datetime.utcnow()
+
+while (1):
+ sensorwert = cs.value()
+ fehler_alt = fehler
+ fehler = desired_light - sensorwert
+
+ ableitung = (fehler - fehler_alt) / t
+
+ turning = (0.4 * fehler) + (0.08 * ableitung)
+
+ right_motor = 30 - turning
+ left_motor = 30 + turning
+
+ tank_drive.on(SpeedPercent(left_motor), SpeedPercent(right_motor))
+
+ right_speed = tank_drive.right_motor.speed / 360
+ left_speed = tank_drive.left_motor.speed / 360
+
+ v_r = right_speed * radumfang # in mm / s
+ v_l = left_speed * radumfang # in mm / s
+
+ v += (v_r + v_l) / 2
+
+ if (use_gyro == 0):
+ w += (v_r - v_l) / (2*d)
+ else:
+ w += (gs.value() / 180) * -3.141592
+
+ counter += 1
+ if(counter == 6):
+ w /= counter
+ v /= counter
+ v_str = str(format((round(v, 5)), '.5f')).zfill(9)
+ w_str = format((round(w, 5)), '.5f')
+ if (w >= 0):
+ w_str = "+" + w_str
+ time_delta = datetime.utcnow() - time_last_send
+ t_str = format((round(time_delta.total_seconds(), 6)), '.6f')
+ stringSend = "w=" + w_str + "v=" + v_str +"t=" + t_str
+ s.sendall(stringSend.encode())
+ time_last_send = datetime.utcnow()
+ counter = 0
+ w = 0
+ v = 0
\ No newline at end of file
diff --git a/README.md b/README.md
index 6ac5304aac302d7346fa0d75ff382cb7631d55ff..329a07f97b005f10f7fd59619938d209a17cd43a 100644
--- a/README.md
+++ b/README.md
@@ -1,12 +1,16 @@
-# localization VROB
+# Toolkit for locating a vehicle on the race track
This project deals with the localization of a vehicle on a race track.
This is a student project from Bochum University of Applied Sciences.
-## Name
-localization of a vehicle on a race track
+The main approach of localization is based on locating the vehicle based on the current and already travelled section types.
+A section type can be a right turn, a left turn and straight.
## Robot
-The used Robot is the LEGO EV3 Brick.
+The used Robot is the LEGO EV3 Brick as a line follower to test the algorithm.
+
+The robot use MicroPython.
+How to install MicroPython and set up your Visual Studio Code to connect your pc to the robot is documented in this file:
+https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb470b9ea6e38f8d4/5f8802fc4376310c19e33714/getting-started-with-micropython-v2_enus.pdf?locale=de-de
Important notes regarding the robot:
* The robot has differential drive.
@@ -16,7 +20,7 @@ Important notes regarding the robot:
* GyroSensor: INPUT 1
## Getting started
-How to run MicroPython programs on LEGO EV3:
+### How to run MicroPython programs on LEGO EV3:
1. Download and flash the EV3 MicroPython image onto a micro SD card
2. Insert the micro SD card into the SD card slot on the EV3 Brick
3. Download, install, and launch Visual Studio Code on your computer
@@ -25,7 +29,7 @@ How to run MicroPython programs on LEGO EV3:
* After installing the extension, a device can be connected in Visual Studio Code (at the bottom, left).
* The LEGO EV3 Brick should be listed and can be selected in the list.
-The following Python-packages must be installed on the computer to run the algorithm:
+### The following Python-packages must be installed on the computer to run the algorithm:
* numpy
* pandas
* matplotlib
@@ -33,6 +37,28 @@ The following Python-packages must be installed on the computer to run the algor
* socket
* datetime
+### Preparations before run
+Clone this git. Connect the EV3 robot to the same network as your executing device.
+
+The following adjustments might be necessary, before the algorithm is executable:
+* change ip-address in Lokalisierung_LEGO/main.py to the ip-address of the executing device
+* insert the sections of the race track in localization.py (get_set_of_section_*())
+ * structure of a section: [name, section_type, previous_sections_type]
+* if the track plotter is used, insert also a set of track parts in the file grafic (get_set_of_trackparts_*()).
+ * structure of a section: [URL, name, start position(x,y), start orientation)]
+ * Images of the race tracks are not included in this git. If interested, please contact the authors.
+
+### Run
+1. Connect the robot via Visual Studio Code.
+2. Start the python file race_management.py
+ * Now some windows should pop up.
+ * You can't move this windows until the UDP-connection with the robot is connected.
+ * __When you want to stop the script you have to restart the python kernel.__
+3. Start the main.py file on the robot.
+ * To do this, select the file in the Lego-EV window in the lower left corner of Visual Studio Code. Then right click and click on "Run"
+4. Now the robot should drive and send data to the pc which plots in the console the current detected section and plot stuff.
+5. To stop the robot/program press the left upper button on the robot, or close the dashboard window.
+
## Description
The main branch contains the following files:
* localization.py: localization algorithm
@@ -41,10 +67,6 @@ The main branch contains the following files:
* race_management.py: main program
* Lokalisierung_LEGO (folder), main.py: programming of the robot LEGO EV3
-The following adjustments might be necessary, before the algorithm is executable:
-* change ip-address in main.py
-* entering the route in localization.py (get_set_of_section_*())
-
The following measured values are stored and must be adjusted if necessary (in main.py):
* wheel circumference (radumfang)
* wheel spacing (d)
@@ -59,12 +81,16 @@ Notes on constant values (in race_management.py):
* S = 0, R = 1, L = 2: value for a straight part, a right turn, a left turn of the track
**These values should not be changed in order to maintain the functionality of the algorithm.**
+## Code overview
+
+
## Authors and acknowledgment
Jessica Dreyer, Sebastian Böttger
## License
-Project is licensed with MIT.
-See at LICENSE
+This project is licensed under the MIT License - see the LICENSE file for details.
## Project status
-Work in progress.
+Project closed.
+
+Cordial invitation to further develop this project.
\ No newline at end of file
diff --git a/driving.py b/driving.py
new file mode 100644
index 0000000000000000000000000000000000000000..9d952dc92fa70edcd7ee6528f48fb4c1904a2618
--- /dev/null
+++ b/driving.py
@@ -0,0 +1,141 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Tue May 9 12:16:54 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+"""
+#%% Imports
+import numpy as np
+
+#%% Constans
+S = 0 # Value for a straight part of the track
+R = 1 # Value for a right turn
+L = 2 # Value for a left turn
+#%% Classes
+"""
+# This class contains some functions to analyze data from the LEGO robot EV3.
+# It detects the section type depending on the rotationspeed of the robot.
+# It calculates the odometrie for x-position, y-position and direction of the robot.
+# It calculates the driven distance on a section.
+"""
+class Driving_Analyser:
+
+ """
+ # Constructor of the driving analyser.
+ # Needs threshold values of rotationspeed for left and right turn
+ # and a block size which represents the number of iterations to analyze the section type
+ """
+ def __init__(self, threshold_left, threshold_right, block_size):
+ self.threshold_left = threshold_left
+ self.threshold_right = threshold_right
+ self.block_size = block_size
+ self.counter_left = 0;
+ self.counter_right = 0;
+ self.counter_straight = 0;
+ self.x_old = 0
+ self.y_old = 0
+ self.theta_old = 0
+ self.driven_distance_on_section = 0
+
+ """
+ # This function detects the section type.
+ # For the detection the given threshold for left and right turns
+ # and the block size to sum up the values are used.
+ # The function decides on the basis of the threshold values
+ # whether the current rotation speed is a left or a right turn
+ # and sums up these types in the amount of the block size to make the decision for the section type.
+ # If the function is called and the sum of values is lower than the block size it returns None
+ # If the detection of the section is completed, the function will return an integer value.
+ # 0 # Value for a straight part of the track
+ # 1 # Value for a right turn
+ # 2 # Value for a left turn
+ # The entire return statemant is a tuple with the section type, the counte for left and right turns and a counter for straight
+ # return:
+ # section_type, counter_left, counter_right, counter_straight
+ """
+ def detect_section_type(self, w):
+ section_type = None
+ if (w >= self.threshold_left): # left turn
+ self.counter_left += 1
+ elif (w <= self.threshold_right): # right turn
+ self.counter_right += 1
+ else: # straight
+ self.counter_straight += 1
+ if ((self.counter_left + self.counter_right + self.counter_straight) == self.block_size):
+ if (self.counter_left > self.counter_right and self.counter_left > self.counter_straight):
+ section_type = L
+ elif (self.counter_right > self.counter_left and self.counter_right > self.counter_straight):
+ section_type = R
+ else:
+ section_type = S
+ self.counter_left = self.counter_right = self.counter_straight = 0
+ #print(self.__get_section_type(section_type))
+ return (section_type, self.counter_left, self.counter_right, self.counter_straight)
+
+ """
+ # This function calcuates the continues odometrie which adds the new positon to the old one.
+ # The calculation of the position depends on the speed, rotationspeed and the time span.
+ """
+ def calculate_odometrie_continues (self, w, v, time_delta):
+
+ if w != 0: #or v != 0:
+ theta = self.theta_old + (w * time_delta)
+ x = self.x_old + (v * ((np.sin(theta) - np.sin(self.theta_old)) / w))
+ y = self.y_old + (v * (-(np.cos(theta) - np.cos(self.theta_old)) / w))
+
+ else:
+ theta = self.theta_old
+ x = self.x_old + (v * np.cos(theta) * time_delta)
+ y = self.y_old + (v * np.sin(theta) * time_delta)
+
+
+ self.theta_old = theta
+ self.y_old = round(y, 5)
+ self.x_old = round(x, 5)
+
+ return (self.x_old, self.y_old, self.theta_old)
+
+ """
+ # This function calcuates the delta odometrie which returns only the delta position of the given time span.
+ # The calculation of the position depends on the speed, rotationspeed and the time span.
+ """
+ def calculate_odometrie_delta (self, w, v, time_delta):
+
+ if w != 0:
+ theta_delta = (w * time_delta)
+ x_delta = (v * (np.sin(theta_delta) / w))
+ y_delta = (v * (-(np.cos(theta_delta) - np.cos(0)) / w))
+
+ else:
+ theta_delta = 0
+ x_delta = v * (np.cos(0))
+ y_delta = v * (np.sin(0))
+
+ return (x_delta, y_delta, theta_delta)
+
+ """
+ # This function returns the drivien distance on the section.
+ """
+ def calculate_driven_distance_on_section (self, v,time_delta):
+ self.driven_distance_on_section += v*time_delta
+ return self.driven_distance_on_section
+
+ """
+ # This function resets the drivien distance.
+ """
+ def reset_driven_distance_on_section (self):
+ self.driven_distance_on_section = 0
+
+ """
+ # This function is for internal use and printing only.
+ # It converts the integer values for the section type to the correct discription of the section type.
+ """
+ def __get_section_type(self, section_type_int):
+ section_type_string = ""
+ if section_type_int == S:
+ section_type_string = "straight"
+ elif section_type_int == R:
+ section_type_string = "right"
+ elif section_type_int == L:
+ section_type_string = "left"
+ return section_type_string
\ No newline at end of file
diff --git a/grafic.py b/grafic.py
new file mode 100644
index 0000000000000000000000000000000000000000..6ab2c7849d7d3f2b1287acfb3fe1503907325191
--- /dev/null
+++ b/grafic.py
@@ -0,0 +1,449 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Sat May 6 13:26:28 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+"""
+#%% imports
+import numpy as np
+import pandas as pd
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from matplotlib.patches import Circle
+import matplotlib.image as img
+import tkinter as tk
+
+# not importend imports
+import time
+import random
+
+
+#%% class
+"""
+# This class can plot values as a consistant graph.
+# With the method update you can insert a new value to the graph and the grafic will refresh automaticly.
+"""
+class Line_Plotter:
+ """
+ # The constructor needs a title, for the diagramm as well as a lable for the x and y axses.
+ # Optional you can set the number of values that are plottet on the x-axses.
+ """
+ def __init__(self, title, x_lable, y_lable ,x_scale = 100):
+ self.counter = 0
+ self.fig = plt.figure()
+ self.x = np.array(range(-x_scale + 1,1))
+ self.y = np.zeros(x_scale)
+ self.ax = self.fig.add_subplot(111)
+ self.line, = self.ax.plot(self.x, self.y)
+ self.ax.set_xlim(self.x[0],self.x[self.x.size -1])
+ self.ax.set_ylim(0, 10)
+ self.ax.set_title(title)
+ self.ax.set_xlabel(x_lable)
+ self.ax.set_ylabel(y_lable)
+ self.fig.canvas.draw()
+ self.fig.canvas.flush_events()
+
+ """
+ # The method update insert a new value to the graph an refresh the graph automaticly.
+ # It needs only the value as a parameter.
+ """
+ def update(self, value):
+ self.y = np.append(self.y, value)
+ self.y = self.y[1:]
+ self.x = np.append(self.x, self.counter)
+ self.x = self.x[1:]
+
+ self.line._x = self.x
+ self.line.set_ydata(self.y)
+ self.fig.canvas.draw()
+ self.fig.canvas.flush_events()
+
+ self.ax.set_xlim(self.x[0],self.x[self.x.size -1])
+ self.ax.set_ylim(self.y.min(), self.y.max())
+
+ self.counter = self.counter + 1
+
+ """
+ # The method close ensures that the window is closed.
+ """
+ def close(self):
+ plt.close(self.fig)
+
+"""
+# This class can plot values as a continues graph.
+# It can plot the values with a given timedelta to the last known value.
+"""
+class Line_Plotter_Time:
+ """
+ # The constructor needs a title, for the diagramm as well as a lable for the x and y axses.
+ # Optional you can set the number of values that are plottet on the x-axses.
+ """
+ def __init__(self, title, x_lable, y_lable ,x_scale = 100):
+ self.x_scale = x_scale
+ self.fig = plt.figure()
+ self.x = np.array(range(0,1))
+ self.y = np.zeros(1)
+ self.ax = self.fig.add_subplot(111)
+ self.line, = self.ax.plot(self.x, self.y)
+ self.ax.set_xlim(0,10)
+ self.ax.set_ylim(0, 10)
+ self.ax.set_title(title)
+ self.ax.set_xlabel(x_lable)
+ self.ax.set_ylabel(y_lable)
+
+ """
+ # The method update insert a new value to the graph on the timestamp delte which is given an refresh the graph automaticly.
+ # It needs the value and a given time delta to the last value as a parameter.
+ # The timedelta will be summed to the hight time of a datapoint in the digramm and will plot at this time the new value.
+ """
+ def update(self, value, time_delta):
+ self.y = np.append(self.y, value)
+ self.x = np.append(self.x, self.x[self.x.size -1] + time_delta)
+ if (self.x.size > self.x_scale):
+ self.y = self.y[1:]
+ self.x = self.x[1:]
+
+ self.line._x = self.x
+ self.line.set_ydata(self.y)
+ self.fig.canvas.draw()
+ self.fig.canvas.flush_events()
+
+ self.ax.set_xlim(self.x[0],self.x[self.x.size -1])
+ self.ax.set_ylim(self.y.min(), self.y.max())
+
+ """
+ # The method close ensures that the window is closed.
+ """
+ def close(self):
+ plt.close(self.fig)
+
+"""
+# This class can plot the racetrack and highlighte the current section.
+# It should also display a little robot an update the position, but unfortunately, it's not function properly.
+"""
+class Track_Plotter:
+ """
+ # This constructor needs a set of trackparts which contains a URL to the backgroundimage for the plot.
+ """
+ def __init__(self, background_url, track, ratio = 1):
+ self.background = img.imread(background_url)
+ self.track = track
+ self.ratio = ratio
+ self.fig = plt.figure()
+ self.ax = self.fig.add_subplot(111)
+ self.live_image = self.ax.imshow(self.background, extent=[0,1000,750,0])
+ self.robot_is_active = False
+ self.robot_position = self.track.iloc[0].start_position
+ self.robot_direction = self.track.iloc[0].start_direction
+ self.ax.set_xlim(0,1000)
+ self.ax.set_ylim(750,0)
+ plt.show(block=False)
+ plt.pause(0.1)
+
+ """
+ # Wich this funtion you get a set of trackparts. The URLs are not includet in this git.
+ """
+ def get_set_of_trackparts_3():
+ # Track plotting
+ trackparts = []
+ trackparts.append(['./Bilder/Strecke_03_S1.PNG', 'S1', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S2.PNG', 'S2', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S3.PNG', 'S3', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S4.PNG', 'S4', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S5.PNG', 'S5', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S6.PNG', 'S6', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S7.PNG', 'S7', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S8.PNG', 'S8', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S9.PNG', 'S9', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S10.PNG', 'S10', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S11.PNG', 'S11', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S12.PNG', 'S12', (0,0), 0])
+ trackparts.append(['./Bilder/Strecke_03_S13.PNG', 'S13', (0,0), 0])
+
+ # Create DataFrame
+ track = pd.DataFrame(trackparts,columns=['image_url', 'name', 'start_position', 'start_direction'])
+ background = './Bilder/Strecke_03.png'
+
+ return (background, track)
+
+ """
+ # This method is internal used to create the robot.
+ """
+ def __create_triangle(self, position, direction):
+ # Definiere die Position der Ecke des Dreiecks
+ x = position[0]
+ y = position[1]
+
+ # Definiere die Länge der Kante, die in Richtung des Winkels theta zeigt
+ length = 100
+
+ # Definiere den Winkel theta in Grad
+ theta_deg = direction
+
+ # Berechne den Winkel theta in Bogenmaß
+ theta = np.deg2rad(theta_deg)
+
+ # Berechne die Koordinaten des Dreiecks
+ x1 = x
+ y1 = y
+
+ x2 = x + length * np.cos(theta -np.deg2rad(135))
+ y2 = y + length * np.sin(theta -np.deg2rad(135))
+ x3 = x + length * np.cos(theta -np.deg2rad(135) + np.pi/2)
+ y3 = y + length * np.sin(theta -np.deg2rad(135) + np.pi/2)
+
+
+ # Erstelle das Dreieck
+ self.__triangle = patches.Polygon([(x1, y1), (x2, y2), (x3, y3)], closed=True, fill=True, facecolor='green', alpha=0.5)
+ return self.__triangle
+
+ """
+ # This function is used to select a section and set up that backgorundimage
+ """
+ def select_trackpart(self, trackpart_name):
+ trackpart = self.track[self.track.name == trackpart_name]
+ image = img.imread(trackpart.iloc[0].image_url)
+ self.live_image.set_data(image)
+ self.fig.canvas.draw()
+ self.fig.canvas.flush_events()
+
+ """
+ # This funktion is used to set the robot to the startposition of a trackpart.
+ # Currently the robot simulation is not usable!
+ """
+ def set_robot_to_trackpart(self, trackpart_name):
+ trackpart = self.track[self.track.name == trackpart_name]
+ self.robot_position = trackpart.iloc[0].start_position
+ self.robot_direction = trackpart.iloc[0].start_direction
+ self.update_robot()
+
+ """
+ # This funktion is used to move the robot forward.
+ # Currently the robot simulation is not usable!
+ """
+ def move_robot_forward(self, distance):
+ x,y = self.robot_position
+ theta = np.deg2rad(self.robot_direction)
+ x_new = x + distance*self.ratio * np.sin(theta)
+ y_new = y - distance*self.ratio * np.cos(theta)
+ self.robot_position = (x_new, y_new)
+ self.update_robot()
+
+ """
+ # This funktion is used to set the robot to a specific position on the plot.
+ # Currently the robot simulation is not usable!
+ """
+ def move_robot_to_position(self, position):
+ self.robot_position += position
+ self.update_robot()
+
+ """
+ # This funktion is used to rotate the robot.
+ # Currently the robot simulation is not usable!
+ """
+ def rotate_robot(self, rotation):
+ self.robot_direction += rotation
+ self.update_robot()
+
+ """
+ # This funktion enable the plotting of the robot.
+ # Currently the robot simulation is not usable!
+ """
+ def activate_robot(self):
+ print("Do not use this method!!! - activate_robot")
+ self.robot_is_active = True
+ self.__circle = Circle(self.robot_position, 10, facecolor='green', edgecolor='green')
+ self.__circle = self.ax.add_artist(self.__circle)
+ self.__triangle = self.__create_triangle(self.robot_position, self.robot_direction)
+ self.__triangle = self.ax.add_artist(self.__triangle)
+ self.fig.canvas.draw_idle()
+ self.fig.canvas.flush_events()
+
+ """
+ # This funktion is used to update the position and orientation of the robot on the plot.
+ # Currently the robot simulation is not usable!
+ """
+ def update_robot(self):
+ print("Do not use this method!!! - update_robot")
+ if self.robot_is_active:
+ try:
+ self.__circle.remove()
+ self.__triangle.remove()
+ except NotImplementedError:
+ pass
+ self.__triangle = self.triangle = self.__create_triangle(self.robot_position, self.robot_direction)
+ self.__triangle = self.ax.add_artist(self.__triangle)
+ self.__circle = Circle(self.robot_position, 15, facecolor='green', edgecolor='green')
+ self.__circle = self.ax.add_artist(self.__circle)
+ self.fig.canvas.draw_idle()
+ self.fig.canvas.flush_events()
+
+ # self.ax.draw_artist(self.__triangle)
+ # self.ax.draw_artist(self.__circle)
+ # self.fig.canvas.blit(self.fig.bbox)
+ # self.ax.figure.canvas.flush_events()
+
+ # try:
+ # self.ax.draw_artist(self.__triangle)
+ # self.ax.draw_artist(self.__circle)
+
+ # except AttributeError:
+ # pass
+ # self.__circle.set_center(self.robot_position)
+ # self.ax.draw_artist(self.__circle)
+ # self.fig.canvas.blit(self.fig.bbox)
+ # self.fig.canvas.flush_events()
+
+ """
+ # This funktion close the window.
+ """
+ def close(self):
+ plt.close(self.fig)
+
+"""
+# This class creates a little Dashborad with the most important information from the robot and the localization.
+"""
+class Dashboard:
+ """
+ # This constructor needs nothing and set up all values for the dashboard.
+ """
+ def __init__(self):
+ self.current_section_type = ""
+ self.last_sections_type = ""
+ self.enter_new_section = ""
+ self.last_known_section = ""
+ self.count_fail_loc = 0
+ self.x_position = 0
+ self.y_position = 0
+ self.driven_distance_on_section = 0
+
+ self.window = tk.Tk()
+ self.window.title("Dashboard")
+
+ self.label_current_section_type = tk.Label(self.window, text="current section type:")
+ self.label_last_sections_type = tk.Label(self.window, text="last sections type:")
+ self.label_enter_new_section = tk.Label(self.window, text="enter new section:")
+ self.label_last_known_section = tk.Label(self.window, text="last known section:")
+ self.label_count_fail_loc = tk.Label(self.window, text="count fail loc:")
+ self.label_x_position = tk.Label(self.window, text="x position:")
+ self.label_y_position = tk.Label(self.window, text="y position:")
+ self.label_driven_distance_on_section = tk.Label(self.window, text="driven distance on section:")
+
+ self.label_current_section_type.pack()
+ self.label_last_sections_type.pack()
+ self.label_enter_new_section.pack()
+ self.label_last_known_section.pack()
+ self.label_count_fail_loc.pack()
+ self.label_x_position.pack()
+ self.label_y_position.pack()
+ self.label_driven_distance_on_section.pack()
+
+ self.window.protocol("WM_DELETE_WINDOW", print(""))
+ self.window.update_idletasks()
+ self.window.update()
+
+ """
+ # This function sets the current section type to the dashboard
+ """
+ def set_current_section_type(self, section_type):
+ self.current_section_type = section_type
+ self.label_current_section_type.config(text="current section type: " + self.current_section_type)
+ self.update_window()
+
+ """
+ # This function sets the last sections type to the dashboard
+ """
+ def set_last_sections_type(self, last_sections_type):
+ self.last_sections_type = last_sections_type
+ self.label_last_sections_type.config(text="last sections type: " + self.last_sections_type)
+ self.update_window()
+
+ """
+ # This function sets the value enter new section to the dashboard
+ """
+ def set_enter_new_section(self, enter_new_section):
+ self.enter_new_section = enter_new_section
+ self.label_enter_new_section.config(text="enter new section: " + self.enter_new_section)
+ self.update_window()
+
+ """
+ # This function sets the last known section to the dashboard
+ """
+ def set_last_known_section(self, last_known_section):
+ self.last_known_section = last_known_section
+ self.label_last_known_section.config(text="last known section: " + self.last_known_section)
+ self.update_window()
+
+ """
+ # This function sets the counter fail localization to the dashboard
+ """
+ def set_count_fail_loc(self, count_fail_loc):
+ self.count_fail_loc = count_fail_loc
+ self.label_count_fail_loc.config(text="count fail loc: " + str(self.count_fail_loc))
+ self.update_window()
+
+ """
+ # This function sets the x position of the robot to the dashboard
+ """
+ def set_x_position(self, x_position):
+ self.x_position = x_position/1000
+ self.label_x_position.config(text="x position: " + str(self.x_position) + " m")
+ self.update_window()
+
+ """
+ # This function sets the y position of the robot to the dashboard
+ """
+ def set_y_position(self, y_position):
+ self.y_position = y_position/1000
+ self.label_y_position.config(text="y position: " + str(self.y_position) + " m")
+ self.update_window()
+
+ """
+ # This function sets the driven distance on the section to the dashboard
+ """
+ def set_driven_distance_on_section(self, driven_distance_on_section):
+ self.driven_distance_on_section = driven_distance_on_section/1000
+ self.label_driven_distance_on_section.config(text="driven distance on section: " + str(self.driven_distance_on_section) + " m")
+ self.update_window()
+
+ """
+ # This function update the dashboard it's self. without this method, the dashboard will not refresh.
+ """
+ def update_window(self):
+ self.window.update_idletasks()
+ self.window.update()
+
+#%% Main Skript
+
+if __name__ == '__main__':
+ # Line plotter without timing
+ #ln_plotter_1 = Line_Plotter("Linieargeschwindigkeit", "Zeitschritte", "mm/s")
+ #ln_plotter_2 = Line_Plotter("Winkelgeschwindigkeit", "Zeitschritte", "rad/s")
+ #for i in range(200):
+ #ln_plotter_1.update(random.random() * 5)
+ #ln_plotter_2.update(random.random() * 5)
+ #time.sleep(0.05)
+
+ # Line plotter with timing
+ # ln_plotter_1 = Line_Plotter_Time("Linieargeschwindigkeit", "s", "mm/s")
+ # ln_plotter_2 = Line_Plotter_Time("Winkelgeschwindigkeit", "s", "rad/s")
+ # for i in range(200):
+ # ln_plotter_1.update(random.random() * 5, 0.05)
+ # ln_plotter_2.update(random.random() * 5, 0.05)
+ # #time.sleep(0.05)
+
+ # Track plotting
+ background, track = Track_Plotter.get_set_of_trackparts_3()
+
+ tr_plotter = Track_Plotter(background, track)
+ #tr_plotter.activate_robot()
+ #tr_plotter.update_robot()
+ #tr_plotter.set_robot_to_trackpart('S1')
+ for i in range(3):
+ if i == 0:
+ tr_plotter.select_trackpart('S1')
+ if i == 1:
+ tr_plotter.select_trackpart('S5')
+ if i == 2:
+ tr_plotter.select_trackpart('S9')
+ time.sleep(3)
\ No newline at end of file
diff --git a/localization.py b/localization.py
new file mode 100644
index 0000000000000000000000000000000000000000..c02cce718a24b8ffa0e94a30383c5efa3c2c6e24
--- /dev/null
+++ b/localization.py
@@ -0,0 +1,236 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Mon May 1 14:48:45 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+"""
+#%% imports
+import numpy as np
+import pandas as pd
+
+#%% constants
+S = 0 # Value for a straight part of the track
+R = 1 # Value for a right turn
+L = 2 # Value for a left turn
+
+
+#%% Classes
+"""
+# This class provides methods to localize a robot on a given race track.
+# The race track is given during the creation of the object and contains the following structure:
+ [name of the section, section type, array of the last section types]
+ The name of the section is a string and can be choosen individually.
+ The sectiontyp is an int and can be one of the following states:
+ 0 = straight; 1 = right turn; 2 = left turn
+ These three values are saved in the const-variables S, R and L.
+ The array of the last section types is a simple array which contains a set of ints.
+ Each int stands for one of the three states S, R or L.
+"""
+class Localizer:
+
+ """
+ # The constructor gets a set of sections and a boolean value for debugging.
+ """
+ def __init__(self, sections, debug = False):
+ self.sections = sections
+ self.current_section_type = None
+ self.last_sections_type = [None, None, None, None, None]
+ self.enter_new_section = False
+ self.last_known_section = None
+ self.count_fail_loc = 0
+ self.debug = debug
+
+ """
+ # This function returns a set of sections
+ """
+ def get_set_of_section_1():
+ track = []
+ track.append(["S1", S, [L,S,L,S,L]])
+ track.append(["S2", L, [S,L,S,L,S]])
+ track.append(["S3", S, [L,S,L,S,L]])
+ track.append(["S4", L, [S,L,S,L,S]])
+ track.append(["S5", S, [L,S,L,S,L]])
+ track.append(["S6", R, [S,L,S,L,S]])
+ track.append(["S7", S, [R,S,L,S,L]])
+ track.append(["S8", L, [S,R,S,L,S]])
+ track.append(["S9", S, [L,S,R,S,L]])
+ track.append(["S10", R, [S,L,S,R,S]])
+ track.append(["S11", L, [R,S,L,S,R]])
+ track.append(["S12", S, [L,R,S,L,S]])
+ track.append(["S13", L, [S,L,R,S,L]])
+ track.append(["S14", S, [L,S,L,R,S]])
+ track.append(["S15", L, [S,L,S,L,R]])
+
+ # DataFrame erstellen
+ track = pd.DataFrame(track, columns=['s_name', 'type', 'previous'])
+ return track
+
+ """
+ # This function returns a set of sections
+ """
+ def get_set_of_section_2():
+ track = []
+ track.append(["S1", S, [L,S,L,S,L]])
+ track.append(["S2", L, [S,L,S,L,S]])
+ track.append(["S3", S, [L,S,L,S,L]])
+ track.append(["S4", L, [S,L,S,L,S]])
+ track.append(["S5", S, [L,S,L,S,L]])
+ track.append(["S6", R, [S,L,S,L,S]])
+ track.append(["S7", S, [R,S,L,S,L]])
+ track.append(["S8", L, [S,R,S,L,S]])
+ track.append(["S9", S, [L,S,R,S,L]])
+ track.append(["S10", R, [S,L,S,R,S]])
+ track.append(["S11", S, [R,S,L,S,R]])
+ track.append(["S12", L, [S,R,S,L,S]])
+ track.append(["S13", S, [L,S,R,S,L]])
+ track.append(["S14", L, [S,L,S,R,S]])
+ track.append(["S15", S, [L,S,L,S,R]])
+ track.append(["S16", L, [S,L,S,L,S]])
+
+ # DataFrame erstellen
+ track = pd.DataFrame(track, columns=['s_name', 'type', 'previous'])
+ return track
+
+ """
+ # This function returns a set of sections
+ """
+ def get_set_of_section_3():
+ track = []
+ track.append(["S1", S, [L,S,L,R,S]])
+ track.append(["S2", L, [S,L,S,L,R]])
+ track.append(["S3", S, [L,S,L,S,L]])
+ track.append(["S4", L, [S,L,S,L,S]])
+ track.append(["S5", S, [L,S,L,S,L]])
+ track.append(["S6", R, [S,L,S,L,S]])
+ track.append(["S7", S, [R,S,L,S,L]])
+ track.append(["S8", L, [S,R,S,L,S]])
+ track.append(["S9", S, [L,S,R,S,L]])
+ track.append(["S10", R, [S,L,S,R,S]])
+ track.append(["S11", L, [R,S,L,S,R]])
+ track.append(["S12", S, [L,R,S,L,S]])
+ track.append(["S13", L, [S,L,R,S,L]])
+
+ # DataFrame erstellen
+ track = pd.DataFrame(track, columns=['s_name', 'type', 'previous'])
+ return track
+
+ """
+ # This function updates the object values depending on the section type which is provided.
+ If the section type is the same as before nothing changes.
+ If the section type has changed, all object variables are updated and the variable "enter_new_section" is set to true.
+ """
+ def update_position(self, section_type):
+ if self.current_section_type is not section_type:
+ self.enter_new_section = True
+ self.last_sections_type = self.last_sections_type[:4]
+ self.last_sections_type.insert(0, self.current_section_type)
+ self.current_section_type = section_type
+ else:
+ self.enter_new_section = False
+
+ """
+ # This function calculates the section where the robot is located with the highest probability
+ # The calculation is based on a given set of sections, the last five recognized sections and the current recognized section.
+ """
+ def localize(self):
+ if(self.debug):
+ print("Current: ", self.__get_section_type(self.current_section_type), " Last: ", self.last_sections_type, " Last know section:" , self.last_known_section)
+
+ # get all sections which have the same sectiontype as the current sectiontype
+ possible_sections = self.sections[self.sections.type == self.current_section_type]
+ previous_count = 0
+
+ # iterate the this sections and compare the previous sectiontyps of the possible section with the last know sectiontypes
+ while(len(possible_sections) > 1 and previous_count < len(self.last_sections_type)):
+ # brock up if the last known sectiontype is not filled
+ if(self.last_sections_type[previous_count] == None):
+ previous_count = 5
+ else:
+ previous_types = pd.DataFrame(possible_sections.previous.array)
+ possible_sections = possible_sections[(previous_types[previous_count] == self.last_sections_type[previous_count]).array]
+ previous_count += 1
+ # check if more than one section is possible
+ if(len(possible_sections) > 1):
+ if(self.debug):
+ print("More than one part of the track is possible for localization.")
+ if (self.last_known_section is not None):
+
+ # Calculate distance from last known_section to all possible sections
+ index_last_known_section = self.sections[self.sections.s_name == self.last_known_section.s_name].index[0]
+ if(self.debug):
+ print("Index of Last Section: ", index_last_known_section)
+
+ help_sections = self.sections.copy()
+ help_sections.index += self.sections.shape[0]
+ help_sections = self.sections.copy().append(help_sections)
+
+ filtert_section = pd.DataFrame()
+ for possible_section_s_name in np.array(possible_sections.s_name):
+ filtert_section = filtert_section.append(help_sections[help_sections.s_name == possible_section_s_name])
+
+
+ indexs = abs(filtert_section.index - index_last_known_section)
+ if(self.debug):
+ print("Distance from indexs: ", indexs)
+
+ # Get index of entry with the smallest distance
+ min_index = np.argmin(indexs)
+
+ # Check if there is more than one entry with the same distance like min_index
+ all_min_indices = np.where(indexs == indexs[min_index])[0]
+
+ # If there is more than on index, localization failed
+ if(len(all_min_indices) > 1):
+ min_index = all_min_indices[1]
+ if(self.debug):
+ print("No localization possible")
+ if(self.debug):
+ print(possible_sections)
+ s_name_possible_section = filtert_section.iloc[min_index].s_name
+ possible_sections = possible_sections[possible_sections.s_name == s_name_possible_section]
+ if(self.debug):
+ print ("Am nahliegenste Section:", possible_sections)
+ else:
+ if(self.debug):
+ print("No localization possible - More than one part of the track can fit")
+ print(possible_sections)
+
+ elif(len(possible_sections) < 1):
+ self.count_fail_loc += 1
+ if(self.last_known_section is not None and self.last_sections_type[4] is not None):
+ self.current_section_type = self.last_known_section.type
+ self.last_sections_type = self.last_known_section.previous
+ if(self.debug):
+ print("No possible section was found. No localization possible.")
+ if (self.count_fail_loc == 3):
+ self.last_sections_type = [None, None, None, None, None]
+ self.last_known_section = None
+ self.count_fail_loc = 0
+
+ if(len(possible_sections) == 1):
+ if(self.debug):
+ print(possible_sections)
+ self.last_known_section = possible_sections.iloc[0]
+ self.count_fail_loc = 0
+
+ return possible_sections
+
+ """
+ # This is an intern function which translates the int values to a string.
+ It is only used for debugging or printing to the consol.
+ """
+ def __get_section_type(self, section_type_int):
+ section_type_string = ""
+ if section_type_int == S:
+ section_type_string = "straight"
+ elif section_type_int == R:
+ section_type_string = "right"
+ elif section_type_int == L:
+ section_type_string = "left"
+ return section_type_string
+#%% Main
+if __name__ == '__main__':
+ lk = Localizer(Localizer.get_set_of_section_1())
+ lk.update_position(L)
+ lk.update_position(R)
+ print(lk.localize())
\ No newline at end of file
diff --git a/race_management.py b/race_management.py
new file mode 100644
index 0000000000000000000000000000000000000000..2db72bbb9816b9685e81b800c73a39e28b4bc6b1
--- /dev/null
+++ b/race_management.py
@@ -0,0 +1,142 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Tue May 2 15:01:44 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+"""
+#%% imports
+import socket
+from datetime import datetime
+from localization import Localizer
+from driving import Driving_Analyser
+from grafic import Line_Plotter, Line_Plotter_Time, Track_Plotter, Dashboard
+
+#%% constant values
+HOST = '' # Symbolic name meaning all available interfaces
+PORT = 50007 # Arbitrary non-privileged port
+CHAR_RECIVING = 31 # Number of chars one UDP-message contains
+THRESHOLD_LEFT = 0.05 # Value to detect a left turn of the robot
+THRESHOLD_RIGHT = -0.05 # Value to detect a right turn of the robot
+BLOCK_SIZE_DA = 10 # Number of iterations to analyze the section type
+S = 0 # Value for a straight part of the track
+R = 1 # Value for a right turn
+L = 2 # Value for a left turn
+
+#%% Functions
+def main_loop():
+ print("TO EXIT THE PROGRAMM, CLOSE THE DASHBOARDWINDOW!!!")
+ try:
+ # create objektes
+ driving_analyser = Driving_Analyser(THRESHOLD_LEFT, THRESHOLD_RIGHT, BLOCK_SIZE_DA)
+ localizer = Localizer(Localizer.get_set_of_section_3(), False)
+
+ # line plotter with time
+ lp_v = Line_Plotter_Time("Linear speed", "s", "mm/s", 1000)
+ lp_w = Line_Plotter_Time("Rotation speed", "s", "rad/s", 1000)
+ #lp_section_type = Line_Plotter_Time("Filtered rotation speed", "s", "current section type", 1000)
+
+ # trackplotter
+ background, track = Track_Plotter.get_set_of_trackparts_3()
+ tp = Track_Plotter(background, track)
+
+ # dashboard
+ dashboard = Dashboard()
+ dashboard.update_window()
+
+ with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+ s.bind((HOST, PORT))
+ s.listen(1)
+ conn, addr = s.accept()
+ with conn:
+ print('Connected by', addr)
+ while True:
+ dashboard.update_window()
+ # reciving data
+ data = conn.recv(CHAR_RECIVING)
+
+ # check if data countions something
+ if len(data) > 0:
+
+ # pars data to v w and t
+ v,w,t = pars_data(data)
+
+ # update odometrie
+ driving_analyser.calculate_odometrie_continues(w, v, t)
+ driving_analyser.calculate_driven_distance_on_section(v, t)
+
+ # update line plotter
+ lp_v.update(v,t)
+ lp_w.update(w,t)
+ #lp_section_type.update(localizer.current_section_type if localizer.current_section_type is not None else 0, t)
+
+ # get section type
+ section_type,_,_,_ = driving_analyser.detect_section_type(w)
+
+ # if section type is None driving_analyser still count to decide which type it is
+ if(section_type is not None):
+ #print(get_section_type_formated(section_type))
+ # update localizer position
+ localizer.update_position(section_type)
+ update_dashboard(dashboard, localizer, driving_analyser)
+
+ if(localizer.enter_new_section):
+ driving_analyser.reset_driven_distance_on_section()
+ section = localizer.localize()
+ #print(section)
+ if len(section) == 1:
+ print(section.iloc[0].s_name)
+ tp.select_trackpart(section.iloc[0].s_name)
+
+ except Exception as e:
+ print("An error occured")
+ print(e)
+ finally:
+ try:
+ dashboard.window.destroy()
+ except Exception:
+ pass
+ lp_w.close()
+ lp_v.close()
+ #lp_section_type.close();
+ tp.close()
+
+
+def pars_data(data):
+ data = data.decode("utf-8")
+ w = float(data[3:10])
+ if data[2] == "-":
+ w *= -1
+ v = float(data[12:21])
+ t = float(data[23:])
+ return (v,w,t)
+
+def get_section_type_formated(section_type_int):
+ section_type_string = ""
+ if section_type_int == S:
+ section_type_string = "straight"
+ elif section_type_int == R:
+ section_type_string = "right"
+ elif section_type_int == L:
+ section_type_string = "left"
+ return section_type_string
+
+def get_last_sections_type_formated(last_sections_type_int):
+ last_sections_type = ""
+ for section_type in last_sections_type_int:
+ last_sections_type += get_section_type_formated(section_type) + ', '
+ return last_sections_type
+
+def update_dashboard(dashboard, localizer, driving_analyser):
+ dashboard.set_current_section_type(get_section_type_formated(localizer.current_section_type))
+ dashboard.set_last_sections_type(get_last_sections_type_formated(localizer.last_sections_type))
+ dashboard.set_enter_new_section("Yes" if localizer.enter_new_section else "No")
+ dashboard.set_last_known_section("None" if localizer.last_known_section is None else localizer.last_known_section.s_name)
+ dashboard.set_count_fail_loc(localizer.count_fail_loc)
+ dashboard.set_x_position(driving_analyser.x_old)
+ dashboard.set_y_position(driving_analyser.y_old)
+ dashboard.set_driven_distance_on_section(driving_analyser.driven_distance_on_section)
+ #dashboard.update_window()
+
+#%% main script for race controle
+if __name__ == '__main__':
+ main_loop()
\ No newline at end of file