diff --git a/README.md b/README.md index ba77262..257fa01 100644 --- a/README.md +++ b/README.md @@ -1 +1,15 @@ -# slam_studio \ No newline at end of file +# Slam Studio + +### Visualization + +#### How to use + +1. `pip3 install -r requirements.txt` +2. sudo apt-get install python3-tk +3. Launch python3 testing.py + +You will see GUI-application. On the left site is random 2D map with robot, on the right site is buttons and application log. This log is formed from a text log file. Now you can go through all free cells and see how laser beams reached the obstacles. + +###### Tests + +1. Launch `pytest` \ No newline at end of file diff --git a/config.json b/config.json new file mode 100644 index 0000000..7032ae3 --- /dev/null +++ b/config.json @@ -0,0 +1,3 @@ +{ + "filename": "slam_studio.log" +} \ No newline at end of file diff --git a/config.py b/config.py new file mode 100644 index 0000000..9a838f1 --- /dev/null +++ b/config.py @@ -0,0 +1,9 @@ +import json + + +def get_filename(): + config_json = 'config.json' + filename = 'filename' + with open(config_json, 'r') as f: + config = json.load(f) + return config[filename] diff --git a/data_utils.py b/data_utils.py new file mode 100644 index 0000000..f7c0ae3 --- /dev/null +++ b/data_utils.py @@ -0,0 +1,4 @@ +def get_iterator(iterable): + for obj in iterable: + yield obj + diff --git a/logger.py b/logger.py new file mode 100644 index 0000000..01e6684 --- /dev/null +++ b/logger.py @@ -0,0 +1,25 @@ +import logging +from config import get_filename + + +class Logger: + _instance = None + + @staticmethod + def get_instance(): + if not Logger._instance: + Logger._instance = Logger() + return Logger._instance.logger + + def __init__(self): + if not Logger._instance: + self.logger = logging.getLogger("Slam studio") + self.logger.setLevel(logging.DEBUG) + filename = get_filename() + file_handler = logging.FileHandler(filename, 'w') + formatter = logging.Formatter('%(asctime)s [%(levelname)s] %(message)s') + file_handler.setFormatter(formatter) + self.logger.addHandler(file_handler) + + + diff --git a/map_model.py b/map_model.py new file mode 100644 index 0000000..660ad89 --- /dev/null +++ b/map_model.py @@ -0,0 +1,96 @@ +import numpy as np + + +def get_simple_map(): + return np.asarray([ + [1, 0, 0, 0, 0], + [1, 0, 0, 1, 0], + [1, 0, 0, 0, 0], + [0, 0, 0, 0, 0], + [0, 0, 0, 1, 1]]) + + +def generate_sparse_matrix(size, density=0.1): + matrix = np.random.choice([0, 1], size=(size, size), p=[1 - density, density]) + return matrix + + +class Map: + def __init__(self, debug=False, size=7, density=0.1, cell_size=1): + if debug: + self.array = get_simple_map() + else: + self.array = generate_sparse_matrix(size, density) + self.cell_size = cell_size + + def __len__(self): + return self.array.__len__() + + def get_random_zero_coordinates(self): + zero_indices = np.argwhere(self.array == 0) + random_index = np.random.randint(zero_indices.shape[0]) + x, y = zero_indices[random_index] + return y + self.cell_size / 2, x + self.cell_size / 2 + + def check_wall(self, x, y): + return 0 == x or x == len(self) - 1 or 0 == y or y == len(self) - 1 + + def get_coord_in_real_world(self, y): + return len(self) - y + + def get_coord_in_reflected_array(self, y): + return len(self) - y - 1 + + def is_cell_occupied_in_real_world(self, x, y): + return self.array[len(self) - y - 1, x] == 1 + + def get_walls_in_real_world(self, x, y): + walls = [] + if self.check_wall(x, y): + if y == 0: + walls.append((x, y, x + self.cell_size, y)) + if y == len(self) - 1: + walls.append((x, y+1, x + self.cell_size, y+1)) + if x == 0: + walls.append((x, y, x, y + self.cell_size)) + if x == len(self) - 1: + walls.append((x+1, y, x+1, y + self.cell_size)) + return walls + + @staticmethod + def get_obstacle_boundaries_in_real_world(x, y): + return [(x, y, x + 1, y), (x + 1, y, x + 1, y + 1), + (x, y, x, y + 1), (x, y + 1, x + 1, y + 1)] + + def get_limits_for_finding_obstacle_in_array(self, alpha, x, y): + if alpha < 90: + limit_x, limit_y = (0, int(x)+1), (0, int(y)+1) + elif alpha < 180: + limit_x, limit_y = (0, int(x)+1), (int(y), len(self.array)) + elif alpha < 270: + limit_x, limit_y = (int(x), len(self.array)), (int(y), len(self.array)) + else: + limit_x, limit_y = (int(x), len(self.array)), (0, int(y) + 1) + return limit_x, limit_y + + def get_limits_for_finding_obstacle_in_real_world(self, alpha, x, y): + if alpha < 90: + limit_x, limit_y = (0, x), (0, y) + elif alpha < 180: + limit_x, limit_y = (0, x), (y, len(self.array)) + elif alpha < 270: + limit_x, limit_y = (x, len(self.array)), (y, len(self.array)) + else: + limit_x, limit_y = (x, len(self.array)), (0, y) + return limit_x, limit_y + + def get_free_positions(self): + positions = [] + for j, row in enumerate(self.array): + for i, value in enumerate(row): + if value != 1: + positions.append((i + 0.5 * self.cell_size, j + 0.5 * self.cell_size, 0)) + return positions + + + diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..80ba43f --- /dev/null +++ b/requirements.txt @@ -0,0 +1,3 @@ +numpy +sh +pytest \ No newline at end of file diff --git a/scans_generator.py b/scans_generator.py new file mode 100644 index 0000000..800dabb --- /dev/null +++ b/scans_generator.py @@ -0,0 +1,200 @@ +from math import cos, sin, sqrt, pi +import numpy as np + + +def euclidean_distance(x1, x2, y1, y2): + return sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2) + + +def get_point_in_direction(array, direction, limit): + k = 0 + if direction > 0: + for i in range(limit + 1, len(array)): + if array[i] == 1: + return k + k += 1 + else: + for i in range(limit - 1, -1, -1): + if array[i] == 1: + return k + k += 1 + return k + + +def get_params_occupied_cell(x, y, angle, simple_map): + x_end, y_end = x, y + length = 0 + e = 0.00000001 + radians = pi * (angle / 180) + cos_alpha = cos(radians) + cos_alpha = 0 if abs(cos_alpha) < e else cos_alpha + sin_alpha = sin(radians) + sin_alpha = 0 if abs(sin_alpha) < e else sin_alpha + + if not cos_alpha: + direction = -1 * sin_alpha + length = get_point_in_direction(simple_map[:, x], direction, y) + if direction > 0: + y_end += (length + 1) + else: + y_end -= (length + 1) + elif not sin_alpha: + direction = -1 * cos_alpha + length = get_point_in_direction(simple_map[y], direction, x) + if direction > 0: + x_end += (length + 1) + else: + x_end -= (length + 1) + return length, x_end, y_end + + +def cross(A1, B1, C1, A2, B2, C2): + A = np.array([[A1, B1], [A2, B2]]) + C = np.array([C1, C2]) + if np.linalg.det(A) != 0: + return np.linalg.solve(A, C) + return None, None + + +def get_matrix_coefficients_for_line(b_robot, k_robot): + if k_robot is None: + A1 = 1 + B1 = 0 + C1 = b_robot + else: + A1 = -1 * k_robot + B1 = 1 + C1 = b_robot + return A1, B1, C1 + + +def get_line_coefficients(**kwargs): + e = 0.00000001 + if 'alpha' in kwargs: + alpha, x, y = kwargs['alpha'], kwargs['x'], kwargs['y'] + radians = pi * (alpha / 180) + cos_alpha = cos(radians) + cos_alpha = 0 if abs(cos_alpha) < e else cos_alpha + if not cos_alpha: + return None, x + k = sin(radians) / cos(radians) + b = y - k * x + elif 'coordinates' in kwargs: + x1, y1, x2, y2 = kwargs['coordinates'] + k = (y2 - y1) / (x2 - x1) if x2 != x1 else None + if k is None: + return None, x1 + b = y1 - k * x1 + else: + raise ValueError('Invalid arguments provided.') + return k, b + + +def is_point_belongs_segment(x_cross, y_cross, segment_coordinates): + x1, y1, x2, y2 = segment_coordinates + return min(x1, x2) <= round(x_cross, 1) <= max(x1, x2) and min(y1, y2) <= round(y_cross, 1) <= max(y1, y2) + + +def is_point_belongs_area(x_cross, y_cross, limit_x, limit_y): + return limit_x[0] <= round(x_cross, 1) <= limit_x[1] and limit_y[0] <= round(y_cross, 1) <= limit_y[1] + + +def get_closest_point(x_robot, y_robot, alpha, local_map): + # y_robot_reflection = local_map.get_coord_in_reflected_array(y_robot) + y_robot_in_real_world = local_map.get_coord_in_real_world(y_robot) + k_robot, b_robot = get_line_coefficients(alpha=alpha, x=x_robot, y=y_robot_in_real_world) + limit_x, limit_y = local_map.get_limits_for_finding_obstacle_in_array(alpha, x_robot, y_robot_in_real_world) + limit_x_real, limit_y_real = local_map.get_limits_for_finding_obstacle_in_real_world(alpha, x_robot, + y_robot_in_real_world) + + walls = local_map.get_walls_in_real_world(int(x_robot), int(y_robot_in_real_world)) + points = {} + crossing_points = [] + + for j in range(*limit_y): + for i in range(*limit_x): + # if i == int(x_robot) and j == int(y_robot_in_real_world): + # continue + if local_map.is_cell_occupied_in_real_world(i, j): + coordinates = local_map.get_obstacle_boundaries_in_real_world(i, j) + cr_points = get_crossing_points(k_robot, b_robot, coordinates, x_robot, y_robot_in_real_world) + if cr_points: + crossing_points.extend(cr_points) + + else: + # wall = wall or local_map.check_wall(i, j) # FIXME + if local_map.check_wall(i, j): + walls += local_map.get_walls_in_real_world(i, j) + if walls: + walls += local_map.get_walls_in_real_world(int(x_robot), int(y_robot_in_real_world)) # FIXME + cr_points = get_crossing_points(k_robot, b_robot, walls, x_robot, y_robot_in_real_world) + if cr_points: + crossing_points.extend(cr_points) + + for crossing_point in crossing_points: + len_cross, x_cross, y_cross = crossing_point + if is_point_belongs_area(x_cross, y_cross, limit_x_real, limit_y_real): + points.update({len_cross: (x_cross, y_cross)}) + + closest_point = min(points.keys()) # FIXME + x_result, y_result = points[closest_point] + y_result = local_map.get_coord_in_real_world(y_result) + return x_result, y_result + + +def get_crossing_points(k_robot, b_robot, coordinates, x_robot, y_robot): # FIXME + a1, b1, c1 = get_matrix_coefficients_for_line(b_robot, k_robot) + result = [] + for coordinate in coordinates: + k_segment, b_segment = get_line_coefficients(coordinates=coordinate) + a2, b2, c2 = get_matrix_coefficients_for_line(b_segment, k_segment) + x_cross, y_cross = cross(a1, b1, c1, a2, b2, c2) + if x_cross is not None and is_point_belongs_segment(x_cross, y_cross, coordinate): + length = euclidean_distance(x_robot, x_cross, y_robot, y_cross) + result.append((length, x_cross, y_cross)) + return result + + # min_len = None + # min_x, min_y = None, None + # if x_cross is not None: + # x_cross, y_cross = round(x_cross, 1), round(y_cross, 1) + # if is_point_belongs_segment(x_cross, y_cross, coordinate): + # length = euclidean_distance(x_robot, x_cross, y_robot, y_cross) + # if not min_len or length < min_len: + # min_len = length + # min_x, min_y = x_cross, y_cross + # if min_len is not None: + # return (min_len, min_x, min_y) + + +def get_cell_fragments_coordinates(cell_size, i, j): + coordinates = [(i, j, i + cell_size, j), + (i + cell_size, j, i + cell_size, j + cell_size), + (i + cell_size, j + cell_size, i, j + cell_size), + (i, j, i, j + cell_size)] + return coordinates + + +def generate(x_robot, y_robot, local_map, alpha_diff=90, cell_size=1): + scan = {} + for alpha in range(0, 360, alpha_diff): + if not alpha % 90: + + _, x_array, y_array = get_params_occupied_cell(int(x_robot), int(y_robot), alpha, local_map.array) + k_robot, b_robot = get_line_coefficients(alpha=alpha, x=x_robot, y=y_robot) + coordinates = get_cell_fragments_coordinates(cell_size, x_array, y_array) + crossing_points = get_crossing_points(k_robot, b_robot, coordinates, x_robot, y_robot) + + closest_point = min(crossing_points, key=lambda x: x[0]) # FIXME + scan.update({alpha: closest_point[1:]}) + else: + try: + x_array, y_array = get_closest_point(x_robot, y_robot, alpha, local_map) + scan.update({alpha: (x_array, y_array)}) + # print(scan) + except Exception as e: + print('Failed! Угол:', alpha, '\nx:', x_robot, 'y:', y_robot) + print(e) + return scan + + diff --git a/test_scan_creating.py b/test_scan_creating.py new file mode 100644 index 0000000..d52b957 --- /dev/null +++ b/test_scan_creating.py @@ -0,0 +1,262 @@ +import pytest +from scans_generator import generate +from map_model import Map + + +@pytest.fixture +def local_map(): + return Map(debug=True) + + +def test_positions_creating(local_map): + positions = local_map.get_free_positions() + correct_positions_for_local_map = [(1.5, 0.5, 0), + (2.5, 0.5, 0), + (3.5, 0.5, 0), + (4.5, 0.5, 0), + (1.5, 1.5, 0), + (2.5, 1.5, 0), + (4.5, 1.5, 0), + (1.5, 2.5, 0), + (2.5, 2.5, 0), + (3.5, 2.5, 0), + (4.5, 2.5, 0), + (0.5, 3.5, 0), + (1.5, 3.5, 0), + (2.5, 3.5, 0), + (3.5, 3.5, 0), + (4.5, 3.5, 0), + (0.5, 4.5, 0), + (1.5, 4.5, 0), + (2.5, 4.5, 0)] + assert positions == correct_positions_for_local_map + + +def test_scans_creating(local_map): + positions = local_map.get_free_positions() + correct_scans = [ + {0: (1.0, 0.5), 10: (1.0, 0.5881634903542325), 20: (1.0, 0.6819851171331015), 30: (1.0, 0.7886751345948131), + 40: (1.0, 0.9195498155886401), 50: (1.0000000000000002, 1.0958767962971048), 60: (1.0, 1.3660254037844384), + 70: (0.9999999999999999, 1.8737387097273106), 80: (0.7065285868119073, 5.0), 90: (1.5, 0.0), + 100: (1.4118365096457675, 0.0), 110: (1.3180148828668992, 0.0), 120: (1.2113248654051876, 0.0), + 130: (1.0804501844113603, 0.0), 140: (1.0, 0.08045018441135987), 150: (1.0, 0.2113248654051869), + 160: (1.0, 0.3180148828668994), 170: (1.0, 0.41183650964576746), 180: (5.0, 0.4999999999999996), + 190: (4.335640909808852, 0.0), 200: (2.873738709727312, 0.0), 210: (2.3660254037844384, 0.0), + 220: (2.0958767962971048, 0.0), 230: (1.9195498155886401, 0.0), 240: (1.7886751345948133, 0.0), + 250: (1.6819851171331015, 0.0), 260: (1.5881634903542323, 0.0), 270: (1.5, 5.0), + 280: (2.2934714131880907, 5.0), 290: (3.0000000000000004, 4.62121612918193), 300: (3.5207259421636903, 4.0), + 310: (4.43684870912048, 4.0), 320: (3.0, 1.7586494467659213), 330: (3.0, 1.3660254037844402), + 340: (3.0, 1.045955351399305), 350: (5.0, 1.1171444324796278)}, + {0: (1.0, 0.5), 10: (1.0, 0.7644904710626976), 20: (1.0, 1.0459553513993036), 30: (1.0, 1.3660254037844384), + 40: (1.0, 1.75864944676592), 50: (0.9999999999999998, 2.2876303888913148), 60: (-0.0, 4.830127018922192), + 70: (0.8621339458020889, 5.0), 80: (1.7065285868119073, 5.0), 90: (2.5, 0.0), 100: (2.4118365096457675, 0.0), + 110: (2.318014882866899, 0.0), 120: (2.2113248654051865, 0.0), 130: (2.0804501844113608, 0.0), + 140: (1.904123203702895, 0.0), 150: (1.6339745962155605, 0.0), 160: (1.1262612902726903, 0.0), + 170: (1.0, 0.23550952893730148), 180: (5.0, 0.4999999999999997), 190: (5.0, 0.059182548228837284), + 200: (3.8737387097273115, 0.0), 210: (3.3660254037844384, 0.0), 220: (3.095876796297105, 0.0), + 230: (2.91954981558864, 0.0), 240: (2.7886751345948135, 0.0), 250: (2.6819851171331015, 0.0), + 260: (2.5881634903542325, 0.0), 270: (2.5, 5.0), 280: (3.117144432479626, 4.0), + 290: (3.0000000000000004, 1.873738709727311), 300: (3.0000000000000004, 1.3660254037844388), + 310: (3.0, 1.0958767962971048), 320: (3.095876796297104, 1.0), 330: (3.3660254037844375, 1.0), + 340: (3.8737387097273075, 1.0), 350: (5.0, 0.9408174517711627)}, + {0: (1.0, 0.5), 10: (1.0, 0.9408174517711627), 20: (1.0, 1.4099255856655057), 30: (1.0, 1.943375672974064), + 40: (1.0, 2.5977490779431993), 50: (3.0804501844113594, 1.0), 60: (3.211324865405187, 1.0), + 70: (3.3180148828668985, 1.0), 80: (3.411836509645768, 1.0), 90: (3.5, 0.0), 100: (3.4118365096457675, 0.0), + 110: (3.318014882866899, 0.0), 120: (3.211324865405187, 0.0), 130: (3.0804501844113608, 0.0), + 140: (2.904123203702895, 0.0), 150: (2.6339745962155616, 0.0), 160: (2.126261290272689, 0.0), + 170: (1.0, 0.059182548228836396), 180: (5.0, 0.49999999999999983), 190: (5.0, 0.23550952893730237), + 200: (4.873738709727311, 0.0), 210: (4.366025403784438, 0.0), 220: (4.095876796297105, 0.0), + 230: (3.9195498155886406, 0.0), 240: (3.788675134594813, 0.0), 250: (3.681985117133101, 0.0), + 260: (3.5881634903542325, 0.0), 270: (3.5, 1.0), 280: (3.588163490354232, 1.0), 290: (3.6819851171331015, 1.0), + 300: (3.7886751345948126, 1.0), 310: (3.9195498155886406, 1.0), 320: (5.0, 1.7586494467659204), + 330: (5.0, 1.3660254037844393), 340: (5.0, 1.045955351399304), 350: (5.0, 0.7644904710626976)}, + {0: (1.0, 0.5), 10: (1.0, 1.1171444324796274), 20: (3.1262612902726885, 1.0), 30: (3.6339745962155607, 1.0), + 40: (3.9041232037028952, 1.0), 50: (4.0, 1.0958767962971052), 60: (4.0, 1.3660254037844384), + 70: (4.0, 1.873738709727312), 80: (3.882855567520372, 4.0), 90: (4.5, 0.0), 100: (4.4118365096457675, 0.0), + 110: (4.3180148828668985, 0.0), 120: (4.211324865405187, 0.0), 130: (4.080450184411361, 0.0), + 140: (3.904123203702895, 0.0), 150: (3.6339745962155607, 0.0), 160: (3.1262612902726903, 0.0), + 170: (1.6643590901911498, 0.0), 180: (5.0, 0.49999999999999994), 190: (5.0, 0.41183650964576746), + 200: (5.0, 0.3180148828668985), 210: (5.0, 0.2113248654051869), 220: (5.0, 0.08045018441135987), + 230: (4.91954981558864, 0.0), 240: (4.788675134594813, 0.0), 250: (4.6819851171331015, 0.0), + 260: (4.5881634903542325, 0.0), 270: (4.5, 4.0), 280: (5.000000000000001, 3.335640909808863), + 290: (5.0, 1.873738709727311), 300: (5.0, 1.3660254037844375), 310: (5.000000000000001, 1.0958767962971048), + 320: (5.0, 0.9195498155886392), 330: (5.0, 0.7886751345948131), 340: (5.0, 0.6819851171331006), + 350: (5.0, 0.5881634903542325)}, + {0: (1.0, 1.5), 10: (1.0, 1.5881634903542325), 20: (1.0, 1.6819851171331015), 30: (1.0, 1.7886751345948126), + 40: (1.0, 1.9195498155886397), 50: (1.0000000000000002, 2.0958767962971048), 60: (1.0, 2.3660254037844384), + 70: (1.0, 2.87373870972731), 80: (0.8828555675203724, 5.0), 90: (1.5, 0.0), 100: (1.2355095289373026, 0.0), + 110: (0.9999999999999999, 0.12626129027268806), 120: (1.0000000000000002, 0.6339745962155607), + 130: (0.9999999999999994, 0.9041232037028939), 140: (1.0, 1.0804501844113599), 150: (1.0, 1.2113248654051874), + 160: (1.0, 1.318014882866899), 170: (1.0, 1.4118365096457675), 180: (3.0, 1.4999999999999998), + 190: (3.0, 1.2355095289373024), 200: (3.0, 0.9540446486006964), 210: (4.098076211353315, 0.0), + 220: (3.287630388891315, 0.0), 230: (2.7586494467659204, 0.0), 240: (2.3660254037844397, 0.0), + 250: (2.0459553513993045, 0.0), 260: (1.7644904710626974, 0.0), 270: (1.5, 5.0), 280: (2.117144432479626, 5.0), + 290: (2.7738958199317096, 5.0), 300: (2.9999999999999996, 4.098076211353315), 310: (3.5977490779431998, 4.0), + 320: (4.4793839814855225, 4.0), 330: (5.0, 3.5207259421636934), 340: (3.0, 2.045955351399305), + 350: (3.0, 1.7644904710626976)}, + {0: (1.0, 1.5), 10: (1.0, 1.7644904710626972), 20: (1.0, 2.0459553513993036), 30: (1.0, 2.3660254037844384), + 40: (1.0, 2.75864944676592), 50: (-0.0, 4.479383981485524), 60: (0.47927405783630916, 5.0), + 70: (1.2261041800682915, 5.0), 80: (1.8828555675203724, 5.0), 90: (2.5, 0.0), 100: (2.235509528937303, 0.0), + 110: (1.9540446486006968, 0.0), 120: (1.6339745962155616, 0.0), 130: (1.2413505532340807, 0.0), + 140: (1.0, 0.2413505532340796), 150: (1.0, 0.6339745962155616), 160: (1.0, 0.9540446486006964), + 170: (1.0, 1.2355095289373024), 180: (3.0, 1.4999999999999998), 190: (3.0, 1.4118365096457675), + 200: (3.0, 1.318014882866899), 210: (3.0, 1.211324865405187), 220: (3.0, 1.0804501844113603), + 230: (3.758649446765921, 0.0), 240: (3.3660254037844397, 0.0), 250: (3.0459553513993045, 0.0), + 260: (2.7644904710626976, 0.0), 270: (2.5, 5.0), 280: (3.0, 4.3356409098088635), + 290: (3.4099255856655066, 4.0), 300: (3.9433756729740645, 4.0), 310: (4.5977490779432, 4.0), + 320: (3.0, 1.919549815588641), 330: (3.0, 1.788675134594813), 340: (3.0, 1.681985117133102), + 350: (3.0, 1.5881634903542325)}, + {0: (4.0, 1.5), 10: (4.0, 1.5881634903542325), 20: (4.0, 1.681985117133101), 30: (4.0, 1.7886751345948126), + 40: (4.0, 1.9195498155886401), 50: (1.5631512908795198, 5.0), 60: (3.056624327025935, 4.0), + 70: (3.590074414334494, 4.0), 80: (4.059182548228837, 4.0), 90: (4.5, 0.0), 100: (4.235509528937302, 0.0), + 110: (3.9540446486006964, 0.0), 120: (3.6339745962155616, 0.0), 130: (3.241350553234081, 0.0), + 140: (4.0, 1.0804501844113603), 150: (4.0, 1.2113248654051874), 160: (4.0, 1.3180148828668985), + 170: (4.0, 1.4118365096457675), 180: (5.0, 1.4999999999999998), 190: (5.0, 1.4118365096457675), + 200: (5.0, 1.3180148828668985), 210: (5.0, 1.211324865405187), 220: (5.0, 1.0804501844113599), + 230: (5.0, 0.9041232037028948), 240: (4.999999999999999, 0.6339745962155625), 250: (5.0, 0.12626129027269162), + 260: (4.764490471062698, 0.0), 270: (4.5, 4.0), 280: (4.940817451771162, 4.0), 290: (5.0, 2.8737387097273097), + 300: (5.0, 2.3660254037844366), 310: (5.0, 2.0958767962971043), 320: (5.0, 1.9195498155886401), + 330: (5.0, 1.788675134594813), 340: (5.0, 1.681985117133101), 350: (5.0, 1.5881634903542325)}, + {0: (1.0, 2.5), 10: (1.0, 2.5881634903542325), 20: (1.0, 2.6819851171331015), 30: (1.0, 2.7886751345948126), + 40: (1.0, 2.91954981558864), 50: (-0.0, 4.287630388891315), 60: (0.05662432702593514, 5.0), + 70: (0.5900744143344936, 5.0), 80: (1.0591825482288375, 5.0), 90: (1.5, 0.0), 100: (1.0591825482288377, 0.0), + 110: (0.9999999999999999, 1.126261290272688), 120: (0.9999999999999999, 1.6339745962155603), + 130: (1.0000000000000002, 1.9041232037028948), 140: (1.0, 2.08045018441136), 150: (1.0, 2.2113248654051874), + 160: (1.0, 2.318014882866899), 170: (1.0, 2.4118365096457675), 180: (5.0, 2.4999999999999996), + 190: (5.0, 1.8828555675203722), 200: (3.0, 1.9540446486006964), 210: (3.0, 1.6339745962155607), + 220: (3.0, 1.2413505532340805), 230: (3.597749077943201, 0.0), 240: (2.943375672974066, 0.0), + 250: (2.4099255856655075, 0.0), 260: (1.9408174517711623, 0.0), 270: (1.5, 5.0), + 280: (1.9408174517711616, 5.0), 290: (2.409925585665507, 5.0), 300: (2.9433756729740645, 5.0), + 310: (3.0, 4.287630388891315), 320: (3.287630388891314, 4.0), 330: (4.0980762113533125, 4.0), + 340: (5.0, 3.773895819931711), 350: (5.0, 3.117144432479628)}, + {0: (1.0, 2.5), 10: (1.0, 2.764490471062697), 20: (1.0, 3.0459553513993036), 30: (0.0, 3.943375672974064), + 40: (0.0, 4.5977490779432), 50: (0.4022509220567995, 5.0), 60: (1.0566243270259352, 5.0), + 70: (1.5900744143344938, 5.0), 80: (2.0591825482288373, 5.0), 90: (2.5, 0.0), 100: (2.0591825482288377, 0.0), + 110: (1.5900744143344945, 0.0), 120: (1.0566243270259363, 0.0), 130: (1.0000000000000002, 0.7123696111086844), + 140: (1.0, 1.2413505532340796), 150: (1.0, 1.6339745962155616), 160: (1.0, 1.954044648600696), + 170: (1.0, 2.2355095289373024), 180: (5.0, 2.5), 190: (5.0, 2.0591825482288373), + 200: (3.8737387097273115, 2.0), 210: (3.3660254037844384, 2.0), 220: (3.095876796297105, 2.0), + 230: (3.0, 1.9041232037028957), 240: (2.9999999999999996, 1.633974596215563), 250: (3.0, 1.1262612902726907), + 260: (2.9408174517711623, 0.0), 270: (2.5, 5.0), 280: (2.9408174517711614, 5.0), 290: (3.045955351399304, 4.0), + 300: (3.3660254037844384, 4.0), 310: (3.7586494467659204, 4.0), 320: (4.287630388891313, 4.0), + 330: (5.0, 3.9433756729740663), 340: (5.0, 3.409925585665508), 350: (5.0, 2.9408174517711627)}, + {0: (1.0, 2.5), 10: (1.0, 2.9408174517711623), 20: (0.0, 3.7738958199317083), 30: (0.0, 4.52072594216369), + 40: (0.5206160185144741, 5.0), 50: (1.4022509220567994, 5.0), 60: (2.056624327025935, 5.0), + 70: (2.954044648600696, 4.0), 80: (3.235509528937303, 4.0), 90: (3.5, 2.0), 100: (3.4118365096457675, 2.0), + 110: (3.318014882866899, 2.0), 120: (3.211324865405187, 2.0), 130: (3.08045018441136, 2.0), + 140: (1.0, 0.40225092205679935), 150: (1.0, 1.056624327025936), 160: (1.0, 1.5900744143344938), + 170: (1.0, 2.059182548228837), 180: (5.0, 2.5), 190: (5.0, 2.2355095289373024), 200: (5.0, 1.9540446486006964), + 210: (5.0, 1.6339745962155607), 220: (5.0, 1.2413505532340796), 230: (3.9195498155886406, 2.0), + 240: (3.788675134594813, 2.0), 250: (3.681985117133101, 2.0), 260: (3.5881634903542325, 2.0), 270: (3.5, 4.0), + 280: (3.7644904710626967, 4.0), 290: (4.0459553513993045, 4.0), 300: (4.366025403784438, 4.0), + 310: (4.7586494467659195, 4.0), 320: (5.0, 3.7586494467659204), 330: (5.0, 3.3660254037844393), + 340: (5.0, 3.0459553513993045), 350: (5.0, 2.7644904710626976)}, + {0: (1.0, 2.5), 10: (0.0, 3.2934714131880924), 20: (0.0, 4.137866054197911), 30: (0.1698729810778054, 5.0), + 40: (1.5206160185144746, 5.0), 50: (3.24135055323408, 4.0), 60: (3.633974596215561, 4.0), + 70: (3.9540446486006964, 4.0), 80: (4.235509528937302, 4.0), 90: (4.5, 0.0), 100: (4.059182548228837, 0.0), + 110: (4.0, 1.126261290272689), 120: (4.000000000000001, 1.633974596215562), 130: (4.0, 1.9041232037028948), + 140: (3.904123203702895, 2.0), 150: (3.6339745962155607, 2.0), 160: (3.1262612902726903, 2.0), + 170: (1.0, 1.8828555675203718), 180: (5.0, 2.5), 190: (5.0, 2.4118365096457675), + 200: (5.0, 2.3180148828668985), 210: (5.0, 2.211324865405187), 220: (5.0, 2.08045018441136), + 230: (5.0, 1.9041232037028948), 240: (4.999999999999999, 1.633974596215563), + 250: (5.000000000000001, 1.1262612902726912), 260: (4.940817451771163, 0.0), 270: (4.5, 4.0), + 280: (4.764490471062697, 4.0), 290: (5.0, 3.87373870972731), 300: (5.0, 3.366025403784437), + 310: (4.999999999999999, 3.0958767962971048), 320: (5.0, 2.91954981558864), 330: (5.0, 2.788675134594813), + 340: (5.0, 2.681985117133101), 350: (5.0, 2.5881634903542325)}, + {0: (0.0, 3.5), 10: (0.0, 3.5881634903542325), 20: (0.0, 3.681985117133101), 30: (0.0, 3.788675134594813), + 40: (0.0, 3.91954981558864), 50: (9.315877304874933e-17, 4.095876796297105), 60: (-0.0, 4.366025403784438), + 70: (-0.0, 4.873738709727311), 80: (0.23550952893730245, 5.0), 90: (0.5, 3.0), 100: (0.4118365096457675, 3.0), + 110: (0.3180148828668988, 3.0), 120: (0.21132486540518736, 3.0), 130: (0.08045018441136018, 3.0), + 140: (0.0, 3.08045018441136), 150: (0.0, 3.211324865405187), 160: (0.0, 3.3180148828668985), + 170: (0.0, 3.4118365096457675), 180: (5.0, 3.4999999999999996), 190: (5.0, 2.706528586811907), + 200: (5.0, 1.8621339458020896), 210: (3.098076211353315, 2.0), 220: (3.0, 1.4022509220568002), + 230: (0.9195498155886404, 3.0), 240: (0.7886751345948132, 3.0), 250: (0.6819851171331015, 3.0), + 260: (0.5881634903542324, 3.0), 270: (0.5, 5.0), 280: (0.7644904710626967, 5.0), 290: (1.045955351399304, 5.0), + 300: (1.3660254037844386, 5.0), 310: (1.75864944676592, 5.0), 320: (2.287630388891314, 5.0), + 330: (3.0, 4.943375672974066), 340: (3.0, 4.409925585665508), 350: (3.335640909808854, 4.0)}, + {0: (0.0, 3.5), 10: (0.0, 3.7644904710626976), 20: (0.0, 4.045955351399304), 30: (0.0, 4.366025403784438), + 40: (0.0, 4.7586494467659195), 50: (0.24135055323407986, 5.0), 60: (0.6339745962155611, 5.0), + 70: (0.954044648600696, 5.0), 80: (1.2355095289373024, 5.0), 90: (1.5, 0.0), 100: (1.0, 0.6643590901911445), + 110: (1.0, 2.1262612902726885), 120: (0.9999999999999999, 2.6339745962155603), + 130: (1.0000000000000002, 2.9041232037028952), 140: (0.9041232037028951, 3.0), 150: (0.6339745962155611, 3.0), + 160: (0.126261290272689, 3.0), 170: (0.0, 3.235509528937302), 180: (5.0, 3.4999999999999996), + 190: (5.0, 2.882855567520372), 200: (5.0, 2.226104180068292), 210: (5.0, 1.4792740578363088), + 220: (3.287630388891315, 2.0), 230: (3.0, 1.7123696111086861), 240: (3.5207259421636925, 0.0), + 250: (2.7738958199317105, 0.0), 260: (2.1171444324796274, 0.0), 270: (1.5, 5.0), 280: (1.764490471062697, 5.0), + 290: (2.045955351399304, 5.0), 300: (2.366025403784439, 5.0), 310: (2.7586494467659195, 5.0), + 320: (3.0, 4.75864944676592), 330: (3.0, 4.366025403784439), 340: (3.0, 4.045955351399305), + 350: (4.335640909808855, 4.0)}, + {0: (0.0, 3.5), 10: (0.0, 3.9408174517711627), 20: (0.0, 4.409925585665506), 30: (0.0, 4.943375672974064), + 40: (0.7123696111086849, 5.0), 50: (1.2413505532340796, 5.0), 60: (1.6339745962155612, 5.0), + 70: (1.9540446486006964, 5.0), 80: (2.2355095289373024, 5.0), 90: (2.5, 0.0), 100: (1.8828555675203729, 0.0), + 110: (1.2261041800682921, 0.0), 120: (1.0000000000000002, 0.9019237886466831), + 130: (1.0000000000000002, 1.7123696111086844), 140: (1.0, 2.241350553234079), 150: (1.0, 2.6339745962155616), + 160: (1.0, 2.954044648600696), 170: (0.0, 3.0591825482288364), 180: (5.0, 3.5), 190: (5.0, 3.0591825482288373), + 200: (5.0, 2.5900744143344943), 210: (5.0, 2.056624327025935), 220: (5.0, 1.4022509220568002), + 230: (3.758649446765921, 2.0), 240: (3.3660254037844397, 2.0), 250: (3.0459553513993045, 2.0), + 260: (3.117144432479627, 0.0), 270: (2.5, 5.0), 280: (2.7644904710626967, 5.0), + 290: (2.9999999999999996, 4.87373870972731), 300: (2.9999999999999996, 4.366025403784438), + 310: (3.0, 4.095876796297105), 320: (3.0958767962971043, 4.0), 330: (3.3660254037844375, 4.0), + 340: (3.8737387097273075, 4.0), 350: (5.0, 3.9408174517711627)}, + {0: (0.0, 3.5), 10: (0.0, 4.117144432479627), 20: (0.0, 4.773895819931708), 30: (0.9019237886466837, 5.0), + 40: (1.7123696111086844, 5.0), 50: (3.0804501844113594, 4.0), 60: (3.211324865405187, 4.0), + 70: (3.3180148828668985, 4.0), 80: (3.411836509645768, 4.0), 90: (3.5, 2.0), 100: (3.235509528937303, 2.0), + 110: (2.9540446486006964, 2.0), 120: (1.4792740578363108, 0.0), 130: (1.0000000000000002, 0.5206160185144748), + 140: (1.0, 1.4022509220567994), 150: (1.0, 2.056624327025936), 160: (1.0, 2.590074414334494), + 170: (0.6643590901911511, 3.0), 180: (5.0, 3.5), 190: (5.0, 3.2355095289373024), + 200: (5.0, 2.9540446486006964), 210: (5.0, 2.6339745962155607), 220: (5.0, 2.2413505532340796), + 230: (5.0, 1.7123696111086866), 240: (4.999999999999999, 0.9019237886466884), 250: (4.0459553513993045, 2.0), + 260: (3.764490471062697, 2.0), 270: (3.5, 4.0), 280: (3.588163490354232, 4.0), 290: (3.6819851171331015, 4.0), + 300: (3.788675134594813, 4.0), 310: (3.9195498155886397, 4.0), 320: (4.095876796297105, 4.0), + 330: (4.3660254037844375, 4.0), 340: (4.873738709727308, 4.0), 350: (5.0, 3.7644904710626976)}, + {0: (0.0, 3.5), 10: (0.0, 4.293471413188092), 20: (3.1262612902726885, 4.0), 30: (3.6339745962155607, 4.0), + 40: (3.9041232037028952, 4.0), 50: (4.08045018441136, 4.0), 60: (4.211324865405187, 4.0), + 70: (4.3180148828668985, 4.0), 80: (4.4118365096457675, 4.0), 90: (4.5, 0.0), 100: (3.8828555675203726, 0.0), + 110: (3.9540446486006964, 2.0), 120: (3.6339745962155616, 2.0), 130: (3.2413505532340805, 2.0), + 140: (1.0, 0.5631512908795191), 150: (1.0, 1.4792740578363106), 160: (1.0, 2.226104180068291), + 170: (1.0, 2.8828555675203718), 180: (5.0, 3.5), 190: (5.0, 3.4118365096457675), + 200: (5.0, 3.3180148828668985), 210: (5.0, 3.211324865405187), 220: (5.0, 3.08045018441136), + 230: (5.0, 2.9041232037028952), 240: (4.999999999999999, 2.6339745962155634), + 250: (5.000000000000001, 2.126261290272691), 260: (5.0, 0.6643590901911436), 270: (4.5, 4.0), + 280: (4.5881634903542325, 4.0), 290: (4.6819851171331015, 4.0), 300: (4.788675134594813, 4.0), + 310: (4.91954981558864, 4.0), 320: (5.0, 3.91954981558864), 330: (5.0, 3.788675134594813), + 340: (5.0, 3.6819851171331015), 350: (5.0, 3.5881634903542325)}, + {0: (0.0, 4.5), 10: (0.0, 4.5881634903542325), 20: (0.0, 4.6819851171331015), 30: (0.0, 4.788675134594813), + 40: (0.0, 4.91954981558864), 50: (0.08045018441135993, 5.0), 60: (0.21132486540518702, 5.0), + 70: (0.3180148828668988, 5.0), 80: (0.41183650964576746, 5.0), 90: (0.5, 3.0), 100: (0.23550952893730262, 3.0), + 110: (0.0, 3.126261290272689), 120: (0.0, 3.6339745962155607), 130: (0.0, 3.904123203702895), + 140: (0.0, 4.08045018441136), 150: (0.0, 4.211324865405187), 160: (0.0, 4.3180148828668985), + 170: (0.0, 4.4118365096457675), 180: (3.0, 4.5), 190: (3.0, 4.059182548228837), 200: (5.0, 2.8621339458020896), + 210: (5.0, 1.901923788646683), 220: (3.479383981485526, 2.0), 230: (3.0, 1.520616018514477), + 240: (3.098076211353319, 0.0), 250: (1.0459553513993045, 3.0), 260: (0.7644904710626975, 3.0), 270: (0.5, 5.0), + 280: (0.5881634903542323, 5.0), 290: (0.6819851171331014, 5.0), 300: (0.7886751345948129, 5.0), + 310: (0.91954981558864, 5.0), 320: (1.0958767962971045, 5.0), 330: (1.3660254037844377, 5.0), + 340: (1.8737387097273082, 5.0), 350: (3.0, 4.940817451771163)}, + {0: (0.0, 4.5), 10: (0.0, 4.764490471062698), 20: (0.1262612902726888, 5.0), 30: (0.6339745962155611, 5.0), + 40: (0.9041232037028949, 5.0), 50: (1.08045018441136, 5.0), 60: (1.2113248654051871, 5.0), + 70: (1.3180148828668985, 5.0), 80: (1.4118365096457675, 5.0), 90: (1.5, 0.0), + 100: (1.0000000000000002, 1.6643590901911454), 110: (0.9540446486006966, 3.0), 120: (0.6339745962155618, 3.0), + 130: (0.24135055323408017, 3.0), 140: (0.0, 3.2413505532340796), 150: (0.0, 3.6339745962155616), + 160: (0.0, 3.9540446486006964), 170: (0.0, 4.2355095289373015), 180: (3.0, 4.5), 190: (3.0, 4.235509528937302), + 200: (3.0, 3.9540446486006964), 210: (5.0, 2.479274057836309), 220: (5.0, 1.56315129087952), + 230: (3.597749077943201, 2.0), 240: (2.9999999999999996, 1.9019237886466875), 250: (3.1378660541979135, 0.0), + 260: (2.293471413188092, 0.0), 270: (1.5, 5.0), 280: (1.5881634903542323, 5.0), 290: (1.6819851171331015, 5.0), + 300: (1.788675134594813, 5.0), 310: (1.91954981558864, 5.0), 320: (2.0958767962971048, 5.0), + 330: (2.3660254037844375, 5.0), 340: (2.8737387097273084, 5.0), 350: (3.0, 4.764490471062698)}, + {0: (0.0, 4.5), 10: (0.0, 4.940817451771163), 20: (1.1262612902726887, 5.0), 30: (1.6339745962155612, 5.0), + 40: (1.9041232037028952, 5.0), 50: (2.08045018441136, 5.0), 60: (2.211324865405187, 5.0), + 70: (2.318014882866899, 5.0), 80: (2.4118365096457675, 5.0), 90: (2.5, 0.0), 100: (1.7065285868119078, 0.0), + 110: (1.0000000000000002, 0.37878387081806686), 120: (1.0000000000000002, 1.901923788646683), + 130: (0.9999999999999998, 2.7123696111086844), 140: (0.712369611108686, 3.0), 150: (0.0, 3.056624327025936), + 160: (0.0, 3.5900744143344934), 170: (0.0, 4.059182548228836), 180: (3.0, 4.5), 190: (3.0, 4.4118365096457675), + 200: (3.0, 4.318014882866899), 210: (3.0, 4.211324865405187), 220: (3.0, 4.08045018441136), + 230: (5.0, 1.520616018514477), 240: (3.9433756729740663, 2.0), 250: (3.4099255856655075, 2.0), + 260: (3.0000000000000004, 1.6643590901911445), 270: (2.5, 5.0), 280: (2.588163490354232, 5.0), + 290: (2.681985117133101, 5.0), 300: (2.7886751345948126, 5.0), 310: (2.91954981558864, 5.0), + 320: (3.0, 4.91954981558864), 330: (3.0, 4.788675134594813), 340: (3.0, 4.6819851171331015), + 350: (3.0, 4.5881634903542325)}] + alpha_diff = 10 + for position, correct_scan in zip(positions, correct_scans): + x, y, _ = position + scan = generate(x, y, local_map, alpha_diff) + assert scan == correct_scan diff --git a/testing.py b/testing.py new file mode 100644 index 0000000..21ee06b --- /dev/null +++ b/testing.py @@ -0,0 +1,32 @@ +from scans_generator import generate +from map_model import Map +from vizualization import Robot, RobotsVis, Window +from data_utils import get_iterator + + +def main(): + + local_map = Map(debug=True, density=0.55) + robot_viz = RobotsVis() + window = Window(local_map.array) + positions = local_map.get_free_positions() + scans = [] + for x_robot, y_robot, orientation in positions: + scans.append(generate(x_robot, y_robot, local_map, alpha_diff=10)) + print(scans) + scan_it = get_iterator(scans) + scan = next(scan_it) + + positions = local_map.get_free_positions() + positions_it = get_iterator(positions) + x_robot, y_robot, orientation = next(positions_it) + + robot = Robot(window, x_robot, y_robot, orientation, scan) + robot_viz.add_robot(robot) + + window.add_buttons(scan_it, positions_it, robot_viz) + + window.run() + + +main() \ No newline at end of file diff --git a/testing_2d_scan_matching.py b/testing_2d_scan_matching.py new file mode 100644 index 0000000..fbd5760 --- /dev/null +++ b/testing_2d_scan_matching.py @@ -0,0 +1,58 @@ +from math import cos, sin, sqrt, pi + + +import numpy as np + +laser_scans_example = np.asarray([[4, 1, 1, 4], [2, 3, 3, 3], + [3, 1, 2, 4], [1, 4, 5, 1], + [1, 1, 3, 5], [2, 2, 3, 1], + [1, 4, 2, 2], [3, 1, 3, 4], ]) + + +def get_example_map(): + return np.asarray([ + [1, 0, 0, 0, 0, 0, 0], + [1, 0, 0, 1, 0, 0, 0], + [1, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 1, 1, 1], + [0, 0, 0, 0, 0, 0, 1], + [0, 0, 0, 1, 1, 0, 0], + [0, 0, 0, 1, 1, 0, 0]]) + + + + + +def check_map(x, y, axis, simple_map, value): + len_to_occup_cell = get_params_occupied_cell(x, y, axis, simple_map) + return value == len_to_occup_cell + + +def main(): + simple_map = get_example_map() + results = [] + scan = {0: 0, 1: 90, 2: 180, 3: 270} + rotations_count = 4 + + for scan_number, laser_scan in enumerate(laser_scans_example): + for rotation in range(rotations_count): + # print('scan number: {}'.format(scan_number)) + for y, row in enumerate(simple_map): + for x, cell_value in enumerate(row): + # for alpha in range(0, 360, 90): + fit = 0 + for axis in range(len(laser_scan)): + value = laser_scan[(axis - rotation) % len(laser_scan)] + if check_map(x, y, axis, simple_map, value): + fit += 1 + if fit == len(scan): + results.append( + ('x: {}, y: {}, rotation: {}, laser scan: {}'.format(x, y, scan[rotation], laser_scan))) + + for number, result in enumerate(results): + print('#{}'.format(number), result) + + +# main() + + diff --git a/vizualization.py b/vizualization.py new file mode 100644 index 0000000..5ea4f5e --- /dev/null +++ b/vizualization.py @@ -0,0 +1,332 @@ +import math +import queue +import tkinter as tk +import threading +from logger import Logger +from sh import tail +from config import get_filename + + +ray_statuses = {True: 'Убрать лучи', False: 'Показать лучи'} +orient_statuses = {True: 'Убрать ориентацию в пространстве', False: 'Показать ориентацию в пространстве'} + + +class Window: + + def __init__(self, local_map, name='Карта', padding=20): + self.window = tk.Tk() + self.window.configure(bg='white') + self.window.title(name) + + self.logger = Logger.get_instance() + + self.padding = padding + + screen_width = self.window.winfo_screenwidth() + screen_height = self.window.winfo_screenheight() + + width, height = 1000, 500 + array_length = len(local_map) + + x = (screen_width // 2) - (width // 2) + y = (screen_height // 2) - (height // 2) + self.window.geometry(f"{width}x{height}+{x}+{y}") + + width_grid, height_grid = height, height + width_control, height_control = width - width_grid, height + self.cell_size = height_grid // array_length - self.padding + + self.grid_frame = tk.Frame(self.window, height=height_grid, width=width_grid, bg='white') + self.control_frame = tk.Frame(self.window, height=height_control, width=width_control, bg='white') + + self.grid_frame.grid(row=0, column=0, sticky='nsew') + self.control_frame.grid(row=0, column=1, sticky='nsew') + + self.__create_grid_frame(local_map, width_grid, height_grid) + self.__create_control_frame(width_control, height_control) + self.window.update_idletasks() + self.window.grid_columnconfigure(0, weight=0) + self.window.grid_columnconfigure(1, weight=1) + + self.logger.info('Application starts') + + def run(self): + self.window.mainloop() + + def __draw_map(self, local_map, label_size): + size = len(local_map) + for y in range(size): + for x in range(size): + if local_map[y][x] == 1: + color = "black" + else: + color = "white" + self.canvas.create_rectangle(x * self.cell_size + self.padding + label_size, + y * self.cell_size + self.padding, + (x + 1) * self.cell_size + self.padding + label_size, + (y + 1) * self.cell_size + self.padding, + fill=color) + + def __create_grid_frame(self, local_map, width_grid, height_grid): + size = len(local_map) + label_size = 1 + self.canvas = tk.Canvas(self.grid_frame, width=width_grid, height=height_grid, bg="white") + self.canvas.grid(row=0, column=0, sticky="nsew") + self.__draw_map(local_map, label_size) + + for i in range(size): + y_coordinate = self.cell_size * i + self.cell_size // 2 + self.__draw_axis_label(str(i), label_size, self.padding // 2, + y_coordinate + self.padding) + + for i in range(size): + x_coordinate = self.cell_size * i + self.cell_size // 2 + self.padding + self.__draw_axis_label(str(i), label_size, x_coordinate, size * self.cell_size + self.padding + label_size) + + def __draw_axis_label(self, text, size, x, y): + label = tk.Label(self.grid_frame, text=text, width=size, height=size) + label.place(x=x, y=y, anchor='n') + + def __create_buttons(self): + self.button_next = tk.Button(self.top_control_frame, text="Следующая позиция", bg="white", wraplength=180) + self.button_show_rays = tk.Button(self.top_control_frame, text="Убрать лучи", bg="white", wraplength=180) + self.button_show_normal = tk.Button(self.top_control_frame, text="Убрать ориентацию в пространстве", bg="white", + wraplength=180) + + self.button_next.pack(side=tk.TOP, pady=10) + self.button_show_rays.pack(side=tk.TOP, pady=10) + self.button_show_normal.pack(side=tk.TOP, pady=10) + + def __create_control_frame(self, width_control, height_control): + self.window.grid_columnconfigure(0, weight=2) + self.window.grid_columnconfigure(1, weight=1) + + height_top = height_control // 2 + height_bottom = height_control - height_top + + self.top_control_frame = tk.Frame(self.control_frame, width=width_control, height=height_top, bg='white') + self.top_control_frame.pack(fill='both') + + self.bottom_control_frame = tk.Frame(self.control_frame, width=width_control, height=height_bottom, bg='white') + self.bottom_control_frame.pack(fill='both') + + self.__create_buttons() + + self.__create_log(width_control) + + def __create_log(self, width_control): + scrollbar = tk.Scrollbar(self.bottom_control_frame) + scrollbar.pack(side=tk.RIGHT, fill=tk.Y) + self.log_text = tk.Text(self.bottom_control_frame, + width=width_control, + yscrollcommand=scrollbar.set) + self.log_text.pack(fill=tk.BOTH, expand=1) + scrollbar.config(command=self.log_text.yview) + + self.log_lines = queue.Queue() + self.thread = threading.Thread(target=self.__update_log_widget) + self.thread.daemon = True + self.thread.start() + self.__log_bind() + + def on_next_robots_button_click(self, scans, positions, robot_viz): + robot_viz.clear_robots(self) + x, y, orientation = next(positions) + scan = next(scans) + RobotsVis.current_scan = scan + robot = Robot(self, x, y, orientation, scan) + robot_viz.add_robot(robot) + + def on_show_rays_button_click(self, robot_viz): + robot_viz.change_ray_status() + status = robot_viz.get_ray_status() + self.button_show_rays.config(text=ray_statuses[status]) + if status: + for robot in robot_viz.robots: + robot.show_rays(self) + else: + for robot in robot_viz.robots: + robot.hide_rays(self) + + def on_show_normal_button_click(self, robot_viz): + robot_viz.change_orient_status() + status = robot_viz.get_orient_status() + self.button_show_normal.config(text=orient_statuses[status]) + if status: + for robot in robot_viz.robots: + robot.show_orientation(self) + else: + for robot in robot_viz.robots: + robot.hide_orientation(self) + + def add_buttons(self, scans_generator, positions_generator, robot_viz): + self.button_next.config( + command=lambda: self.on_next_robots_button_click(scans_generator, + positions_generator, + robot_viz)) + self.button_show_rays.config(command=lambda: + self.on_show_rays_button_click(robot_viz)) + self.button_show_normal.config(command=lambda: + self.on_show_normal_button_click(robot_viz)) + + def log_widget_config(self): + self.log_text.config(state='disabled') + self.log_text.tag_config("INFO", foreground="black") + self.log_text.tag_config("DEBUG", foreground="grey") + self.log_text.tag_config("WARNING", foreground="orange") + self.log_text.tag_config("ERROR", foreground="red") + self.log_text.tag_config("CRITICAL", foreground="red", underline=1) + + def __update_log_widget(self): + filename = get_filename() + for line in tail("-F", " -c 1", filename, _iter=True): + self.log_lines.put(line) + self.window.event_generate('<>') + + def __update_log(self): + self.log_text.config(state='normal') + self.log_text.insert(tk.END, self.log_lines.get()) + self.log_text.see(tk.END) + self.log_text.config(state='disabled') + + def __log_bind(self): + self.window.bind('<>', lambda e: self.__update_log()) + + +class Robot: + + def __init__(self, window, x, y, orientation, scan): + self.x = x + self.y = y + self.orientation = orientation + self.rays = [] + self.__create_robot(window, scan) + self.logger = Logger.get_instance() + self.logger.info("New robot position {}".format(self)) + print("New robot position {}".format(self)) + + def __str__(self): + return 'x: {}, y: {}, angle: {}'.format(self.x, self.y, self.orientation) + + def __create_robot(self, window, scan): + self.circle_coords = ( + self.x * window.cell_size + window.padding, + self.y * window.cell_size + window.padding) + + self.circle = window.canvas.create_oval(self.circle_coords[0] - window.cell_size // 4, + self.circle_coords[1] - window.cell_size // 4, + self.circle_coords[0] + window.cell_size // 4, + self.circle_coords[1] + window.cell_size // 4, + fill='red', outline='black') + + self.draw_orientation(window) + self.draw_rays(window, scan) + if not RobotsVis.orient_status: + self.hide_orientation(window) + if not RobotsVis.ray_status: + self.hide_rays(window) + + @staticmethod + def convert_to_map_coords(window, x, y): + return x * window.cell_size + window.padding, y * window.cell_size + window.padding + + def draw_rays(self, window, scan): + radius = 3 + for angle in scan: + x, y = self.convert_to_map_coords(window, *scan[angle]) + self.rays.append(window.canvas.create_line(*self.circle_coords, x, y, fill='blue')) + self.rays.append(window.canvas.create_oval( + x - radius, y - radius, x + radius, y + radius, fill='red')) + + def delete_rays(self, window): + for ray in self.rays: + window.canvas.delete(ray) + + def draw_orientation(self, window): + main_line_coords, dashed_line_coords, angle_text_coords = self.calculate_line_orientation_coords( + self.orientation, + window.cell_size, + self.circle_coords) + self.main_line = window.canvas.create_line(*main_line_coords, fill='black') + self.dashed_line = window.canvas.create_line(*dashed_line_coords, fill='black', dash=(4, 4)) + self.angle_text = window.canvas.create_text(*angle_text_coords, text=f"{self.orientation}°", + font=('Arial', 10), + anchor='nw') + + def delete_orientation(self, window): + window.canvas.delete(self.main_line) + window.canvas.delete(self.dashed_line) + window.canvas.delete(self.angle_text) + self.main_line = False + self.dashed_line = False + self.angle_text = False + + def delete_robot(self, window): + window.canvas.delete(self.circle) + if RobotsVis.orient_status: + self.delete_orientation(window) + if RobotsVis.ray_status: + self.delete_rays(window) + + def show_rays(self, window): + for ray in self.rays: + window.canvas.itemconfigure(ray, state='normal') + + def hide_rays(self, window): + for ray in self.rays: + window.canvas.itemconfigure(ray, state='hidden') + + def show_orientation(self, window): + window.canvas.itemconfigure(self.main_line, state='normal') + window.canvas.itemconfigure(self.dashed_line, state='normal') + window.canvas.itemconfigure(self.angle_text, state='normal') + + def hide_orientation(self, window): + window.canvas.itemconfigure(self.main_line, state='hidden') + window.canvas.itemconfigure(self.dashed_line, state='hidden') + window.canvas.itemconfigure(self.angle_text, state='hidden') + + @staticmethod + def calculate_line_orientation_coords(orientation, cell_size, circle_coords): + rad = math.radians(orientation - 90) + line_length = cell_size // 2 + + x_start, y_start = circle_coords + x_orientation, y_orientation = x_start - line_length * math.sin(rad), y_start - line_length * math.cos(rad) + + x_dashed = x_start + line_length + + angle_text_x, angle_text_y = x_start + 4, y_start - line_length // 2 + + return (x_start, y_start, x_orientation, y_orientation), (x_start, y_start, x_dashed, y_start), ( + angle_text_x, angle_text_y) + + +class RobotsVis: + orient_status = True + ray_status = True + current_scan = False + + def __init__(self): + self.robots = [] + + def add_robot(self, robot): + self.robots.append(robot) + + def get_orient_status(self): + return RobotsVis.orient_status + + def change_orient_status(self): + RobotsVis.orient_status = not RobotsVis.orient_status + + def get_ray_status(self): + return RobotsVis.ray_status + + def change_ray_status(self): + RobotsVis.ray_status = not RobotsVis.ray_status + + def clear_robots(self, window): + for robot in self.robots: + robot.delete_robot(window) + self.robots = [] +