Travelling Salesman update

TSP.py

@@ -0,0 +1,109 @@
+
+from ortools.constraint_solver import routing_enums_pb2
+from ortools.constraint_solver import pywrapcp
+from bfs import distance, bfs
+from house import create_houses
+
+
+def create_data_model(multi_trash, truck):
+    data = {}
+    graphrow = []
+    graph = []
+    lista = multi_trash
+    for i in range(8):
+        list1=multi_trash
+        if (i == 0):
+            pos = truck.pos
+        else:
+            pos = lista[i-1].pos
+        for j in range(8):
+            if(j==0):
+                endpos = truck.pos
+            else:
+                endpos = list1[j-1].pos
+            if(i==j):
+                graphrow.append(0)
+            else:
+                dist=distance(pos, endpos)
+                graphrow.append(dist)
+
+        graph.append(graphrow.copy())
+        graphrow.clear()
+    data['distance_matrix'] = graph.copy()
+    data['num_vehicles'] = 1
+    data['depot'] = 0
+    print(data['distance_matrix'])
+    return data
+
+
+def print_solution(manager, routing, solution):
+    index = routing.Start(0)
+    plan_output = []
+    route_distance = 0
+    while not routing.IsEnd(index):
+        plan_output.append(manager.IndexToNode(index))
+        previous_index = index
+        index = solution.Value(routing.NextVar(index))
+        route_distance += routing.GetArcCostForVehicle(previous_index, index, 0)
+    plan_output.append(manager.IndexToNode(index))
+    print(plan_output)
+    return plan_output
+
+
+def tsp(x, y):
+    data = create_data_model(x, y)
+
+    manager = pywrapcp.RoutingIndexManager(len(data['distance_matrix']),
+                                           data['num_vehicles'], data['depot'])
+
+    routing = pywrapcp.RoutingModel(manager)
+
+
+    def distance_callback(from_index, to_index):
+        from_node = manager.IndexToNode(from_index)
+        to_node = manager.IndexToNode(to_index)
+        return data['distance_matrix'][from_node][to_node]
+
+    transit_callback_index = routing.RegisterTransitCallback(distance_callback)
+
+    routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index)
+
+    search_parameters = pywrapcp.DefaultRoutingSearchParameters()
+    search_parameters.first_solution_strategy = (
+        routing_enums_pb2.FirstSolutionStrategy.PATH_CHEAPEST_ARC)
+
+    solution = routing.SolveWithParameters(search_parameters)
+
+    if solution:
+        sol = print_solution(manager, routing, solution)
+    return sol
+
+def tspmove(order, truck, Ltrash):
+    houses = create_houses(40)
+    path = []
+    endpos=truck.pos
+    for i in range(1, 8):
+        startpos = endpos
+        endpos = Ltrash[order[i]-1].pos
+        #print(i,startpos,endpos)
+        x = bfs(startpos, truck.dir_control, endpos, houses)
+        #print("$$$$",x)
+        for i in x:
+            if(i==97):
+                truck.rotate(-1)
+            elif(i==100):
+                truck.rotate(1)
+        path.append(x)
+    x = bfs(endpos, truck.dir_control, truck.pos, houses)
+    for i in x:
+        if (i == 97):
+            truck.rotate(-1)
+        elif (i == 100):
+            truck.rotate(1)
+    path.append(x)
+    #print("###############################\n", path)
+    truck.dir_control=0
+    truck.direction = [1, 0]
+    return path
+
+

bfs.py

@@ -1,4 +1,4 @@
-from house import is_house
+from house import *
 import pygame
 
 
@@ -46,7 +46,8 @@ def bfs(pos, direction, end_pos, houses):
 
         if not is_house(curr_node.pos, houses) and curr_node.pos == end_pos:
             while curr_node.parent:
-                print(curr_node.pos, end_pos)
+
+                #print(curr_node.pos, end_pos)
                 actions.append(curr_node.action)
                 curr_node = curr_node.parent
             return actions
@@ -57,3 +58,8 @@ def bfs(pos, direction, end_pos, houses):
             if neighbour_node not in visited and neighbour_node not in queue:
                 queue.append(neighbour_node)
     return actions
+
+def distance(pos, endpos):
+    houses = create_houses(40)
+    actions = bfs(pos, 0, endpos, houses)
+    return len(actions)

houses.txt

@@ -1,16 +1,16 @@
 11111111111111111111111111111111
-10000000000000000000000000000001
-10000000000000000000000000000001
-10011111110001000010001111111001
-10000000000001000010000000000001
-10000000000001000010000000000001
-11111111111111000011111111111111
-10000000000000000000000000000001
-10000000000000000000000000000001
-11111111111111000011111111111111
-10000000000001000010000000000001
-10000000000001000010000000000001
-10011111111001000010011111111001
-10000000000000000000000000000001
-10000000000000000000000000000001
+10100011110000111100010000011101
+10100000000000011000000111000001
+10111111110001011010001110011001
+10000000000001000010000100000001
+11111111110001000010000000001111
+11110000000111000100001111111111
+10000000000000000001100000000001
+10001110000000000010000000000001
+11111111111111000010111111111011
+10000000000000000010000001110001
+10000000000001000000111100000011
+10011111111001000010011001111011
+10000110001000000001110000000011
+10010000100000111100000001100001
 11111111111111111111111111111111

main.py

@@ -5,10 +5,10 @@ from colors import gray
 from house import create_houses
 from truck import Truck
 from trash import Trash
-
+from TSP import tsp, tspmove
 from bfs import bfs
 
-
+multi_trash = []
 pygame.init()
 
 
@@ -63,12 +63,11 @@ def game_loop():
     truck = Truck(game_w, game_h, grid_size)
     houses = create_houses(grid_size)
 
-    multi_trash = []
+
     for _ in range(7):
         trash = Trash(game_w, game_h, grid_size)
         trash.new_pos(truck.pos, houses, multi_trash)
         multi_trash.append(trash)
-
     while not gameExit:
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
@@ -94,6 +93,25 @@ def game_loop():
 
                         game_keys(truck, multi_trash, houses, True)
                         update_images(gameDisplay, truck, multi_trash, houses)
+                if (event.key == pygame.K_t):
+                    solution = tsp(multi_trash, truck)
+                    actions = tspmove(solution, truck, multi_trash)
+                    if not actions:
+                        print('Path couldn\'t be found')
+                        break
+
+
+                    for i in actions:
+                        arr=i
+                        #print(arr)
+                        print('##################################################')
+                        while arr:
+                            action = arr.pop()
+                            pygame.event.post(pygame.event.Event(
+                                pygame.KEYDOWN, {'key': action}))
+
+                            game_keys(truck, multi_trash, houses, True)
+                            update_images(gameDisplay, truck, multi_trash, houses)
                 else:
                     pygame.event.post(event)
                     game_keys(truck, multi_trash, houses)