Skip to content

Commit f5c84b2

Browse files
knicholson32knicholson32
committed
Added the previous position to the guess list. This greatly improved the accuracy of the system.
1 parent 22ce148 commit f5c84b2

File tree

3 files changed

+42
-74
lines changed

3 files changed

+42
-74
lines changed

‎helpers/direct_triangulation.py‎

Lines changed: 12 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -4,20 +4,21 @@
44

55

66
class DirectTriangulation:
7-
def __init__(self, meas1, meas2, res1=None, res2=None, title=None, multi_pipe=None):
7+
def __init__(self, meas1, meas2, res1=None, res2=None, empty=False, title=None, multi_pipe=None):
88
self.meas1 = meas1
99
self.meas2 = meas2
1010
self.res1 = res1
1111
self.res2 = res2
1212
self.title = title
1313
self.multi_pipe = multi_pipe
14-
if self.res1 is None:
15-
self.res1 = self.meas1.get_resolved_node()
16-
if self.res2 is None:
17-
self.res2 = self.meas2.get_resolved_node()
18-
if self.res1 is None or self.res1 is False or self.res2 is None or self.res2 is False:
19-
print("---ERROR--- Both measurements require a resolved node, or resolved nodes must be provided.")
20-
return
14+
if empty is False:
15+
if self.res1 is None:
16+
self.res1 = self.meas1.get_resolved_node()
17+
if self.res2 is None:
18+
self.res2 = self.meas2.get_resolved_node()
19+
if self.res1 is None or self.res1 is False or self.res2 is None or self.res2 is False:
20+
print("---ERROR--- Both measurements require a resolved node, or resolved nodes must be provided.")
21+
return
2122
self.guess_list = []
2223

2324
# Triangulate two guesses from the provided measurements
@@ -68,14 +69,14 @@ def display(self):
6869
"args":{
6970
"x":guess.x,
7071
"y":guess.y,
71-
"r":0.5,
72+
"r":150,
7273
"dashed":False,
7374
"outline":"",
7475
"fill":"red"
7576
}
7677
})
7778

78-
def displayTriangulation(self, resolved_position):
79+
def display_triangulation(self, resolved_position):
7980
# print("dt displayTriangulation")
8081
if self.multi_pipe is None:
8182
return
@@ -171,10 +172,8 @@ def get_radius(self):
171172

172173
# Display the cluster on the UI
173174
def display(self, ghost=False):
174-
print("cluster display")
175175
if self.multi_pipe is None:
176176
return
177-
print("cluster display 2")
178177
if self.pure is False:
179178
self.getRadius()
180179
for p in self._points:
@@ -183,7 +182,7 @@ def display(self, ghost=False):
183182
"args":{
184183
"x": p.x,
185184
"y": p.y,
186-
"r": 0.2,
185+
"r": 100,
187186
"dashed": False,
188187
"outline": "",
189188
"fill":"red"
@@ -192,7 +191,6 @@ def display(self, ghost=False):
192191
if ghost is True:
193192
# point_obj['args']['fill'] = "#4678a1"
194193
point_obj['args']['fill'] = "orange"
195-
print("cluster display send")
196194
self.multi_pipe.send(point_obj)
197195
main_obj = {
198196
"cmd":"draw_circle",

‎helpers/node.py‎

Lines changed: 28 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
import config
33
from algorithms.helpers.measurement import Measurement
44
from algorithms.algorithm import Vector2
5+
from algorithms.helpers.direct_triangulation import DirectTriangulation
56

67

78
class Node:
@@ -48,17 +49,6 @@ def set_pipe(self, pipe_in):
4849
self.piping = True
4950

5051
def start_new_cycle(self):
51-
# for other_node in self.communicate_list:
52-
# key = other_node.id
53-
# if key not in self.measurement_history:
54-
# # Add a new list to keep track of measures from this new node
55-
# self.measurement_history[key] = []
56-
# if self.measurements:
57-
# for meas in self.measurements:
58-
# if meas.node1 == self:
59-
# self.measurement_history[key].append(meas)
60-
# if len(self.measurement_history[key]) > config.MAX_HISTORY:
61-
# self.measurement_history[key].pop(0)
6252
self.measurements = []
6353
self.triangulate_list = []
6454
if not self.is_base:
@@ -90,37 +80,9 @@ def add_measurement(self, node_b, dist, std=None):
9080
# self.add_to_communicate_list(node_b)
9181
print(f"Discarded meas with {dist} distance to node {node_b.id} due to bounding error.")
9282

93-
# def add_to_communicate_list(self, node_b):
94-
# if node_b not in self.communicate_list:
95-
# self.communicate_list.append(node_b)
96-
# key = node_b.id
97-
# if key not in self.measurement_history:
98-
# # Add a new list to keep track of measures from this new node
99-
# self.measurement_history[key] = []
100-
10183
def is_resolved(self):
10284
return self.resolved
10385

104-
# def set_position(self, x, y):
105-
# avg = self.get_avg_position()
106-
# cur_pos = Vector2(avg[0], avg[1])
107-
# new_pos = Vector2(x, y)
108-
# offset = cur_pos - new_pos
109-
# d = offset.magnitude()
110-
# if d <= Node.max_move_per_cycle:
111-
# self.x = x
112-
# self.y = y
113-
# else:
114-
115-
# self.x = avg[0]
116-
# self.y = avg[1]
117-
# self.extrapolate = True
118-
119-
# self.resolved = True
120-
121-
# self.position_history.append((x, y))
122-
# if len(self.position_history) > Node.max_history:
123-
# self.position_history.pop(0)
12486

12587
def get_last_position(self):
12688
if len(self.position_history) == 0:
@@ -154,38 +116,44 @@ def get_triangulations(self):
154116

155117
def display_triangulations(self):
156118
for dt in self.triangulate_list:
157-
dt.displayTriangulation((self.x, self.y))
119+
dt.display_triangulation((self.x, self.y))
158120

159121
def set_position_vec(self, pos):
160-
last = self.get_last_position()
161-
if last is None:
162-
last_pos = pos
163-
else:
164-
last_pos = Vector2(last[0], last[1])
165-
offset = last_pos - pos
166-
d = offset.magnitude()
122+
# last = self.get_last_position()
123+
# if last is None:
124+
# last_pos = pos
125+
# else:
126+
# last_pos = Vector2(last[0], last[1])
127+
# offset = last_pos - pos
128+
# d = offset.magnitude()
167129

168130
# print(d)
169131

132+
# if d <= Node.max_move_per_cycle:
133+
self.x = pos.x
134+
self.y = pos.y
135+
# else:
136+
# vel = self.get_avg_velocity()
137+
# pos = Vector2(last[0], last[1])
138+
# new_pos = pos + vel
139+
# self.x = new_pos.x
140+
# self.y = new_pos.y
141+
# self.extrapolate = True
142+
170143
self.position_history.append((pos.x, pos.y))
171144
self.added_position = True
172145
if len(self.position_history) > Node.max_history:
173146
self.position_history.pop(0)
174147

175-
if d <= Node.max_move_per_cycle:
176-
self.x = pos.x
177-
self.y = pos.y
178-
else:
179-
# avg = self.get_avg_position()
180-
vel = self.get_avg_velocity()
181-
pos = Vector2(last[0], last[1])
182-
new_pos = pos + vel
183-
self.x = new_pos.x
184-
self.y = new_pos.y
185-
self.extrapolate = True
186-
187148
self.resolved = True
188149

150+
def consider_last_location(self):
151+
last = self.get_last_position()
152+
if last is not None:
153+
dt = DirectTriangulation(None, None, None, None, empty=True)
154+
dt.directly_add_guess(Vector2(last[0], last[1]))
155+
self.add_triangulation(dt)
156+
189157
def get_position(self):
190158
return self.x, self.y
191159

@@ -266,7 +234,7 @@ def show(self):
266234
"cmd": "draw_circle",
267235
"args": {
268236
"fill": "white",
269-
"r": 0.5,
237+
"r": 200,
270238
"tags": ['node'],
271239
"outline": "",
272240
"x": self.x,

‎multi_tri/multi_tri.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -157,6 +157,7 @@ def process(self, callback, multi_pipe=None):
157157
if node in self.resolved_nodes:
158158
continue
159159
# print(node)
160+
node.consider_last_location()
160161
dts = node.get_triangulations()
161162
# print(dts)
162163
# 1 direct triangulation object with 1 guess only
@@ -238,6 +239,7 @@ def process(self, callback, multi_pipe=None):
238239
if best_cluster is not None and best_cluster.get_radius() <= self.config['max_cluster_radius']:
239240
# Resolved!!
240241
best_cluster.display()
242+
print(f"R: {best_cluster.get_radius()}")
241243
loc = best_cluster.get_center()
242244
if self.multi_pipe is not None:
243245
for point in best_cluster.get_points():

0 commit comments

Comments
 (0)