grid_tsp.py

"""Simple travelling salesman problem between cities.
The particular module implements a grid and runs the TSP
in the grid.
"""

from __future__ import print_function
from ortools.constraint_solver import routing_enums_pb2
from ortools.constraint_solver import pywrapcp
from scipy.spatial import distance
import matplotlib.pyplot as plt
import pdb
import time



def create_data_model():
    """Stores the data for the problem."""
    data = {}
    data['distance_matrix'] = [
        [0, 2451, 713, 1018, 1631, 1374, 2408, 213, 2571, 875, 1420, 2145, 1972],
        [2451, 0, 1745, 1524, 831, 1240, 959, 2596, 403, 1589, 1374, 357, 579],
        [713, 1745, 0, 355, 920, 803, 1737, 851, 1858, 262, 940, 1453, 1260],
        [1018, 1524, 355, 0, 700, 862, 1395, 1123, 1584, 466, 1056, 1280, 987],
        [1631, 831, 920, 700, 0, 663, 1021, 1769, 949, 796, 879, 586, 371],
        [1374, 1240, 803, 862, 663, 0, 1681, 1551, 1765, 547, 225, 887, 999],
        [2408, 959, 1737, 1395, 1021, 1681, 0, 2493, 678, 1724, 1891, 1114, 701],
        [213, 2596, 851, 1123, 1769, 1551, 2493, 0, 2699, 1038, 1605, 2300, 2099],
        [2571, 403, 1858, 1584, 949, 1765, 678, 2699, 0, 1744, 1645, 653, 600],
        [875, 1589, 262, 466, 796, 547, 1724, 1038, 1744, 0, 679, 1272, 1162],
        [1420, 1374, 940, 1056, 879, 225, 1891, 1605, 1645, 679, 0, 1017, 1200],
        [2145, 357, 1453, 1280, 586, 887, 1114, 2300, 653, 1272, 1017, 0, 504],
        [1972, 579, 1260, 987, 371, 999, 701, 2099, 600, 1162, 1200, 504, 0],
    ]  # yapf: disable
    data['num_vehicles'] = 1
    data['depot'] = 0
    return data

def create_equal_distance():
    data = {}
    points_matrix = create_grid_points()
    data['distance_matrix'] = calculate_grid_distances(points_matrix)
    data['num_vehicles'] = 1
    data['depot'] = 0
    return data


def create_grid_points():
    points_matrix = []
    for i in range(0,10):
        for j in range(0,10):
            points_matrix.append((i, j))
    print(points_matrix)
    print("points matrix just above!\n")
    return points_matrix

def calculate_grid_distances(points_matrix):
    distance_matrix = []
    row_matrix = []
    for point_a in points_matrix:
        for point_b in points_matrix:
            dst = distance.euclidean(point_a, point_b)
            row_matrix.append(dst)
        distance_matrix.append(row_matrix)
        row_matrix = []
    #print(distance_matrix)
    #print("distance matrix just above!\n")
    return distance_matrix

def return_nodes(manager, routing, solution):
    """returns nodes sorted with the travel."""
    print('Objective: {} miles'.format(solution.ObjectiveValue()))
    index = routing.Start(0)
    plan_output = 'Route for vehicle 0:\n'
    tsp_nodes = []
    route_distance = 0
    while not routing.IsEnd(index):
        plan_output += ' {} ->'.format(manager.IndexToNode(index))
        tsp_nodes.append(manager.IndexToNode(index))
        previous_index = index
        index = solution.Value(routing.NextVar(index))
        route_distance += routing.GetArcCostForVehicle(previous_index, index, 0)
    plan_output += ' {}\n'.format(manager.IndexToNode(index))
    tsp_nodes.append(manager.IndexToNode(index))
    print(plan_output)
    plan_output += 'Route distance: {}miles\n'.format(route_distance)
    #print(tsp_nodes)
    return tsp_nodes


def print_grid_to_map(nodes, tsp_path):
	x_axis = []
	y_axis = []
	# convert points to coords
	for n in nodes:
		x, y = n
		x_axis.append(x)
		y_axis.append(y)
	plt.plot(x_axis, y_axis, 'ro')
	plt.show(block=False)

	for n in tsp_path:
		x, y = nodes[n]
		x_axis.append(x)
		y_axis.append(y)
		print("Node is %s and coords are %s" %(n, nodes[n]))
	for n in range(0, len(tsp_path)-1):
		plt.plot([x_axis[tsp_path[n]], x_axis[tsp_path[n+1]]], [y_axis[tsp_path[n]], y_axis[tsp_path[n+1]]])
		plt.draw()
		plt.pause(0.2)
		#plt.show(block=False)
	#plt.plot(x_axis, y_axis)
	plt.show()



def print_solution(manager, routing, solution):
    """Prints solution on console."""
    print('Objective: {} miles'.format(solution.ObjectiveValue()))
    index = routing.Start(0)
    plan_output = 'Route for vehicle 0:\n'
    route_distance = 0
    while not routing.IsEnd(index):
        plan_output += ' {} ->'.format(manager.IndexToNode(index))
        previous_index = index
        index = solution.Value(routing.NextVar(index))
        route_distance += routing.GetArcCostForVehicle(previous_index, index, 0)
    plan_output += ' {}\n'.format(manager.IndexToNode(index))
    print(plan_output)
    plan_output += 'Route distance: {}miles\n'.format(route_distance)


def main():
    """Entry point of the program."""
    # Instantiate the data problem.
    data = create_equal_distance()  #create_data_model()

    # Create the routing index manager.
    manager = pywrapcp.RoutingIndexManager(len(data['distance_matrix']),
                                           data['num_vehicles'], data['depot'])

    # Create Routing Model.
    routing = pywrapcp.RoutingModel(manager)


    def distance_callback(from_index, to_index):
        """Returns the distance between the two nodes."""
        # Convert from routing variable Index to distance matrix NodeIndex.
        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)

    # Define cost of each arc.
    routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index)

    # Setting first solution heuristic.
    search_parameters = pywrapcp.DefaultRoutingSearchParameters()
    search_parameters.first_solution_strategy = (
        routing_enums_pb2.FirstSolutionStrategy.PATH_CHEAPEST_ARC)

    # Solve the problem.
    solution = routing.SolveWithParameters(search_parameters)

    # Print solution on console.
    if solution:
        print_solution(manager, routing, solution)
        tsp_node_path = return_nodes(manager, routing, solution)
        nodes = create_grid_points()
        print_grid_to_map(nodes, tsp_node_path)


if __name__ == '__main__':
    main()