Алгоритм search_features и заполнение списка фич

controller/core_logic/lapshin_algorithm/binding_probe_to_feature.py

@@ -79,6 +79,12 @@ def return_to_feature(self, feature: Feature) -> Tuple[int, int]:
     def set_stop(self, is_stop: bool) -> None:
         self.stop = is_stop
 
+    def jumping(self, current_feature: Feature, next_feature: Feature) -> None:
+        self.binding_in_delay.wait()
+        self.allow_binding.clear()
+        #todo вычислять точный вектора до фич и усреднять вектор
+        pass
+
     def __update_feature_coord(self, feature: Feature, figure, x_correction, y_correction, feature_height, actual_center):
         feature.set_coordinates(
             feature.coordinates[0] + x_correction,

controller/core_logic/lapshin_algorithm/binding_probe_to_feature_interface.py

@@ -20,3 +20,7 @@ def return_to_feature(self, feature: Feature) -> Tuple[int, int]:
     @abstractmethod
     def set_stop(self, is_stop: bool) -> None:
         pass
+
+    @abstractmethod
+    def jumping(self, current_feature: Feature, next_feature: Feature) -> None:
+        pass

controller/core_logic/lapshin_algorithm/feature_searcher.py

@@ -1,11 +1,15 @@
 import threading
 from threading import Event
+from typing import Tuple
+
 import numpy as np
 from controller.core_logic.lapshin_algorithm.binding_probe_to_feature_interface import BindingProbeToFeatureInterface
+from controller.core_logic.lapshin_algorithm.entity.feature import Feature
 from controller.core_logic.lapshin_algorithm.feature_collection.doubly_linked_list import DoublyLinkedList
 from controller.core_logic.lapshin_algorithm.service.recognition.feature_recognizer_interface import \
     FeatureRecognizerInterface
 from controller.core_logic.lapshin_algorithm.service.scanner_around_feature import ScannerAroundFeature
+from controller.core_logic.lapshin_algorithm.service.vector_operations import VectorOperations
 from controller.core_logic.service.scanner_interface import ScannerInterface
 
 """
@@ -37,6 +41,8 @@
 
 class FeatureSearcher:
 
+    COS_QUARTER_PI = 0.7071
+
     def __init__(self,
                  binding_to_feature: BindingProbeToFeatureInterface,
                  feature_scanner: ScannerInterface,
@@ -61,61 +67,85 @@ def search_features(self):
         while self.structure_of_feature.count < 5:
             self.binding_in_delay.wait()
             self.allow_binding.clear()
-            surface = self.scanner_around_feature.scan_aria_around_feature(self.structure_of_feature.get_current_feature(), 5)
+            surface = self.scanner_around_feature.scan_aria_around_feature(self.structure_of_feature.get_current_feature(), 5) #todo выделить в рекурсивную функцию
             self.allow_binding.set()
-            self.find_next_feature(surface)
+            try:
+                next_feature = self.find_next_feature(surface)
+            except RuntimeError:
+                self.binding_in_delay.wait()
+                self.allow_binding.clear()
+                surface = self.scanner_around_feature.scan_aria_around_feature(self.structure_of_feature.get_current_feature(), 7)
+                self.allow_binding.set()
+                next_feature = self.find_next_feature(surface)
+            current_feature = self.structure_of_feature.get_current_feature()
+            self.binding_to_feature.jumping(current_feature, next_feature)
 
     def find_first_feature(self, figure: np.ndarray, surface: np.ndarray):
         feature = self.feature_recognizer.recognize_feature(figure, surface)
         self.structure_of_feature.insert_to_end(feature)
         self.binding_to_feature.set_current_feature(feature)
         threading.Thread(target=self.binding_to_feature.bind_to_current_feature).start()
 
-    def find_next_feature(self, surface: np.ndarray):
-        vectors_to_neighbors = self.__calc_vectors_to_neighbors(surface)
+    def find_next_feature(self, surface: np.ndarray) -> Feature:
+        current, neighbors = self.__get_figures_center(surface)
+        current_center = list(current.keys())[0]
+        neighbors_center = list(neighbors.keys())
+        vectors_to_neighbors = VectorOperations.calc_vectors_to_neighbors(current_center, neighbors_center)
         next_direction = self.structure_of_feature.get_next_direction()
+        vector_to_next_feature = self.__find_close_vector(vectors_to_neighbors, next_direction)
+        if vector_to_next_feature in None:
+            raise RuntimeError('next feature not found')
 
-        self.find_close_vector(vectors_to_neighbors, next_direction)
+        next_feature = self.feature_recognizer.recognize_feature(
+            neighbors[(current_center[0] + vector_to_next_feature[0], current_center[1] + vector_to_next_feature[1])],
+            surface
+        )
+        current_feature = self.structure_of_feature.get_current_feature()
+        if current_feature is not None:
+            current_feature: Feature
+            current_feature.vector_to_next = vector_to_next_feature
 
-        # расчитать угол между векторами направления движения и найденной фичой
-        # если угол > 90 то надо увеличивать аппертуру, чтобы искать дургие фичи
-        pass
-        # self.feature_scanner.go_in_direction()
+        return next_feature
 
     def bind_to_nearby_feature(self) -> None:  # в случае ошибки при сильном смещении
         pass
         # получить координаты из self.get_val_func
         # делать сканы по кругу и в каждом искать фичу
 
-    def __calc_vectors_to_neighbors(self, surface) -> np.ndarray:
+    def __get_figures_center(self, surface) -> Tuple[dict, dict]:
         figure_gen = self.feature_recognizer.recognize_all_figure_in_aria(surface.copy())
-        neighbor_centers = []
-        current_center = None
+        neighbors = {}
+        current = None
         for figure in figure_gen:
-            if self.feature_recognizer.feature_in_aria(self.feature_scanner.get_scan_aria_center(surface), figure):
-                current_center = self.feature_recognizer.get_center(figure)
+            if self.feature_recognizer.feature_in_aria(self.feature_scanner.get_scan_aria_center(surface), figure):  # todo вычислять все ценрры и потом current_center тот который ближе всех к центра скана
+                current = {self.feature_recognizer.get_center(figure): figure}
                 continue
-            neighbor_centers.append(self.feature_recognizer.get_center(figure))
-        if current_center is None:
+            neighbors[self.feature_recognizer.get_center(figure)] = figure
+        if current is None:
             raise RuntimeError('center not found')
-        if len(neighbor_centers) == 0:
+        if len(neighbors) == 0:
             raise RuntimeError('neighbor not found')
-        vectors = np.array([[0, 0]], dtype='int8')
-        for neighbor_center in neighbor_centers:
-            vectors = np.append(vectors, [
-                [
-                    neighbor_center[0] - current_center[0],
-                    neighbor_center[1] - current_center[1]
-                ]
-            ], axis=0)
-        return np.delete(vectors, 0, 0)
-
-    def __calc_vectors_angle(self, v1: np.ndarray, v2: np.ndarray) -> float:
-        dot_pr = v1.dot(v2)
-        norms = np.linalg.norm(v1) * np.linalg.norm(v2)
-
-        return np.arccos(dot_pr / norms)
-
-    def find_close_vector(self, vectors_to_neighbors: np.ndarray, next_direction: np.ndarray) -> np.ndarray:
+        return current, neighbors
+
+    def __find_close_vector(self, vectors_to_neighbors: np.ndarray, next_direction: np.ndarray) -> np.ndarray or None:
+        """
+        диапазон допустимых углов углов
+            cos(0) - cos(pi/4)
+            cos(7pi/4) - cos(2pi)
+        """
+        optimal_angle = None
+        result = None
         for vector in vectors_to_neighbors:
-            angle = self.__calc_vectors_angle(next_direction, vector)
+            angle = VectorOperations.calc_vectors_cos_angle(next_direction, vector)
+            if angle < self.COS_QUARTER_PI:
+                continue
+            if optimal_angle is None:
+                optimal_angle = angle
+                result = vector
+                continue
+            if angle > optimal_angle:
+                optimal_angle = angle
+                result = vector
+        return result
+
+

controller/core_logic/lapshin_algorithm/service/recognition/lapshin_feature_recognizer.py

@@ -79,7 +79,7 @@ def __reset_to_zero_feature_area(self, figure: np.ndarray, surface: np.ndarray):
             fig1 = figtr[1][figtr[0] == st]
             surface[np.min(fig1):np.max(fig1) + 1, st] = 0
 
-    def __bypass_feature(self, start_point: Tuple[int, int], optimal_height: int, surface: np.ndarray) -> np.ndarray:
+    def __bypass_feature(self, start_point: Tuple[int, int], optimal_height: int, surface: np.ndarray) -> np.ndarray:  #todo кандидат чтобы переписать в go
         x_start = start_point[0]
         y_start = start_point[1]
         points = np.array([[0, 0]], dtype='int8')
@@ -111,7 +111,7 @@ def __is_vector_entry(self, arr: np.ndarray, entry: np.ndarray) -> bool:
     def __is_start_point(self, val: int) -> bool:
         return val > self.__optimal_height
 
-    def __gen_bypass_point(self, point: Tuple[int, int]):
+    def __gen_bypass_point(self, point: Tuple[int, int]): #todo кандидат чтобы переписать в go
         """
             обход 8-ми точек против часовой от стартовой
         """

controller/core_logic/lapshin_algorithm/service/scanner_around_feature.py

@@ -8,6 +8,9 @@ class ScannerAroundFeature:
     def __init__(self, feature_scanner: ScannerInterface):
         self.feature_scanner = feature_scanner
 
+    def scan_aria_around_current_position(self) -> np.ndarray:
+        pass
+
     def scan_aria_around_feature(self, feature: Feature, rad_count: int) -> np.ndarray:
         x_min, x_max, y_min, y_max = self.__calc_aria_borders(feature, rad_count)
         return self.feature_scanner.scan_aria(x_min, y_min, x_max, y_max)

controller/core_logic/lapshin_algorithm/service/vector_operations.py

@@ -0,0 +1,23 @@
+import numpy as np
+
+
+class VectorOperations:
+
+    @staticmethod
+    def calc_vectors_to_neighbors(current_center: tuple, neighbors_center: list) -> np.ndarray:
+        vectors = np.array([[0, 0]], dtype='int8')
+        for center in neighbors_center:
+            vectors = np.append(vectors, [
+                [
+                    center[0] - current_center[0],
+                    center[1] - current_center[1]
+                ]
+            ], axis=0)
+        return np.delete(vectors, 0, 0)
+
+    @staticmethod
+    def calc_vectors_cos_angle(v1: np.ndarray, v2: np.ndarray) -> float:
+        dot_pr = v1.dot(v2)
+        norms = np.linalg.norm(v1) * np.linalg.norm(v2)
+
+        return dot_pr / norms