ninjacar_map/mapMaker.py at master · JaumeAlbardaner/ninjacar_map · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
import numpy as np
import sys
from scipy import ndimage

pixelsPerMeter = 100
margin = 1. # Meters of margin in the costmap
trackWidth = 0.75 # Width on each side of the track


def usage():
    print("USAGE: python mapMaker.py <coordsFile>")


def main(coordsFile):

    # Read coords file
    with open(coordsFile,'r') as f:
        coords = f.read().split('\n')[:-1]

    # Prepare variables
    xMin = 999
    yMin = 999
    xMax = -999
    yMax = -999

    # First look at the data
    for i in range(len(coords)):
        coords[i] = map(float, coords[i].split(', '))

        if(coords[i][0]>xMax):
            xMax = coords[i][0]
        if(coords[i][1]>yMax):
            yMax = coords[i][1]

        if(coords[i][0]<xMin):
            xMin = coords[i][0]
        if(coords[i][1]<yMin):
            yMin = coords[i][1]

    xMin = xMin - margin
    yMin = yMin - margin
    xMax = xMax + margin
    yMax = yMax + margin

    # Generate the empty map
    rows = int(((yMax - yMin) * pixelsPerMeter))
    cols = int(((xMax - xMin) * pixelsPerMeter))
    channel0 = channel1 = channel2 = channel3 = \
        np.zeros((rows, cols), dtype = np.float32)

    # Draw the path
    for i in range(len(coords)):
        x =  int(((coords[i][0] - xMin) * pixelsPerMeter))
        y =  int(((coords[i][1] - yMin) * pixelsPerMeter))
        channel0[y,x] = 1.

    # Dilate it to form the distance from every point to the path
    tmpMat1 = channel0
    tmpMat2 = np.zeros(np.shape(channel0))

    while not np.array_equal(tmpMat1, tmpMat2):
        tmpMat2 = tmpMat1
        tmpMat1 = ndimage.binary_dilation(tmpMat1).astype(tmpMat1.dtype)
        channel0 = np.add(channel0,tmpMat1)

    channel0 = channel0 / pixelsPerMeter

    # Invert the points so the closest to the line have the least weight
    currentMax = np.amax(channel0)
    channel0 = channel0 - currentMax
    channel0 = np.absolute(channel0)

    # Truncate the closest points to form a nice track
    tmp = channel0 <= trackWidth

    for i in range(np.shape(channel0)[0]):
        for j in range(np.shape(channel0)[1]):
            if tmp[i][j]:
                channel0[i][j] /= trackWidth
            else:
                channel0[i][j] += 10

    # Save the data in the expected configuration
    channel0 = np.resize(channel0, (1,rows*cols))
    channel1 = np.resize(channel1, (1,rows*cols))
    channel2 = np.resize(channel2, (1,rows*cols))
    channel3 = np.resize(channel3, (1,rows*cols))


    np.savez(coordsFile[:-4]+".npz", \
    pixelsPerMeter=np.array([pixelsPerMeter], dtype=np.float32),\
    xBounds=np.array([xMin,xMax], dtype=np.float32),\
    yBounds=np.array([yMin,yMax], dtype=np.float32),\
    channel0=channel0,channel1=channel1,channel2=channel2,channel3=channel3)


if __name__ == '__main__':
    if len(sys.argv) != 2:
        usage()
    else:
        main(sys.argv[1])