draw_map.py

from eye import *
from math import cos, sin, pi

class Robo:
    def __init__(self, map):
        self.map = map
        self.x = self.map.max_x / 2
        self.y = self.map.max_y / 2
        self.phi = 90
        VWSetPosition(int(self.x),int(self.y),90)
        self.current_color = "red"

    def draw_circle(self):
        a = LIDARGet();
        for i in range(len(a)):
            angle = i
            dist = a[(i + self.phi) %360]
            x = self.x - cos(angle / 180.0 * pi) * dist
            y = self.y - sin(angle / 180.0 * pi) * dist
            circle="<circle cx=" + '"' + str(x) +'"'
            circle+=" cy=" + '"' + str(y) +'"'
            circle+= " r="+'"'+str(3)+'"'
            circle+=" stroke ="+'"'+self.current_color+'"'
            circle+=" stroke-width="+'"'+str(2)+'"'
            circle+= " fill="+'"'+"white"+'"' + " />\n"
            self.map.image.write(circle)

    def get(self):
        self.x,self.y,self.phi = VWGetPosition()
        self.y = self.map.max_y - self.y
        # self.x = self.map.max_x - self.x

    def draw(self):
        self.get()
        self.draw_circle()
        s = "\n\t<rect x=" + '"' + str(self.x)+ '"'
        s +=  " y=" + '"' + str(self.y) + '"'
        s += " width=" +'"' + str(10)+ '"'
        s += " height=" +'"' +  str(10) + '"'
        s +=" style= " + '"' + "fill:" + self.current_color + "();stroke-width:1;stroke:rgb(0,0,0)" + '"' + " />\n"
        self.map.image.write(s)


class Map:
    def __init__(self, file):
        self.image = open( file +".svg", "w+")
        self.max_x = 4000
        self.max_y = 4000
        s = "<svg width=" +'"' + str(30 + self.max_x) + '"' +" height=" + '"' + str(30+ self.max_y) + '"' + ">"
        self.image.write(s)
        s = "\n\t<rect x=" + '"' + "0"+ '"'
        s +=  " y=" + '"' + "0" + '"'
        s += " width=" +'"' + str(30 + self.max_x)+ '"'
        s += " height=" +'"' +  str(30 + self.max_y) + '"'
        s +=" style= " + '"' + "fill:rgb" + str((255,255,255)) + "();stroke-width:1;stroke:rgb(0,0,0)" + '"' + " />"
        self.image.write(s)
        self.robot = Robo(self)

    def end(self):
        self.image.write("\n</svg>")
        self.image.close()