<repo_name>project-pantheon/pantheon_glob_planner<file_sep>/pantheon_planner/plots_functions.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Tue Jan  8 14:23:33 2019

@author: Jacopo
"""
import os
import matplotlib.pyplot as plt

def getCurrWDir():

    return os.getcwd()

def plotMapTrees(trees_pos,tree_radious):
    
    plt.figure(figsize=(8,6))
    plt.scatter(trees_pos[:,0],trees_pos[:,1],color='g',s=100 * tree_radious)
    
    for i, txt in enumerate(trees_pos[:,2]):
        plt.annotate(txt, (trees_pos[i,0],trees_pos[i,1]))
    
    plt.axis('equal')
    plt.title("Trees positions")
    plt.show()

def plotCapturePts(capture_pos):
    
    plt.figure(figsize=(8,6))
    plt.scatter(capture_pos[:,0],capture_pos[:,1],color='b')
    
    plt.axis('equal')
    plt.title("Captures points positions")
    plt.show()


def plotTreesAndCaptures(trees_pos,capture_pos,tree_radious):
    
    plt.figure(figsize=(8,6))
    ax = plt.gca()
    ax.cla() 
#    plt.scatter(trees_pos[:,0],trees_pos[:,1],color='g', s = 100 * tree_radious)
    plt.scatter(capture_pos[:,0],capture_pos[:,1],color='b')

    for i, txt in enumerate(trees_pos[:,2]):
        circle3 = plt.Circle((trees_pos[i,0], trees_pos[i,1]), tree_radious, color='g', clip_on=False)
        plt.annotate(int(txt), (trees_pos[i,0],trees_pos[i,1]))
        ax.add_artist(circle3)

    for i, txt in enumerate(capture_pos[:,0]):
        plt.annotate(str(i+1), (capture_pos[i,0],capture_pos[i,1]))
    
    plt.axis('equal')
    plt.title("All the points")

    name = "capture_pts_and_trees"
    path = getCurrWDir()

    plt.savefig( path + "/results/"+ name, dpi = 800 ,format = 'pdf')
    plt.show()  
 

def plotPoints(points,edges):
    x = points[:,0].flatten()
    y = points[:,1].flatten()
    fig, ax= plt.subplots(figsize=(10,8))
    #ax.scatter(points[:,0],points[:,1])
    
    num =range(points.shape[0])  
    
    for i,txt in enumerate(num):
        ax.annotate(txt, (points[i,0], points[i,1]),)
    
    plt.plot(x[edges.T], y[edges.T], linestyle='-', color='y')     

def plotWithoutLines(points,centers,n_col,n_row):
    x = points[:,0].flatten()
    y = points[:,1].flatten()
    plt.figure(figsize=(16,12))
    plt.plot(centers[:,0],centers[:,1],'o',color ='g')
    plt.plot(x,y,'o',color ='r')
    name = "points "+str(n_col)+"x"+str(n_row)
    plt.savefig("/home/majo/libs/pyconcorde/pantheon_planner/results/"+name, dpi = 800 ,format = 'pdf')
    

def plotSolutionGraph(capture_pos,edges,trees_pos,filename):
    
    x = capture_pos[:,0]
    y = capture_pos[:,1]

    plt.figure(figsize=(8,6))
    ax = plt.gca()
    ax.cla() 

    plt.scatter(capture_pos[:,0],capture_pos[:,1],color='b')

    for i, txt in enumerate(trees_pos[:,2]):
        circles = plt.Circle((trees_pos[i,0], trees_pos[i,1]), 0.3, color='g', clip_on=False)
        plt.annotate(int(txt), (trees_pos[i,0],trees_pos[i,1]))
        ax.add_artist(circles)

    for i, txt in enumerate(capture_pos[:,0]):
        plt.annotate(str(i+1), (capture_pos[i,0],capture_pos[i,1]))

    plt.plot(x[edges.T], y[edges.T], linestyle='-', color='r') 

    plt.annotate("start/stop", (x[edges[0,0]]-0.5,y[edges[0,0]]-0.5))

    path = getCurrWDir()

    plt.savefig(path + "/results/" + filename, format = 'pdf')

    plt.axis("equal")
    plt.show()


def plotGraph(points,edges,trees_pos,n_col,n_row):
    
    x = points[:,0].flatten()
    y = points[:,1].flatten()

    plt.figure(figsize=(8,6))
    ax = plt.gca()
    ax.cla() 

#    plt.figure(figsize=(12,8))
#    plt.plot(trees_pos[:,0],trees_pos[:,1],'o',color ='g')

    for i, txt in enumerate(trees_pos[:,2]):
        circles = plt.Circle((trees_pos[i,0], trees_pos[i,1]), 0.3, color='g', clip_on=False)
        plt.annotate(txt, (trees_pos[i,0],trees_pos[i,1]))
        ax.add_artist(circles)


    plt.plot(x[edges.T], y[edges.T], linestyle='-', color='y',markerfacecolor='red', marker='o') 
    plt.plot(x,y,'o',color ='r')
    plt.plot(x[edges.T], y[edges.T], color='y',markerfacecolor='red', marker='o') 
    
    name = str(n_col)+"x"+str(n_row)
    path = getCurrWDir()
    plt.savefig(path + "/results/" + name, format = 'pdf')
    

    plt.axis("equal")

    plt.show()
    
def plotGraphLight(points,edges,centers):
    
    x = points[:,0].flatten()
    y = points[:,1].flatten()

    plt.figure(figsize=(10,8))
    plt.plot(centers[:,0],centers[:,1],'o',color ='g')
    plt.plot(x[edges.T], y[edges.T], linestyle='-', color='y',markerfacecolor='red', marker='o') 
 
<file_sep>/README.md
# pantheon_glob_planner


This repository contains the global planner for the sherpa robots of the Pantheon Project.

-----

It includes also the [PyConcorde](https://github.com/jvkersch/pyconcorde) which acts as a Python wrapper around the [Concorde TSP
solver] (http://www.math.uwaterloo.ca/tsp/concorde.html).



<file_sep>/pantheon_planner/run_test.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import numpy as np
from concorde.tsp import TSPSolver
import functions as fc
import plots_functions as pf
import file_reader_writer as frw
import time
import sys
import os
from shapely.geometry import LineString
from shapely.geometry import Point
from termcolor import colored
from collections import defaultdict
from concorde.tests.data_utils import get_dataset_path

np.set_printoptions(threshold=sys.maxsize)

def getCurrWDir():

    return os.getcwd()


def computeTsp(filename):   
     
    solver = TSPSolver.from_tspfile( getCurrWDir() + "/inputs/" + filename)

    start_time = time.time()
    solution = solver.solve()
    eval_time = time.time() - start_time
    
    print( colored("--- %s seconds ---" % eval_time) )

    print( colored("Outcome TSP:","red") )
    print(solution.found_tour)
    print( colored("With optimal value [m]:","red") )
    print(solution.optimal_value/100)

    return solution,eval_time

def distMatrix(n_trees, points,centers): #ASYMMETRIC MATRIX
    dist = np.zeros(shape=[n_trees*4,n_trees*4],dtype=int)    
    for i in range(n_trees*4):
        for j in range(n_trees*4):
           if ((i%4 == 0) and (j == i+3)) or ((i%4 == 1)and(j ==i+1)):
                dist[i][j] = 1000000
                dist[j][i] = 1000000
           else: 
               dist[i][j] = np.round(np.sqrt(np.power(points[i][0]-points[j][0],2)+np.power(points[i][1]-points[j][1],2)))
                
    return dist    

def computeAdjMatrix(n_rows,n_cols,inBetweenRows):

    #given the n° of trees we have distribution based on inBetweenRows 
    if ( inBetweenRows == 1):
        MAP_DIM = ( n_rows + 1 ) * ( n_cols + 1 )
        BLOCK_SIZE = ( n_cols + 1 );
        N_BLOCKS = ( n_rows + 1) ;
        v1 = np.ones( (MAP_DIM - BLOCK_SIZE) );

        COLS_connections = np.add( np.diag(v1,BLOCK_SIZE), np.diag(v1,-BLOCK_SIZE) );

        v2 = np.ones( (BLOCK_SIZE-1) );

        BLOCK = np.add( np.diag(v2,1), np.diag(v2,-1) );

        ROWS_connections = np.zeros((MAP_DIM,MAP_DIM))

        for i in range(N_BLOCKS):
            ROWS_connections[ i*BLOCK_SIZE : (i+1) * BLOCK_SIZE, i*BLOCK_SIZE : (i+1) * BLOCK_SIZE] = BLOCK;
    
        Adj_matrix = np.add( COLS_connections, ROWS_connections );
    
    else:
        MAP_DIM = ( ( n_rows + 1) * n_cols ) + ( ( n_cols + 1) * n_rows )
        
        v1 = np.ones( (MAP_DIM - n_cols) );
        v2 = np.ones( (MAP_DIM - (n_cols + 1) ) );


        for i in range(1,n_rows+1):
            v1[ n_cols + (i-1)*(2*n_cols +1) ] = 0;

        for i in range(1,n_rows):
            v2[2*n_cols+ (i-1)*(2*n_cols +1)] = 0

        upper = np.add( np.diag(v1,n_cols) , np.diag(v2, n_cols + 1) ) 

        lower = np.transpose(upper) 

        Adj_matrix = np.add(upper,lower)


    return Adj_matrix


def fromMatrix2List(A):

    graph = defaultdict(list)
    edges = set()

#    graph2 = defaultdict(list)
#    edges2 = set()

    for i, v in enumerate(A, 1):
        for j, u in enumerate(v, 1):
            if u != 0 and frozenset([i, j]) not in edges:
                edges.add(frozenset([i, j]))
                graph[i].append(j)   # put graph[i].append({j: u}) if you need also to have the weight 
				     # in this case the TSP file writing has to be modified
#    for i, v in enumerate(A, 1):
#        for j, u in enumerate(v, 1):
#            if u != 0 and frozenset([i-1, j-1]) not in edges2:
#                edges2.add(frozenset([i-1, j-1]))
#                graph2[i-1].append(j-1)   # put graph[i].append({j: u}) if you need also to have the weight 
				     # in this case the TSP file writing has to be modified

    return graph


def computeWeightMatrix(Adj,points):

    dist = np.zeros(shape=[Adj.shape[0],Adj.shape[0]],dtype=int)    
    for i in range(Adj.shape[0]):
        for j in range(Adj.shape[0]):
                dist[i][j] = np.round(np.sqrt(np.power(points[i][0]-points[j][0],2)+np.power(points[i][1]-points[j][1],2)))


    #include the tree constraints directly by weight matrix

    return dist


def addConstraints(W, n_rows, n_cols, inBetweenRows):

    if ( inBetweenRows ): 
        for i in range(1,W.shape[0]-(n_cols)):
            if ( i%(n_cols+1) == 1 ):
                W[i-1][i+n_cols+1] = 999
                W[i+n_cols+1][i-1] = 999
            elif ( i%(n_cols+1) == 0 ):
                W[i-1][i+n_cols-1] = 999
                W[i+n_cols-1][i-1] = 999
            else:
                W[i-1][i+n_cols-1] = 999
                W[i-1][i+n_cols+1] = 999
                W[i+n_cols-1][i-1] = 999
                W[i+n_cols+1][i-1] = 999
    else:
        for i in range(n_rows):
            for j in range(n_cols):
                W[i*(2*n_cols+1) + j][(i+1) * (2*n_cols+1) +j] = 999
                W[(i+1) * (2*n_cols+1) +j][i*(2*n_cols+1) + j] = 999

        for i in range(n_cols):
            for j in range(n_rows):
                W[n_cols + j*(2*n_cols+1) + i ][n_cols +j*(2*n_cols+1) +i+1] = 999
                W[n_cols +j*(2*n_cols+1) +i+1][n_cols + j*(2*n_cols+1) + i ] = 999


    return W


def generateTSPFile(namefile,n_tree,distMatrix):
    
    f = open("inputs/"+namefile,"w")
    f.write( "NAME: %s" % namefile )
    f.write( "\nTYPE: TSP" )
    f.write( "\nCOMMENT: waypoints" )
    f.write( "\nDIMENSION: %d" % (n_tree*4) )
    f.write( "\nEDGE_WEIGHT_TYPE: EXPLICIT" )
    f.write( "\nEDGE_WEIGHT_FORMAT: FULL_MATRIX" )
    f.write( "\nEDGE_WEIGHT_SECTION\n" )
    
    
    for i in range(n_tree*4):
        for j in range(n_tree*4):
            s =  str(distMatrix[i][j])+" "
            f.write(s)
        s = "\n"
        f.write(s)
    f.write("EOF")
    f.close()



def main(): 


    params_dict = frw.getParams( 'parameters.yaml' )

    #############
    #store params
    n_rows = params_dict['rows_number']
    n_cols = params_dict['cols_number']
    tree_radious = params_dict['tree_radious']
    dist_row = params_dict['row_distance']
    dist_col = params_dict['col_distance']
    stop_time = params_dict['stop_time']
    inBetweenRows = params_dict['stop_in_between']
    
    MAX_cap_dist = params_dict['maximize_capture_distance']

    obstacles_enabled = params_dict['include_trees_as_obstacles']
   
    #############

 
    # EVEN n_rows and n_cols brings to an ODD number of total stop points, on which the TSP algorithm would fail 
    if ( inBetweenRows ) and (not(n_rows%2) and not(n_cols%2)):
        print("For the selected Lattice an Hamiltonian Path does not exists. Change number of stops.")
        sys.exit()

#    My solution
   
    trees_pos = fc.generateTrees(n_rows,n_cols,dist_row,dist_col)
    
#    pf.plotMapTrees(trees_pos,R)
    if (inBetweenRows == 0):

        capture_pts = fc.generateCapturePts(trees_pos,n_rows,n_cols, dist_row,dist_col) #ok

    else:
        capture_pts = fc.generateinBetweenCapturePts(n_rows,n_cols, dist_row,dist_col) #ok
#    pf.plotCapturePts(capture_pts)

    print(capture_pts.shape[0])
 

    pf.plotTreesAndCaptures(trees_pos,capture_pts,tree_radious)

    Adj_matrix = computeAdjMatrix(n_rows,n_cols,inBetweenRows)

    W = computeWeightMatrix(Adj_matrix,capture_pts)

    if ( obstacles_enabled ):
        W = addConstraints(W,n_rows,n_cols,inBetweenRows)


    #TODO 
    # W = addSteeringWeight()


    Adj_list = fromMatrix2List(Adj_matrix)
    
    frw.generateTSPFileFromAdj(inBetweenRows,capture_pts.shape[0], W*100, Adj_list) #TSP solver takes only int numbers 

    #TSP solver assumption: complete graph.
    soluti, comp_time = computeTsp("Grid" + str(inBetweenRows) + ".tsp")

    edges = fc.formatEdges(soluti.tour)
    
    pf.plotSolutionGraph(capture_pts,edges,trees_pos,"SolGrid" + str(inBetweenRows) + str(n_rows) + "x" + str(n_cols) + ".txt")

    frw.writeOutCome(stop_time,inBetweenRows, n_rows, n_cols, soluti.optimal_value/100 , soluti.tour, comp_time)

    sys.exit()

#CHECK IF AN HAMILTONIAN PATH IS POSSIBLE 

#FILE FORMAT:

#NAME : alb1000
#COMMENT : Hamiltonian cycle problem (Erbacci) 
#TYPE : HCP
#DIMENSION : 1000
#EDGE_DATA_FORMAT : EDGE_LIST
#EDGE_DATA_SECTION
#  1000    593
#  1000    456


   ##### ALESSANDRO ######
    centers = fc.generateCenters(n_rows,n_cols,dist_row,dist_col)

    points_ref = fc.generateRefFrame(centers,R)

    pf.plotCapturePts(points_ref)

    if (inBetweenRows == 0): #in-between trees
        thetas = np.zeros(shape=(1,n_trees))#np.pi/4#+(np.pi/2)*np.random.rand(1,n_trees)
        points = fc.computePoints(thetas, points_ref, centers)
        dist = distMatrix(n_trees,points,centers)

        fo,sol = computeTsp(n_trees,dist,points)
        edges = fc.formatEdges(sol)
        pf.plotGraph(points,edges,centers,n_cols,n_rows)
        pf.plotPoints(points,edges)
        print(colored("inBetweenRows = 0, distance [km] : " +str(fo/molt),"cyan"))
    else:

        thetas45 = np.zeros(shape=(1,n_trees))+3*np.pi/4#+(np.pi/2)*np.random.rand(1,n_trees)
        points45 = fc.computePoints(thetas45, points_ref, centers)
        dist45 = distMatrix(n_trees,points45,centers)
        fo45,sol45 = computeTsp(n_trees,dist45,points45)
        edges45 = fc.formatEdges(sol45)
    
        pf.plotGraph(points45,edges45,centers,n_cols,n_rows)
        pf.plotPoints(points45,edges45)
        print(colored("inBetweenRows = 45 distance [km]: " +str(fo45/molt),"green"))
#    time.sleep(0.5)
  

#    pf.plotWithoutLines(points,centers,n_cols,n_rows)
#    
   
    sys.exit()


    
if __name__ == '__main__':  
    main()
<file_sep>/pantheon_planner/functions.py
# -*- coding: utf-8 -*-
"""
Created on Sat Sep 22 19:20:08 2018

@author: Alessandro
"""
from __future__ import division
import numpy 

from concorde.tsp import TSPSolver
from numpy import linalg as LA

molt = 1



'''
	my functions!!
'''

def generateCapturePts(trees_pos,n_rows,n_cols,row_step,col_step):
    #assumption: trees are generated row-wise from bottom to up
    
    row_mask = createRowMask2(n_cols,col_step)

    capture_pts = numpy.zeros(shape = ( n_rows * row_mask.shape[0] + n_cols, 2))

    pts = numpy.zeros(shape = ( row_mask.shape[0], 2 ) )

    last_pts = numpy.zeros(shape = ( n_cols, 2 ) )


    for i in range(n_rows):
        curr_row = i*row_step
        pts[:,0] = row_mask
        for j in range(n_cols + 1):
                pts[j,1] = curr_row - row_step/2
  
        for j in range(n_cols,pts.shape[0]):
                pts[j,1] = curr_row
           
        capture_pts[i*row_mask.shape[0]:(i+1)*row_mask.shape[0],:] = pts


    last_row_to_draw = i*row_step
    last_pts[:,0] = row_mask[0:n_cols]

    for j in range(n_cols):
        last_pts[j,1] = last_row_to_draw + row_step/2
    
    capture_pts[-n_cols:,:] = last_pts  

    return capture_pts



def generateinBetweenCapturePts(n_rows,n_cols,row_step,col_step):

    row_mask = createRowMask(n_cols,col_step)

    capture_pts = numpy.zeros(shape = ( (n_rows + 1) * row_mask.shape[0], 2))
    
    pts = numpy.zeros(shape = ( row_mask.shape[0], 2 ) )

    for i in range(n_rows + 1):
        curr_row = i*row_step
        pts[:,0] = row_mask 
        pts[:,1] = curr_row - row_step/2
  
        capture_pts[i*row_mask.shape[0]:(i+1)*row_mask.shape[0],:] = pts

    return capture_pts


def createRowMask(n,step):

    row_mask = numpy.zeros(shape = (n+1))

    row_mask[0] = -step/2
    for i in range(1,n+1,1):
        row_mask[i] = row_mask[i-1] + step
    
    return row_mask

def createRowMask2(n,step):

    row_mask = numpy.zeros(shape = (2*n + 1))

    row_mask[0] = 0

    for i in range(1,n):
        row_mask[i] = row_mask[i-1] + step

    row_mask[n] = -step/2


    for i in range(n+1,row_mask.shape[0]):
        row_mask[i] = row_mask[i-1] + step
    
    return row_mask

def createColMask(n,step):

    col_mask = numpy.zeros(shape = (n+1))

    col_mask[0] = -step/2
    for i in range(1,n+1,1):
        col_mask[i] = col_mask[i-1] + step
    
    return col_mask


def readFile(name,n_nodes):
    #edges = numpy.zeros(shape=(n_nodes, 2),dtype=int)
    nodes = numpy.zeros(shape=(n_nodes, 1),dtype=int)
    file = open(name,"r")
    i=0
    for line in file:
        elem = line.split(" ")
        for e in elem:
            if e!="\n":
                nodes[i,0]=int(e)
                i+=1
         
    '''for i in range(len(nodes)-1):
        edges[i,:] = [nodes[i],nodes[i+1]]
     
    edges[i+1,:] = [nodes[i+1],nodes[0]]'''
    return nodes    
        

def matlab(file):
    ed = numpy.zeros(shape=(400,2),dtype=int)
    
    file = open(file,"r")
    i=0
    for line in file:
        ed[i][0]=line.split(" ")[0]
        ed[i][1]=line.split(" ")[1]
        i+=1
    
    return ed

def getNodes(n):
    nodes = []
    for i in range(n):
        nodes.append(str(i))
    return nodes

def mapPoint(nodes,colors):
    
    dic = {}
    for n in nodes:
        col = colors.get(n)  
        for i in range(4):
            dic[(int(n)*4+i)]=col
            
    return dic       

def generateRandomCenters(num_rows, num_col):
    
    n = num_rows*num_col
    centers = numpy.zeros(shape=(n, 2))
    centers[:,0]=20000 * numpy.random.rand(n)
    centers[:,1]=20000 * numpy.random.rand(n)
    
    
    return centers    

def generateCenters(n_rows,n_col,dist_row,dist_col):
    xx = 0#1*molt
    yy =0#1*molt

    centers = numpy.zeros(shape=(n_rows*n_col, 2))
    h =0;
    for i in range(0,n_rows):  
        for j in range(0,n_col):
            
            centers[h,0] = xx
            centers[h,1] = yy  
            h=h+1
           
            xx =xx +dist_col
            
        yy =yy +dist_row
#        yy = 1*molt
        xx = 0
        
    return centers   

def generateTrees(num_rows, num_cols, dist_row,dist_col):
     
    xx = 0#1*molt
    yy =0#1*molt
    tree_ID = 1

    centers = numpy.zeros(shape=(num_rows*num_cols, 3))
    h =0;
    for i in range(0,num_rows):  
        for j in range(0,num_cols):
            
            centers[h,0] = xx
            centers[h,1] = yy
            centers[h,2] = tree_ID
            h=h+1
            tree_ID = tree_ID + 1
            xx =xx +dist_col
            
        yy =yy +dist_row
#        yy = 1*molt
        xx = 0
        
    return centers        
            
    
def generateRefFrame(centers,r):
    
    h=0
    #1.15176*molt
    print(" Selected tree radious = "+ str(r))
    v = numpy.matrix([[0,r],[r,0],[-r,0],[0,-r]])

    points =numpy.zeros(shape =(4*centers.shape[0],2))
   
    for i in range(centers.shape[0]):
        points[h:(h+4),:]=v+centers[i,:]
        h=h+4

    return points        
            

    
def formatEdges(solution):
    
    edges = numpy.zeros(shape=(solution.size, 2),dtype=int)    
    for i in range(solution.size-1):
        edges[i,:]=[solution[i],solution[i+1]]
        
    edges[i+1,:]=[solution[i+1], solution[0]]    #to close the loop
    return edges
      
def groupByColour(dic):
    dic2 ={}
    
    for key in dic:
        val = dic.get(key)
        if val in dic2:
            elem = dic2[val]
            elem.append(key)
            dic2[val]= elem
        else:
            elem = []
            elem.append(key)
            dic2[val]=elem
    return dic2

def get_Trees_Edges(neighbors,n_trees):
    
    n_edges_i = []
    n_edges_j = []
    h=0
    for key in neighbors:
        elem = neighbors.get(key)
        for i in range(len(elem)):
            n_edges_i.append(int(key))
            n_edges_j.append(int(elem[i]))
            h+=1
    return n_edges_i,n_edges_j


def rotMatrix(x,y,theta,cx,cy):
    p = numpy.zeros(shape=(1, 2))
    new_x = x-cx;
    new_y = y-cy;

    p[0][0]= new_x*numpy.cos(theta)-new_y*numpy.sin(theta)+ cx
    p[0][1]= new_x*numpy.sin(theta)+new_y*numpy.cos(theta)+ cy
    
    return p
    
