diff --git a/lib/graphbuild.py b/lib/graphbuild.py index acdf390..b525af7 100644 --- a/lib/graphbuild.py +++ b/lib/graphbuild.py @@ -18,12 +18,12 @@ def graphbuild(d_racks_segments, d_rack_conn_points): G.add_node(end) # Knoten für Endpunkt G.add_edge(start, end) # Kante zwischen Start und Endpunkt - # 4. Verbindungen aus d_rack_conn_points hinzufügen - for connection, points in d_rack_conn_points.items(): - for point in points: - # Wir fügen die Kante zwischen den Punkten der Verbindung hinzu - # Hinweis: Wir nehmen hier an, dass der Punkt von beiden Racks als Verbindungspunkt betrachtet wird - G.add_edge(point, point) # Verbindungspunkt als Kante + # # 4. Verbindungen aus d_rack_conn_points hinzufügen + # for connection, points in d_rack_conn_points.items(): + + # # Wir fügen die Kante zwischen den Punkten der Verbindung hinzu + # # Hinweis: Wir nehmen hier an, dass der Punkt von beiden Racks als Verbindungspunkt betrachtet wird + # G.add_edge(point, point) # Verbindungspunkt als Kante @@ -41,13 +41,13 @@ class TestShapely(unittest.TestCase): } d_rack_conn_points = { - 'Rack_2-0 + Rack_2-1': [Point(0.1, 3777.6)], - 'Rack_2-1 + Rack_1-0': [Point(4946.5, 3777.6)], - 'Rack_2-1 + Rack_4-0': [Point(2866.6, 3777.6)], - 'Rack_2-1 + Rack_5-0': [Point(8866.1, 3777.6)], - 'Rack_3-0 + Rack_1-0': [Point(4946.5, 15865.5)], - 'Rack_3-0 + Rack_4-0': [Point(2866.6, 15865.5)], - 'Rack_3-0 + Rack_5-0': [Point(8866.1, 15865.5)] + 'Rack_2-0 + Rack_2-1': Point(0.1, 3777.6), + 'Rack_2-1 + Rack_1-0': Point(4946.5, 3777.6), + 'Rack_2-1 + Rack_4-0': Point(2866.6, 3777.6), + 'Rack_2-1 + Rack_5-0': Point(8866.1, 3777.6), + 'Rack_3-0 + Rack_1-0': Point(4946.5, 15865.5), + 'Rack_3-0 + Rack_4-0': Point(2866.6, 15865.5), + 'Rack_3-0 + Rack_5-0': Point(8866.1, 15865.5) } G = graphbuild(d_racks_segments, d_rack_conn_points) @@ -70,18 +70,25 @@ class TestShapely(unittest.TestCase): nodes = G.nodes - - - self.assertEqual(str(G.nodes), nodes_str) - self.assertEqual(str(G.edges), edges_str) - allids = NodeIDs(nodes) - ids = allids.get_ids(nodes) + for k,v in d_racks_segments.items(): + allids.add_points(v) + + for k,v in d_rack_conn_points.items(): + allids.add_point(v) + + + #ids = allids.get_ids(nodes) + + #cords = allids.get_points(ids) - self.assertEqual(len(ids), len(nodes)) - + # self.assertEqual(len(ids), len(nodes)) + + + # self.assertEqual(cords,nodes_str) + # self.assertEqual(allids.get_point(1), Point(4946.5, 15865.5)) # 6. Optional: Visualisierung des Graphen @@ -92,7 +99,8 @@ class TestShapely(unittest.TestCase): #nx.draw(G, pos, with_labels=False, node_size=10, font_size=8) #plt.show() - + def test_easy(self): + pass diff --git a/lib/linesweep_circle.py b/lib/linesweep_circle.py index 7cc6e3c..6fed19f 100644 --- a/lib/linesweep_circle.py +++ b/lib/linesweep_circle.py @@ -68,7 +68,10 @@ class RackIDs(): def add_point_to_rack(self, point:Point, name:str): - pass + if point in self._point2rack: + self._point2rack[point].append(name) + else: + self._point2rack[point] = [name] def get_racks_from_point(self, point:Point) -> list[str]: return self._point2rack[point] @@ -95,8 +98,7 @@ class RackIDs(): return True else: return False - - + class PointIndex2D: def __init__(self): @@ -196,6 +198,41 @@ def increase_circle(tol, tol_step, line, pt, rack_id, idx, other_rack_id, other_ break radius += tol_step +def find_nearest_rack_from_sensor(max_dist, coarse_step, sensor:Point, racks:dict) -> tuple[Point, str]: + # 1. grobe Kandidatensuche + candidate_lines = [] + radius = coarse_step + rack_lines = dict() + while radius <= max_dist: + circle = sensor.buffer(radius) + for r_name, pts in racks.items(): + line = LineString([pts[0], pts[-1]]) #Linestring aus erstem und letzten Eintrag in Rack dict erzeugen + if circle.intersects(line): + candidate_lines.append((r_name, line)) + if candidate_lines: + break + radius += coarse_step + + if not candidate_lines: + return None, None + + # 2. Feinbestimmung über Distanz + candidates_distance = [ + (r_name, line, line.distance(sensor)) + for r_name, line in candidate_lines + ] + + # Sortieren nach Abstand + candidates_distance.sort(key=lambda x: x[2]) + '''# Theoretisch könnten mehrere ähnlich naheliegende Racks zurückgegeben werden.''' + r_best, line_best, _ = candidates_distance[0] # Hier wird nur das tatsächlich dem Senso nächste Rack gegriffen + + # Aufpunkt bestimmen + nearest_point = line_best.interpolate(line_best.project(sensor)) + + return (nearest_point, r_best) + + # === 3. Verbindungen suchen === @@ -357,12 +394,48 @@ class TestLinesweep(unittest.TestCase): 'Rack_2-1': [Point(0, 10), Point(5, 10)]} - allids = RackIDs(res_rack_seg) + point2rack = RackIDs(res_rack_seg) - self.assertEqual(allids.get_racks_from_point(Point(1, 0)), ["Rack_1-0", "Rack_2-0"]) - self.assertEqual(allids.get_racks_from_point(Point(5, 6)), ["Rack_1-0"]) - self.assertEqual(allids.get_points_from_rack("Rack_2-0"), [Point(1, 0), Point(1, 8)]) + self.assertEqual(point2rack.get_racks_from_point(Point(1, 0)), ["Rack_1-0", "Rack_2-0"]) + self.assertEqual(point2rack.get_racks_from_point(Point(5, 6)), ["Rack_1-0"]) + self.assertEqual(point2rack.get_points_from_rack("Rack_2-0"), [Point(1, 0), Point(1, 8)]) + def test_add_point_interim(self): + + res_rack_seg = {'Rack_1-0': [Point(1, 0), Point(5, 6)], + 'Rack_2-0': [Point(1, 8), Point(1, 0)], + 'Rack_2-1': [Point(0, 10), Point(5, 10)]} + + + point2rack = RackIDs(res_rack_seg) + point2rack.add_point_to_rack(Point(1,4), "Rack_2-0") + + self.assertEqual(point2rack.get_points_from_rack("Rack_2-0"), [Point(1, 0), Point(1,4), Point(1, 8)]) + + def test_add_sensor(self): + + rack_segs = {'Rack_1-0': [Point(0, 0), Point(0, 10)], + 'Rack_2-0': [Point(10, -2), Point(10, 5)], + 'Rack_2-1': [Point(0, 3), Point(10, 3)]} + + sensors = {'Sens_1': Point(1, 1), + 'Sens_2': Point(2, 4), + 'Sens_3': Point(9, 2)} + + point2rack = RackIDs(rack_segs) + + sensor_points = {} + for s, p in sensors.items(): + onpoint, rack_name = find_nearest_rack_from_sensor(2, 0.5, p, rack_segs) + sensor_points[s] = ( onpoint, rack_name) + point2rack.add_point_to_rack(onpoint, rack_name) + + plist = point2rack.get_points_from_rack("Rack_1-0") + + self.assertEqual(plist, [Point(0, 0), Point(0,1), Point(0, 10)]) + + +