Updated nodes for better time calculations

Author: knicholson32

helpers/measurement.py

@@ -1,17 +1,11 @@
 class Measurement:
-    def __init__(self, node1, node2, dist, err=0):
+    def __init__(self, node1, node2, dist, std=0):
         self.node1 = node1
         self.node2 = node2
         self.dist = dist
-        self.err = err
+        self.std = std
         self.cleared = False
-        self.confidence = 1.0
         self.opinions = 1
-        if self.err == 0 and dist is not None:
-            self.calc_error()
-
-    def calc_error(self):
-        self.err = self.dist * 0.01
 
     def get_resolved_node(self):
         if self.is_resolved():
@@ -44,18 +38,21 @@ def get_node_pair(self):
     def is_resolved(self):
         return self.node1.is_resolved() and self.node2.is_resolved()
 
-    def avg_with_history(self, history):
-        m = [self.dist]
-        for measurements in history:
-            for measurement in measurements:
-                if self == measurement:
-                    m.append(measurement.dist)
-        nm = Measurement(self.node1, self.node2, sum(m) / len(m))
-        nm.opinions = self.opinions
-        return nm
+    # def avg_with_history(self, history):
+    #     m = [self.dist]
+    #     for measurements in history:
+    #         for measurement in measurements:
+    #             if self == measurement:
+    #                 m.append(measurement.dist)
+    #     nm = Measurement(self.node1, self.node2, sum(m) / len(m))
+    #     nm.opinions = self.opinions
+    #     return nm
+
+    def get_std(self):
+        return self.std
 
     def __str__(self):
-        return '{:12s} ->  {:12s}: {:.3f} . {:.3f}'.format(str(self.node1), str(self.node2), self.dist, self.confidence)
+        return '{:12s} ->  {:12s}: {:.3f} . {:.3f}'.format(str(self.node1), str(self.node2), self.dist, self.std)
 
     def __eq__(self, other):
         return self.node1 == other.node1 and self.node2 == other.node2

helpers/node.py

@@ -8,6 +8,8 @@ class Node:
     node_arr = {}
     min_dist = 500    # mm
     max_dist = 1000000 # mm
+    max_history = 10
+    max_move_per_cycle = 5000
 
     def __init__(self, node_id=None, name=None, is_base=False, x=None, y=None, multi_pipe=None):
         if node_id is None:
@@ -20,9 +22,12 @@ def __init__(self, node_id=None, name=None, is_base=False, x=None, y=None, multi
         self.real_x = x  # random.randint(25, 600)
         self.real_y = y  # random.randint(25, 600)
         self.resolved = False
+        self.extrapolate = False
         self.triangulate_list = []
         self.multi_pipe = multi_pipe
         self.communicate_list = []
+        self.position_history = []
+        self.added_position = False
         if self.multi_pipe is not None:
             self.piping = True
         else:
@@ -60,18 +65,25 @@ def start_new_cycle(self):
             self.x = None
             self.y = None
             self.resolved = False
+        self.extrapolate = False
+
+        if self.added_position is False:
+            if len(self.position_history) > 0:
+                self.position_history.pop(0)
+
+        self.added_position = False
         # if self.measurements:
         #     self.measurement_history.append(self.measurements)
         # if len(self.measurement_history) > config.MAX_HISTORY:
         #     self.measurement_history.pop(0)
         # self.measurements = []
 
-    def add_measurement(self, node_b, dist):
+    def add_measurement(self, node_b, dist, std=None):
         if dist > Node.min_dist and dist < Node.max_dist:
             # print(key)
             # print(self.measurement_history)
             # print(self.measurement_history[key])
-            self.measurements.append(Measurement(self, node_b, dist))
+            self.measurements.append(Measurement(self, node_b, dist, std=std))
             # self.add_to_communicate_list(node_b)
         else:
             # self.measurements.append(Measurement(self, node_b, None))
@@ -89,10 +101,50 @@ def add_measurement(self, node_b, dist):
     def is_resolved(self):
         return self.resolved
 
-    def set_position(self, x, y):
-        self.x = x
-        self.y = y
-        self.resolved = True
+    # def set_position(self, x, y):
+    #     avg = self.get_avg_position()
+    #     cur_pos = Vector2(avg[0], avg[1])
+    #     new_pos = Vector2(x, y)
+    #     offset = cur_pos - new_pos
+    #     d = offset.magnitude()
+    #     if d <= Node.max_move_per_cycle:
+    #         self.x = x
+    #         self.y = y
+    #     else:
+            
+    #         self.x = avg[0]
+    #         self.y = avg[1]
+    #         self.extrapolate = True
+        
+    #     self.resolved = True
+
+    #     self.position_history.append((x, y))
+    #     if len(self.position_history) > Node.max_history:
+    #         self.position_history.pop(0)
+
+    def get_last_position(self):
+        if len(self.position_history) == 0:
+            return None
+        return self.position_history[len(self.position_history)-1]
+    
+    def get_avg_position(self):
+        x_sum = 0
+        y_sum = 0
+        counter = 0
+        for pos in self.position_history:
+            x_sum += pos[0]
+            y_sum += pos[1]
+            counter += 1
+        return (x_sum/counter, y_sum/counter)
+    
+    def get_avg_velocity(self):
+        hist_vec = []
+        for pos in self.position_history:
+            hist_vec.append(Vector2(pos[0], pos[1]))
+        vec_sum = Vector2(0, 0)
+        for pos in hist_vec:
+            vec_sum = vec_sum + pos
+        return vec_sum / len(hist_vec)
 
     def add_triangulation(self, tri):
         self.triangulate_list.append(tri)
@@ -105,8 +157,33 @@ def display_triangulations(self):
             dt.displayTriangulation((self.x, self.y))
 
     def set_position_vec(self, pos):
-        self.x = pos.x
-        self.y = pos.y
+        last = self.get_last_position()
+        if last is None:
+            last_pos = pos
+        else:
+            last_pos = Vector2(last[0], last[1])
+        offset = last_pos - pos
+        d = offset.magnitude()
+
+        # print(d)
+
+        self.position_history.append((pos.x, pos.y))
+        self.added_position = True
+        if len(self.position_history) > Node.max_history:
+            self.position_history.pop(0)
+
+        if d <= Node.max_move_per_cycle:
+            self.x = pos.x
+            self.y = pos.y
+        else:
+            # avg = self.get_avg_position()
+            vel = self.get_avg_velocity()
+            pos = Vector2(last[0], last[1])
+            new_pos = pos + vel
+            self.x = new_pos.x
+            self.y = new_pos.y
+            self.extrapolate = True
+        
         self.resolved = True
 
     def get_position(self):
@@ -189,7 +266,7 @@ def show(self):
             "cmd": "draw_circle",
             "args": {
                 "fill": "white",
-                "r": 1,
+                "r": 0.5,
                 "tags": ['node'],
                 "outline": "",
                 "x": self.x,
@@ -199,6 +276,10 @@ def show(self):
         }
         if self.is_base:
             cmd_obj['args']['fill'] = "yellow"
+
+        if self.extrapolate:
+            cmd_obj['args']['fill'] = "purple"
+
         self.multi_pipe.send(cmd_obj)
         # obj = canvas.create_circle(
         #     self.x, self.y, size, fill=fill, outline="", tags=['node'])

multi_tri/multi_tri.py

@@ -16,9 +16,9 @@ def __init__(self, nodes):
         self.resolved_nodes = [nodes['0'], nodes['1']]  # Known
 
         self.config = {
-            'max_cluster_radius': 50,
-            'min_guess_isolation': 250,
-            'min_cluster_difference': 10
+            'max_cluster_radius': 25000, #50,
+            'min_guess_isolation': 72600,
+            'min_cluster_difference': 3000
         }
 
     def process(self, callback, multi_pipe=None):
@@ -72,7 +72,7 @@ def process(self, callback, multi_pipe=None):
         self.resolved_nodes = [self.backup_nodes['0'], self.backup_nodes['1']]  # Known
 
         self.reduce_measure_list()  # Step 1 / Step 3?
-        self.apply_raw_confidence()  # Step2
+        # self.apply_raw_confidence()  # Step2
         self.print_list(self.measure_list, title='Current Meas')
 
         self.piping = False
@@ -83,7 +83,7 @@ def process(self, callback, multi_pipe=None):
             #     "cmd": "clear_screen",
             #     "args": None
             # })
-            # time.sleep(0.01)
+            
 
         # Loop coordination for step 4
         guessing = True
@@ -393,11 +393,11 @@ def reduce_measure_list(self):
 
     # Apply basic confidences based on the distance between nodes and the
     # number of original measurements used
-    def apply_raw_confidence(self):
-        for m in self.measure_list:
-            m.confidence = 0.5
-            for x in range(m.opinions - 1):
-                m.confidence *= 1.1
+    # def apply_raw_confidence(self):
+    #     for m in self.measure_list:
+    #         m.confidence = 0.5
+    #         for x in range(m.opinions - 1):
+    #             m.confidence *= 1.1
 
     ############################### Helpers ###############################