I'm trying to set Dijkstra and A* algorithm in same scene. I've added Quadcopter's target object threaded child script. When the simulation begins, codes can draw dijkstra and a* shortest paths. But when i press the follow buttons i got abort execuiton errors. How can i fix that ?
Code: Select all
#python
import time
import heapq
import math
# Heuristik fonksiyon: Manhattan mesafesi
def heuristic(node, target_node):
x1, y1 = node
x2, y2 = target_node
return abs(x1 - x2) + abs(y1 - y2)
# A* Algoritmas?
def a_star(graph, start_node, target_node):
distances = {node: float('inf') for node in graph}
previous_nodes = {node: None for node in graph}
distances[start_node] = 0
priority_queue = [(0, start_node)]
while priority_queue:
current_distance, current_node = heapq.heappop(priority_queue)
if current_node == target_node:
break
for neighbor, weight in graph[current_node]:
g_cost = current_distance + weight
h_cost = heuristic(neighbor, target_node)
f_cost = g_cost + h_cost
if g_cost < distances[neighbor]:
distances[neighbor] = g_cost
previous_nodes[neighbor] = current_node
heapq.heappush(priority_queue, (f_cost, neighbor))
path = []
current = target_node
while current is not None:
path.append(current)
current = previous_nodes[current]
path.reverse()
return distances[target_node], path
def create_nodes(grid_size):
nodes = []
for i in range(grid_size + 1):
for j in range(grid_size + 1):
nodes.append((i, j))
return nodes
def create_graph(nodes, grid_size, obstacles=[]):
graph = {}
for node in nodes:
x, y = node
neighbors = []
if x > 0 and (x - 1, y) not in obstacles:
neighbors.append(((x - 1, y), 1))
if x < grid_size and (x + 1, y) not in obstacles:
neighbors.append(((x + 1, y), 1))
if y > 0 and (x, y - 1) not in obstacles:
neighbors.append(((x, y - 1), 1))
if y < grid_size and (x, y + 1) not in obstacles:
neighbors.append(((x, y + 1), 1))
graph[node] = neighbors
return graph
# Dijkstra Algoritmas?
def dijkstra(graph, start_node, target_node):
distances = {node: float('inf') for node in graph}
previous_nodes = {node: None for node in graph}
distances[start_node] = 0
priority_queue = [(0, start_node)]
while priority_queue:
current_distance, current_node = heapq.heappop(priority_queue)
if current_node == target_node:
break
for neighbor, weight in graph[current_node]:
distance = current_distance + weight
if distance < distances[neighbor]:
distances[neighbor] = distance
previous_nodes[neighbor] = current_node
heapq.heappush(priority_queue, (distance, neighbor))
path = []
current = target_node
while current is not None:
path.append(current)
current = previous_nodes[current]
path.reverse()
return distances[target_node], path
# A?a??daki fonksiyon, yolu sahnede ?izmek i?in kullan?lacak
def draw_path(path, color, thickness=5):
drawing_object_handle = sim.addDrawingObject(sim.drawing_lines, thickness, 0, -1, 99999, color)
for i in range(len(path) - 1):
start_node = path[i]
end_node = path[i + 1]
start_pos = [start_node[0] * 0.5, start_node[1] * 0.5, 0.2]
end_pos = [end_node[0] * 0.5, end_node[1] * 0.5, 0.2]
sim.addDrawingObjectItem(drawing_object_handle, start_pos + end_pos)
# Quadcopter'in yolu takip etmesini sa?layan fonksiyon
def follow_path(path, quadcopter_handle):
if not path or len(path) < 2:
print("Hata: Yol gecersiz veya bos.")
return
if quadcopter_handle is None or quadcopter_handle < 0:
print(f"Hata: Gecersiz quadcopter handle: {quadcopter_handle}")
return
steps_per_move = 100
print(f"Quadcopter, yolu takip etmeye basliyor. Handle: {quadcopter_handle}")
def move_along_path(path, steps_per_move):
for i in range(len(path) - 1):
current_node = path[i]
next_node = path[i + 1]
current_x, current_y = current_node
next_x, next_y = next_node
current_pos = [current_x * 0.5, current_y * 0.5, 0.2]
next_pos = [next_x * 0.5, next_y * 0.5, 0.2]
print(f"Segment: {current_pos} -> {next_pos}")
for step in range(steps_per_move):
interpolated_x = current_pos[0] + (next_pos[0] - current_pos[0]) * (step / steps_per_move)
interpolated_y = current_pos[1] + (next_pos[1] - current_pos[1]) * (step / steps_per_move)
interpolated_z = current_pos[2]
interpolated_pos = [interpolated_x, interpolated_y, interpolated_z]
try:
sim.setObjectPosition(quadcopter_handle, -1, interpolated_pos)
except Exception as e:
print(f"Pozisyon ayarlanirken hata: {e}")
return
sim.wait(0.05)
sim.switchThread()
start_node = path[0]
start_x, start_y = start_node
start_pos = [start_x * 0.5, start_y * 0.5, 0.2]
try:
sim.setObjectPosition(quadcopter_handle, -1, start_pos)
print(f"Quadcopter baslangic pozisyonuna yerlestirildi: {start_pos}")
except Exception as e:
print(f"Baslangic pozisyonu ayarlanirken hata: {e}")
return
move_along_path(path, steps_per_move)
reverse_path = path[::-1]
print("Yol tersine takip ediliyor...")
move_along_path(reverse_path, steps_per_move)
print("Yol basariyla tamamlandi.")
def sysCall_init():
global dijkstra_path, astar_path
sim = require('sim')
simUI = require('simUI')
# XML ile UI olu?turma
xml = '''
<ui title="Path Finding and Quadcopter Control" closeable="true" resizeable="false" activate="false">
<group layout="vbox">
<group layout="hbox">
<button text="Dijkstra Path" on-click="onDijkstraPathClick" id="1"/>
<button text="A* Path" on-click="onAStarPathClick" id="2"/>
</group>
<group layout="hbox">
<button text="Follow Dijkstra Path" on-click="onFollowDijkstraPathClick" id="3"/>
<button text="Follow A* Path" on-click="onFollowAStarPathClick" id="4"/>
</group>
</group>
</ui>
'''
simUI.create(xml)
def sysCall_thread():
global dijkstra_path, astar_path
# A* ve Dijkstra hesaplamalar?n? burada ba?lat?yoruz
grid_size = 10
nodes = create_nodes(grid_size)
obstacles = [(5, 9), (3, 9), (1, 5), (7, 5), (4, 2), (8, 2), (4, 7), (0, 0), (0, 6), (2, 3)]
graph = create_graph(nodes, grid_size, obstacles)
start_node = (9, 9)
target_node = (1, 3)
# A* ve Dijkstra algoritmalar?n? burada ?a??r?yoruz
_, dijkstra_path = dijkstra(graph, start_node, target_node)
_, astar_path = a_star(graph, start_node, target_node)
print("A* Path:", astar_path)
print("Dijkstra Path:", dijkstra_path)
# ??lem tamamland?ktan sonra beklemek i?in
sim.wait(0.5) # Sim?lasyonun ilerleyi?ini durdurmamak i?in
pass
def onDijkstraPathClick(ui, data):
global dijkstra_path
draw_path(dijkstra_path, color=[0, 1, 0]) # Ye?il renk
print("Dijkstra yolu cizildi.")
def onAStarPathClick(ui, data):
global astar_path
draw_path(astar_path, color=[1, 0, 0]) # K?rm?z? renk
print("A* yolu cizildi.")
def onFollowDijkstraPathClick(ui, data):
global dijkstra_path
quadcopter_handle = sim.getObject('/Quadcopter')
print("Quadcopter Handle:", quadcopter_handle)
follow_path(dijkstra_path, quadcopter_handle)
print("Dijkstra yolu takip edildi.")
def onFollowAStarPathClick(ui, data):
global astar_path
quadcopter_handle = sim.getObject('/Quadcopter')
print("Quadcopter Handle:", quadcopter_handle)
follow_path(astar_path, quadcopter_handle)
print("A* yolu takip edildi.")