Обработка ошибок

controller/core_logic/lapshin_algorithm/binding_probe_to_feature.py

@@ -5,6 +5,7 @@
 import numpy as np
 from controller.core_logic.lapshin_algorithm.entity.feature import Feature
 from controller.core_logic.lapshin_algorithm.binding_probe_to_feature_interface import BindingProbeToFeatureInterface
+from controller.core_logic.lapshin_algorithm.exception.loss_current_feature_exception import LossCurrentFeatureException
 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
@@ -50,7 +51,7 @@ def bind_to_current_feature(self) -> None:
         while not self.stop:
             self.allow_binding.wait()
             try:
-                correction = self.return_to_feature(self.current_feature)
+                correction = self.return_to_feature(self.current_feature) #todo обработать LossCurrentFeatureException
                 self.__correct_delay(*correction)
             except Exception as e:
                 self.stop = True
@@ -63,26 +64,30 @@ def bind_to_current_feature(self) -> None:
     def set_current_feature(self, feature: Feature) -> None:
         self.current_feature = feature
 
-    def return_to_feature(self, feature: Feature) -> Tuple[int, int]:
+    def return_to_feature(self, feature: Feature, try_count=1) -> Tuple[int, int]:
+        if try_count > 2:
+            raise LossCurrentFeatureException
         _surface = self.scanner_around_feature.scan_aria_around_current_position(int(round(feature.max_rad)) * 3)
         feature_height = self.feature_recognizer.get_max_height(_surface.copy())
         figure_gen = self.feature_recognizer.recognize_all_figure_in_aria(_surface)
         scan_center = self.scanner.get_scan_aria_center(_surface)
         scan_center_int = tuple(int(item) for item in scan_center)
-        for figure in figure_gen:
-            if self.feature_recognizer.feature_in_aria(scan_center_int, figure):
-                actual_center = self.feature_recognizer.get_center(figure)
-                correction = self.__calc_correction(actual_center, scan_center)
-                self.__update_feature_coord(feature, figure, correction, feature_height, actual_center)
-                self.scanner.go_to_direction(np.asarray((*correction, feature_height - feature.max_height)))
-                return correction
+        try:
+            for figure in figure_gen:
+                if self.feature_recognizer.feature_in_aria(scan_center_int, figure):
+                    actual_center = self.feature_recognizer.get_center(figure)
+                    correction = self.__calc_correction(actual_center, scan_center)
+                    self.__update_feature_coord(feature, figure, correction, feature_height, actual_center)
+                    self.scanner.go_to_direction(np.asarray((*correction, feature_height - feature.max_height)))
+                    return correction
+        except ValueError:
+            self.return_to_feature(feature, try_count+1)
         print(_surface) #todo логировать
-        raise RuntimeError('feature not found')
+        raise LossCurrentFeatureException
 
     def jumping(self, current_feature: Feature, next_feature: Feature, jump_count: int) -> None:
         vector = current_feature.vector_to_next
         on_next_feature = False
-        # sleep(self.delay/4)
         for i in range(jump_count):
             if i % 2 == 0:
                 feature = next_feature
@@ -100,12 +105,15 @@ def jumping(self, current_feature: Feature, next_feature: Feature, jump_count: i
 
     def __refine_vector_to_feature(self, vector, feature, contribution_coefficient: int) -> np.ndarray:
         self.scanner.go_to_direction(vector)
-        x_correction, y_correction = self.return_to_feature(feature)
+        try:
+            x_correction, y_correction = self.return_to_feature(feature)
+        except LossCurrentFeatureException:
+            raise LossCurrentFeatureException('in jumping')
         #todo логирование correction
         # print(x_correction, y_correction)
         vector[0] += x_correction / contribution_coefficient
         vector[1] += y_correction / contribution_coefficient
-        # print(vector) #todo логирование vector
+        #todo логирование vector
         return vector
 
     def set_stop(self, is_stop: bool) -> None:

controller/core_logic/lapshin_algorithm/feature_searcher.py

@@ -5,6 +5,10 @@
 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.exception.loss_current_feature_exception import LossCurrentFeatureException
+from controller.core_logic.lapshin_algorithm.exception.neighbors_not_found_exception import NeighborsNotFoundException
+from controller.core_logic.lapshin_algorithm.exception.next_feature_not_found_exception import \
+    NextFeatureNotFoundException
 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
@@ -63,11 +67,11 @@ def search_features(self, surface: np.ndarray):
         self.find_first_feature(surface)
         while self.structure_of_feature.count < 6:
             current_feature = self.structure_of_feature.get_current_feature()
-            next_feature = self.recur_find_next_feature(current_feature, 5)
+            next_feature = self.recur_find_next_feature(current_feature, 5) #todo обработать NextFeatureNotFoundException
 
             self.binding_in_delay.wait()
             self.allow_binding.clear()
-            self.binding_to_feature.jumping(current_feature, next_feature, 5)
+            self.binding_to_feature.jumping(current_feature, next_feature, 5) #todo обработать LossCurrentFeatureException
 
             self.structure_of_feature.insert_to_end(next_feature)
             self.binding_to_feature.set_current_feature(next_feature)
@@ -76,14 +80,17 @@ def search_features(self, surface: np.ndarray):
 
     def recur_find_next_feature(self, current_feature: Feature, rad_count: int):
         if rad_count > 7:
-            raise RuntimeError('next feature not found')
+            raise NextFeatureNotFoundException
         self.binding_in_delay.wait()
         self.allow_binding.clear()
         surface = self.scanner_around_feature.scan_aria_around_current_position(current_feature.max_rad * rad_count)
         self.allow_binding.set()
         try:
             next_feature = self.find_next_feature(surface)
         except RuntimeError:
+            self.allow_binding.set()
+            self.binding_in_delay.wait()
+            self.allow_binding.clear()
             next_feature = self.recur_find_next_feature(current_feature, rad_count + 2)
         return next_feature
 
@@ -111,10 +118,14 @@ def go_to_feature_more_accurate(self, feature: Feature, rad_count: int):
         surface_for_accurate = self.scanner_around_feature.scan_aria_around_current_position(
             feature.max_rad * rad_count)
         # todo логирование print('=========_surface===========')
-        actual, _ = self.__get_figures_center(surface_for_accurate.copy())
+        try:
+            actual, _ = self.__get_figures_center(surface_for_accurate.copy())
+        except ValueError as e:
+            print(surface_for_accurate)
+            raise e
         actual_center = list(actual.keys())[0]
         aria_center = self.scanner.get_scan_aria_center(surface_for_accurate)
-        vector_to_center = VectorOperations.get_vector_between_to_point(actual_center, aria_center)
+        vector_to_center = VectorOperations.get_vector_between_to_point(aria_center, actual_center)
         z_current = self.scanner.get_current_position()[2]
         vector_to_center = np.append(vector_to_center, feature.max_height - z_current)
         self.scanner.go_to_direction(vector_to_center)
@@ -127,40 +138,46 @@ def __get_first_feature(self, surface):
         return feature
 
     def find_next_feature(self, surface: np.ndarray) -> Feature:  # TODO сделать рефакторинг функции, выделить приватный метод
-        current, neighbors = self.__get_figures_center(surface.copy())
+        try:
+            current, neighbors = self.__get_figures_center(surface.copy())
+        except ValueError as e:
+            print(surface)
+            raise e
         print(neighbors)
         if len(neighbors) == 0:
-            raise RuntimeError('neighbors not found')
+            raise NeighborsNotFoundException
         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()
         print(next_direction)
         vector_to_next_feature = self.__find_close_vector(vectors_to_neighbors, next_direction)
         if vector_to_next_feature is None:
-            raise RuntimeError('next feature not found')
-
-        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
-        )
+            raise NextFeatureNotFoundException
+        try:
+            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
+            )
+        except ValueError:
+            raise NextFeatureNotFoundException
         current_feature = self.structure_of_feature.get_current_feature()
         vector_to_next_feature = np.append(vector_to_next_feature, next_feature.max_height - current_feature.max_height)
         next_feature.vector_to_prev = VectorOperations.get_reverse_vector(vector_to_next_feature)
         if current_feature is not None:
             current_feature: Feature
             current_feature.vector_to_next = vector_to_next_feature
-
         return next_feature
 
-    def bind_to_nearby_feature(self) -> None:  # в случае ошибки при сильном смещении
+    def bind_to_nearby_feature(self) -> None:  # todo в случае LossCurrentFeatureException
         pass
         # получить координаты из self.get_val_func
         # делать сканы по кругу и в каждом искать фичу
 
     """
     :return current: dict{figure_center:tuple : figure:np.ndarray}
     :return neighbors: dict{figure_center:tuple : figure:np.ndarray}
+    :raise ValueError
     """
     def __get_figures_center(self, surface) -> Tuple[dict, dict]:
         figure_gen = self.feature_recognizer.recognize_all_figure_in_aria(surface.copy())

controller/core_logic/lapshin_algorithm/service/feature_factory.py

@@ -8,7 +8,7 @@ class FeatureFactory:
 
     @staticmethod
     def create(feature_perimeter: int, max_rad: float, *coord) -> Feature:
-        if (feature_perimeter < MAX_ATOM_SIZE):
+        if feature_perimeter < MAX_ATOM_SIZE:
             feature = Atom(coord)
         else:
             feature = Feature(coord)

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

@@ -29,7 +29,7 @@ def recognize_all_figure_in_aria(self, surface: np.ndarray) -> Iterator[np.ndarr
                 if self.__is_start_point(val, optimal_height):
                     try:
                         figure = self.recognize_figure((x, y), surface, optimal_height)
-                    except IndexError as e:
+                    except (IndexError, ValueError) as e:
                         print(e)
                         continue
                     except RuntimeError as e:
@@ -92,7 +92,7 @@ def __bypass_feature(self, start_point: Tuple[int, int], optimal_height: int, su
         points = np.array([[0, 0]], dtype='int8')
         x, y = x_start, y_start
         x_prev, y_prev = x_start, y_start
-        max_iterations = 500
+        max_iterations = 50
         i = 0
         while not self.__is_vector_entry(points, np.array([x_start, y_start], dtype='int8')):
             i += 1

tests/core_logic/lapshin_algorithm/test_binding_probe_to_feature.py

@@ -1,6 +1,6 @@
 import sys, os
 
-
+from controller.core_logic.lapshin_algorithm.exception.loss_current_feature_exception import LossCurrentFeatureException
 from controller.core_logic.lapshin_algorithm.service.vector_operations import VectorOperations
 root = sys.path[1]
 path = os.path.join(root, "stub_microcontroller")
@@ -65,7 +65,7 @@ def test_feature_not_found(self) -> None:
         any_val = 1
         self.binding_probe_to_feature.scanner.get_val_func = MagicMock(side_effect=[9, 9, 27, any_val, any_val, any_val, any_val, any_val, any_val, any_val, any_val, any_val, any_val])
 
-        self.assertRaises(RuntimeError, self.binding_probe_to_feature.return_to_feature, feature)
+        self.assertRaises(LossCurrentFeatureException, self.binding_probe_to_feature.return_to_feature, feature)
 
     def test_jumping(self) -> None:
         vector_to_next_atom = np.array([6.0, 6.0, 24.0])