Initial Alignment Algorithm integration.

Not tested.

src/TheAntFarm/controller/controller_control.py

@@ -6,7 +6,7 @@
 import random
 import numpy as np
 from collections import OrderedDict, deque
-from shape_core.gcode_manager import GCoder, GCodeParser, GCodeLeveler
+from shape_core.gcode_manager import GCoder, GCodeParser, GCodeLeveler, GCodeAlignment
 
 logger = logging.getLogger(__name__)
 
@@ -39,6 +39,7 @@ def __init__(self, settings):
         self.abl_updated = False
         self.prb_val = deque([[-1.0, -1.0, -1.0], [-1.0, -1.0, -1.0]], maxlen=2)
         self.abl_val = []
+        self.align_data = []
         self.abl_steps = ()
         self.abl_cmd_ls = []
         self.prb_num_todo = 0
@@ -54,6 +55,9 @@ def __init__(self, settings):
     def get_probe_value(self):
         return self.prb_val[0]
 
+    def get_align_data(self):
+        return self.align_data
+
     def get_abl_value(self):
         return self.abl_val
 
@@ -279,6 +283,21 @@ def get_gcode_tag_and_v(self, gcode_path):
         tag = self.gcodes_od[gcode_path]["tag"]
         return tag, v
 
+    def apply_alignment(self, gcode_path):
+        print("Apply Alignment")
+        gcp = self.get_gcode_gcp(gcode_path)
+        align = GCodeAlignment(gcp.gc)
+        align.update_align_info(self.align_data.copy())
+        align.apply_align()
+
+    def remove_alignment(self, gcode_path):
+        gcp = self.get_gcode_gcp(gcode_path)
+        if gcp.gc.aligned_vectors:
+            gcp.gc.aligned_vectors = []
+            return True
+        else:
+            return False
+
     def apply_abl(self, gcode_path):
         print("Apply ABL")
         gcp = self.get_gcode_gcp(gcode_path)
@@ -288,8 +307,6 @@ def apply_abl(self, gcode_path):
         abl.get_grid_data(abl_val, self.abl_steps, last_probe, self.wco_a)
         abl.interp_grid_data()
         abl.apply_abl()
-        # print("Leveled")
-        # print(gcp.gc.modified_vectors)
 
     def remove_abl(self, gcode_path):
         gcp = self.get_gcode_gcp(gcode_path)

src/TheAntFarm/controller/controller_manager.py

@@ -67,6 +67,7 @@ def __init__(self, serial_rx_queue, serial_tx_queue, settings):
         self.dro_status_updated = False
 
         self.abl_apply_active = True
+        self.align_apply_active = True
         self.prb_activated = False
         self.abl_activated = False
         self.prb_updated = False
@@ -364,6 +365,11 @@ def set_abl_active(self, abl_active=True):
         self.abl_apply_active = abl_active
         self.select_active_gcode(self.active_gcode_path)
 
+    def set_align_active(self, align_active=True):
+        self.align_apply_active = align_active
+        self.select_active_gcode(self.active_gcode_path)
+
+
     def vectorize_new_gcode_file(self, gcode_path):
         self.control_controller.load_gcode_file({}, gcode_path)
         self.gcode_vectorized_s.emit(gcode_path)
@@ -372,16 +378,31 @@ def select_active_gcode(self, gcode_path):
         self.active_gcode_path = gcode_path
         redraw = False
         visible = True
+
         abl_val = self.control_controller.get_abl_value()
         logger.debug("ABL_val " + str(abl_val))
         logger.debug("ABL_active " + str(self.abl_apply_active))
+
+        align_data = self.control_controller.get_align_data()
+        logger.debug("Align_val " + str(abl_val))
+        logger.debug("Align_active " + str(self.align_apply_active))
+
+        if align_data != [] and self.align_apply_active:
+            logger.debug("Apply Alignment")
+            self.control_controller.apply_alignment(gcode_path)
+            redraw_align = True
+        else:
+            logger.debug("Remove ABL")
+            redraw_align = self.control_controller.renove_alignment(gcode_path)
+
         if abl_val != [] and self.abl_apply_active:
             logger.debug("Apply ABL")
             self.control_controller.apply_abl(gcode_path)
-            redraw = True
+            redraw_abl = True
         else:
             logger.debug("Remove ABL")
-            redraw = self.control_controller.remove_abl(gcode_path)
+            redraw_abl = self.control_controller.remove_abl(gcode_path)
+        redraw = redraw_abl or redraw_align
         logger.debug("ABL Done")
         (tag, v) = self.control_controller.get_gcode_tag_and_v(gcode_path)
         self.update_gcode_s.emit(tag, v, visible, redraw)

src/TheAntFarm/shape_core/align_manager.py

@@ -105,6 +105,9 @@ def load_sampled_points(self, sampled_points):
         self.sampled_points = sampled_points
         self.update_tps_coefficients()
 
+    def is_sampled_point_loaded(self):
+        return self.sampled_points != []
+
     def update_tps_coefficients(self):
         self.tps_coeff = TpsCoefficients(self.sampled_points)
 

src/TheAntFarm/shape_core/gcode_manager.py

@@ -9,6 +9,7 @@
 from shapely.geometry import LineString
 from .macros_manager import Macros
 from .commands_manager import CommandManager
+from .align_manager import AlignManager
 
 
 class GCoder:
@@ -638,6 +639,7 @@ def __init__(self, lines):
         self.modified_lines = []
         self.gcll = []
         self.original_vectors = []
+        self.aligned_vectors = []
         self.modified_vectors = []
         self.bb = None
 
@@ -820,16 +822,49 @@ def get_gcode(self):
     def get_gcode_original_vectors(self):
         return self.gc.original_vectors
 
+    def get_gcode_original_vectors_copy(self):
+        govc = [p.copy() for p in self.gc.original_vectors]
+        return govc
+
     def get_gcode_vectors(self):
         if self.gc.modified_vectors:
             return self.gc.modified_vectors
         else:
-            return self.gc.original_vectors
+            if self.gc.aligned_vectors:
+                return self.gc.aligned_vectors
+            else:
+                return self.gc.original_vectors
 
     def get_bbox(self):
         return self.gc.bb
 
 
+class GCodeAlignment:
+    def __init__(self, gc):
+        self.gc = gc
+        self.am = AlignManager()
+
+    def update_align_info(self, align_point_data):
+        self.am.load_sampled_points(align_point_data)
+
+    def apply_align(self):
+        print("GCode Alignment Start")
+        align_flag = True
+        if self.gc is not None and self.am.is_sampled_point_loaded():
+            coords = [p.coords.copy() for p in self.gc.original_vectors]
+            new_coords = self.am.compute_points_transform(coords)
+            avl = self.gc.get_gcode_original_vectors_copy()
+            for i, p in enumerate(avl):
+                p.coords = new_coords[i]
+            self.gc.aligned_vectors = avl
+            align_flag = True
+        else:
+            self.gc.aligned_vectors = []
+            align_flag = False
+        print("GCode Alignment Stop")
+        return align_flag
+
+
 class GCodeLeveler:
 
     def __init__(self, gc, grid_data=None):
@@ -925,7 +960,13 @@ def apply_abl(self):
             pre_pc = None
             min_step = min(self.grid_step)
             print("Min Step ", min_step)
-            for p in self.gc.original_vectors:
+
+            if self.gc.aligned_vectors:
+                data = self.gc.aligned_vectors
+            else:
+                data = self.gc.original_vectors
+
+            for p in data:
                 np_l = []
                 nwp = p.copy()
                 delta = self.ig(nwp.coords[0], nwp.coords[1])
@@ -934,7 +975,7 @@ def apply_abl(self):
                     pc = np.array(nwp.coords[:2])
                     seg_len = np.linalg.norm(pc - pre_pc)
                     sub_line = 0
-                    if seg_len <= min_step and seg_len > 0.1:
+                    if min_step >= seg_len > 0.1:
                         i_l = self.get_grid_intersection(pc, pre_pc)
                         if i_l:
                             for i in i_l: