grafic.py

# -*- 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)