# simu - Beacon simulation. {{{ # # Copyright (C) 2011 Florent DUCHON # # APBTeam: # Web: http://apbteam.org/ # Email: team AT apbteam DOT org # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by # the Free Software Foundation; either version 2 of the License, or # (at your option) any later version. # # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # # You should have received a copy of the GNU General Public License # along with this program; if not, write to the Free Software # Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. # # }}} """Graphic interface for beacon simulator.""" import re from Tkinter import * from simu.inter.drawable import * from subprocess import Popen, PIPE from math import pi import math import random import decimal class Obstacle: def __init__ (self, pos, radius, factor): self.pos = pos self.radius = radius self.factor = factor def move (self, pos): self.pos = pos class Beacon (Drawable): def __init__ (self,onto, num, pos, orientation, size): Drawable.__init__ (self, onto) self.id = num # ID self.pos = pos # position (x,y) self.orientation = orientation # orientation in degrees self.size = size # side size self.angle = 0 # obstacles angle self.mode = 0 # 0 = deactivated # 1 = activated def draw_beacon (self): # Color depends of the beacon mode if self.mode is 1: color = 'black' else: color = 'red' # Draw it self.draw_rectangle ((self.pos[0]-self.size/2,self.pos[1]-self.size/2),(self.pos[0]+self.size/2,self.pos[1]+self.size/2), fill = color) def draw_axes (self): self.draw_line (self.pos,(self.pos[0]+(300)*math.cos(math.radians(self.orientation)),self.pos[1]+300*math.sin(math.radians(self.orientation))),fill='red', arrow = LAST) self.draw_line (self.pos,(self.pos[0]+(300)*math.cos(math.radians(self.orientation+90)),self.pos[1]+300*math.sin(math.radians(self.orientation+90))),fill='red', arrow = LAST) def toogle_mode (self): if self.mode is 0: self.mode = 1 else: self.mode = 0 class Area (Drawable): def __init__ (self, onto, border_min, border_max): Drawable.__init__ (self, onto) self.border_min = border_min self.border_max = border_max self.border = None self.phantoms_counter = 0 self.obstacles = [ ] self.beacons = [ ] self.phantoms = [ ] # virtual obstacles computed by the beacon module. def draw (self): self.reset () self.draw_rectangle (self.border_min, self.border_max, fill = 'grey') self.draw_rectangle ((self.border_min[0],self.border_max[1]-500),(self.border_min[0]+500,self.border_max[1]), fill = 'blue') self.draw_rectangle ((self.border_max[0]-500,self.border_max[1]-500), self.border_max, fill = 'red') self.draw_line ((0,0),(150,0),fill='black', arrow = LAST) self.draw_line ((0,0),(0,150),fill='black', arrow = LAST) for b in self.beacons: if b.pos is not None: b.draw_beacon () for o in self.obstacles: if o.pos is not None: self.draw_circle (o.pos, o.radius,fill = o.factor and 'gray50' or 'gray25') for p in self.phantoms: if p is not None: self.draw_circle (p, 20,fill = 0 and 'gray50' or 'red') def show_angles (self): for b in self.beacons: for o in self.obstacles: self.draw_line ((b.pos[0],b.pos[1]),(o.pos[0],o.pos[1]),fill='cyan', arrow = NONE) def populate (self): self.obstacles.append (Obstacle ((500, 500), 200, 0)) self.beacons.append (Beacon (self, 1, (-40,2040), 270,80)) self.beacons.append (Beacon (self, 2, (-40,-40),0,80)) self.beacons.append (Beacon (self, 3, (3040,1000), 180,80)) def add_phantom (self,center): # Only take care of the 100 latest value. Delete previous one. if self.phantoms_counter is 100: del self.phantoms[0] else: self.phantoms_counter += 1 self.phantoms.append(center) class AreaView (DrawableCanvas): def __init__ (self, border_min, border_max, master = None): self.border_min = border_min self.border_max = border_max width = border_max[0] - border_min[0] height = border_max[1] - border_min[0] DrawableCanvas.__init__ (self, width * 1.3, height * 1.3, -width / 2,-height / 2,master, borderwidth = 1, relief = 'sunken', background = 'white') self.area = Area (self, border_min, border_max) self.area.populate () def draw (self): self.area.draw () def add_phantom (self,center): self.area.add_phantom(center) class beacon_simu (Frame): def __init__ (self, border_min, border_max, master = None): Frame.__init__ (self, master) self.pack (expand = 1, fill = 'both') self.createWidgets (border_min, border_max) self.robot_pos.set("Robot position = (0 , 0)") self.phantom_pos.set("Last phantom position = ") def createWidgets (self, border_min, border_max): # Bottom Panel self.bottomFrame = Frame (self) self.bottomFrame.pack (side = 'bottom', fill = 'both') # First subPanel for display options self.subPanel1 = Frame (self.bottomFrame) self.subPanel1.pack (side = 'left', fill = 'both') self.display_axes = IntVar () self.axesButton = Checkbutton (self.subPanel1,variable = self.display_axes, command = self.update,text = 'Display axes', indicatoron = True) self.axesButton.pack (anchor='w') self.display_angles = IntVar () self.anglesButton = Checkbutton (self.subPanel1,variable = self.display_angles, command = self.update,text = 'Display angles', indicatoron = True) self.anglesButton.pack (anchor='w') # Second subPanel for simulator options self.subPanel2 = Frame (self.bottomFrame) self.subPanel2.pack (side = 'left', fill = 'both') self.mode = StringVar() self.manualRadioButton = Radiobutton (self.subPanel2, text = 'Manual', variable = self.mode, value='manual').pack(anchor='w') self.autoRadioButton = Radiobutton (self.subPanel2, text = 'Auto', variable = self.mode, value='auto').pack(anchor='w') # Third subPanel for simulator options self.subPanel3 = Frame (self.bottomFrame) self.subPanel3.pack (side = 'left', fill = 'both') self.precisionScale = Scale (self.subPanel3, label = 'Precision', orient = 'horizontal', from_ = 1, to = 3) self.precisionScale.pack () # Fourth subPanel for buttons self.subPanel4 = Frame (self.bottomFrame) self.subPanel4.pack (side = 'left', fill = 'both') self.clearPhantomsButton = Button (self.subPanel4, text = 'Clear', command = self.clear_phantoms) self.clearPhantomsButton.pack (side = 'bottom') self.startButton = Button (self.subPanel4, text = 'Start', command = self.start) self.startButton.pack (side = 'top') # Fifth subPanel for Label self.subPanel5 = Frame (self.bottomFrame) self.subPanel5.pack (side = 'left', fill = 'both') self.robot_pos = StringVar() self.label = Label(self.subPanel5, textvariable=self.robot_pos).pack(anchor='w') self.phantom_pos = StringVar() self.label = Label(self.subPanel5, textvariable=self.phantom_pos).pack(anchor='w') # Sixth subPanel for Exit button self.subPanel6 = Frame (self.bottomFrame) self.subPanel6.pack (side = 'right', fill = 'both') self.quitButton = Button (self.subPanel6, text = 'Quit', command = self.quit) self.quitButton.pack (side = 'right', fill = 'both') self.areaview = AreaView (border_min, border_max, self) self.areaview.pack (expand = True, fill = 'both') self.areaview.bind ('<1>', self.click) def clear (self): self.areaview.area.draw () def clear_phantoms (self): del self.areaview.area.phantoms[:] self.areaview.area.phantoms_counter = 0 self.update() def update (self): self.areaview.area.update () self.areaview.area.draw () if self.display_angles.get() is 1: self.areaview.area.show_angles() if self.display_axes.get() is 1: for b in self.areaview.area.beacons: b.draw_axes() def click (self, ev): pos = self.areaview.screen_coord ((ev.x, ev.y)) # Update obstacles position for o in self.areaview.area.obstacles: if self.areaview.area.border_min[0] < pos[0] < self.areaview.area.border_max[0] and self.areaview.area.border_min[1] < pos[1] < self.areaview.area.border_max[1]: o.pos = pos self.robot_pos.set("Robot position = (%.0f , %.0f)" % pos) # Check beacon mode for b in self.areaview.area.beacons: dx = abs(b.pos[0] - pos[0]) dy = abs(b.pos[1] - pos[1]) if dx < b.size and dy < b.size: b.toogle_mode () # Update area self.update () def call_algorithm (self,num1,num2): args = [0,0,0,0,0,0,0,0] for b in self.areaview.area.beacons: if b.id is num1: args[1]=num1 args[2]=b.angle if b.id is num2: args[3]=num2 args[4]=b.angle args = [repr(a) for a in args] args[0:0] = [ './beacon.host' ] p = Popen (args, stdout = PIPE) output = p.communicate ()[0] del p output = output.split ('\n') return output def rotate_beacons (self): # Simulate a rotation for a all beacons, ie set beacon.angles. # Set the requested imprecision imprecision = self.precisionScale.get () #1 to 3 degrees imprecision = int (math.radians(imprecision)*1000) imprecision = decimal.Decimal(random.randrange(-imprecision,imprecision))/1000 # Compute angles for every beaconss for o in self.areaview.area.obstacles: for b in self.areaview.area.beacons: if b.id is 1: b.angle = math.atan(o.pos[0]/(2000-o.pos[1]))+float(imprecision) if b.id is 2: b.angle = math.atan(o.pos[0]/o.pos[1])+float(imprecision) if b.id is 3: b.angle = math.atan((3000-o.pos[0])/(1000-o.pos[1]))+float(imprecision) if b.angle < 0: b.angle = pi - abs(b.angle) def manual_mode (self): # Manual mode : warning : two beacons must already be activated self.rotate_beacons () temp = [0,0,0] i = 0 for b in self.areaview.area.beacons: if b.mode is 1: temp[i] = b.id i=i+1 phantom_pos = self.call_algorithm(temp[0],temp[1]) self.areaview.add_phantom((int (phantom_pos[0]),int (phantom_pos[1]))) self.phantom_pos.set("Last phantom position = (%.0f , %.0f)" %(float(phantom_pos[0]),float(phantom_pos[1]))) self.update () def automatic_mode (self): # Automatic mode : all beacons are used self.rotate_beacons () # Randomly select two beacons and update obstacles position exclude = random.randrange(1,4) if exclude is 1: phantom_pos = self.call_algorithm(2,3) if exclude is 2: phantom_pos = self.call_algorithm(1,3) if exclude is 3: phantom_pos = self.call_algorithm(1,2) # Draw the computed position self.areaview.add_phantom((int (phantom_pos[0]),int (phantom_pos[1]))) self.phantom_pos.set("Last phantom position = (%.0f , %.0f)" %(float(phantom_pos[0]),float(phantom_pos[1]))) def start (self) : if self.mode.get() == "manual": self.manual_mode() return if self.mode.get() == "auto": #for o in self.areaview.area.obstacles: #x = o.pos[0] + float (decimal.Decimal(random.randrange(-5,40))) #y = o.pos[1] + float (decimal.Decimal(random.randrange(-25,25))) #o.pos = (x,y) self.automatic_mode() self.after (30,self.start) self.update() else: print "No mode selected" return if __name__ == '__main__': app = beacon_simu ((0, 0), (3000, 2000)) app.mainloop ()