add gen and vis for all angles

README.md

@@ -1 +1,15 @@
-# slam_studio
+# 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`

config.json

@@ -0,0 +1,3 @@
+{
+  "filename": "slam_studio.log"
+}

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]

data_utils.py

@@ -0,0 +1,4 @@
+def get_iterator(iterable):
+    for obj in iterable:
+        yield obj
+

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)
+
+
+

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
+
+
+

requirements.txt

@@ -0,0 +1,3 @@
+numpy
+sh
+pytest

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
+
+