From 8ac7f52728a8ec2a09a6eeebc3cf5d4afff9b485 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Tue, 13 Mar 2012 22:50:23 +0100 Subject: digital/beacon, digital/lol: beacon folders reorganisation --- digital/beacon/src/simulator.py | 436 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 436 insertions(+) create mode 100644 digital/beacon/src/simulator.py (limited to 'digital/beacon/src/simulator.py') diff --git a/digital/beacon/src/simulator.py b/digital/beacon/src/simulator.py new file mode 100644 index 00000000..7e8b4e1c --- /dev/null +++ b/digital/beacon/src/simulator.py @@ -0,0 +1,436 @@ +# 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 +import time + +import fcntl +import os +from subprocess import * + + +class Obstacle: + def __init__ (self, id, pos, radius, factor): + self.id = id + 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 = {} # 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 (1,(random.randrange(100,2700),random.randrange(100,1700)), 200, 0)) + self.obstacles.append (Obstacle (2,(random.randrange(100,2700), random.randrange(100,1700)), 200, 0)) + + #self.obstacles.append (Obstacle (1,(1666,1411), 200, 0)) + #self.obstacles.append (Obstacle (2,(2604,344), 200, 0)) + #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 = ") + args = [] + args[0:0] = [ './beacon.host' ] + self.p = Popen (args,shell=True, stdin = PIPE, stdout = PIPE) + self.counter = 0 + self.valeur_ko = 0 + self.valeur_ok = 0 + self.total = 0 + + 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.exit) + self.quitButton.pack (side = 'right', fill = 'both') + self.recoveryButton = Button (self.subPanel6, text = 'Recovery', command = self.recovery) + self.recoveryButton.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]: + dx = o.pos[0] - pos[0] + dy = o.pos[1] - pos[1] + if dx * dx + dy * dy < 300*300: + print "Obstacle trouve" + 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,num,angle,angleID): + temp = [] + self.p.stdin.write(str(num)+'\n') + self.p.stdin.flush() + self.p.stdin.write(str(angle)+'\n') + self.p.stdin.flush() + self.p.stdin.write(str(angleID)+'\n') + self.p.stdin.flush() + for o in self.areaview.area.obstacles: + x = self.p.stdout.readline().split('\n') + y= self.p.stdout.readline().split('\n') + trust = self.p.stdout.readline().split('\n') + temp.append([x,y,trust]) + return temp + + 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 + #imprecision = 0 + # Compute angles for every beaconss + for b in self.areaview.area.beacons: + for o in self.areaview.area.obstacles: + if b.id is 1: + b.angle[o.id] = math.atan(float(o.pos[0])/(float(2000)-float(o.pos[1])))+float(imprecision) + if b.id is 2: + b.angle[o.id] = math.atan(float(o.pos[0])/float(o.pos[1]))+float(imprecision) + #print math.degrees(b.angle[o.id]) + if b.id is 3: + b.angle[o.id] = math.atan((float(3000)-float(o.pos[0]))/(float(1000)-float(o.pos[1])))+float(imprecision) + if b.angle[o.id] < 0: + b.angle[o.id] = pi - abs(b.angle[o.id]) + + + def manual_mode (self): + # Manual mode : warning : two beacons must already be activated + self.rotate_beacons () + for b in self.areaview.area.beacons: + if b.mode is 1: + phantom_pos = self.call_algorithm(b.id,b.angle[1],1) + phantom_pos = self.call_algorithm(b.id,b.angle[2],2) + self.areaview.add_phantom((int (phantom_pos[0][0]),int (phantom_pos[0][1]))) + self.areaview.add_phantom((int (phantom_pos[1][0]),int (phantom_pos[1][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 () + select = random.randrange(1,4) + for b in self.areaview.area.beacons: + if b.id is select: + temp = 1 + for o in self.areaview.area.obstacles: + phantom_pos = self.call_algorithm(b.id,b.angle[temp],temp) + for o in self.areaview.area.obstacles: + if phantom_pos[temp-1][2][0] is not '0': + self.areaview.add_phantom((int (phantom_pos[temp-1][0][0]),int (phantom_pos[temp-1][1][0]))) + temp = temp + 1 + + + + + def automatic_mode_old (self): + # Automatic mode : all beacons are used + self.rotate_beacons () + select = random.randrange(1,4) + for b in self.areaview.area.beacons: + if b.id is select: + phantom_pos = self.call_algorithm(b.id,b.angle[1],1) + if phantom_pos[0][2][0] is not '0': + self.areaview.add_phantom((int (phantom_pos[0][0][0]),int (phantom_pos[0][1][0]))) + if phantom_pos[1][2][0] is not '0': + self.areaview.add_phantom((int (phantom_pos[1][0][0]),int (phantom_pos[1][1][0]))) + phantom_pos = self.call_algorithm(b.id,b.angle[2],2) + # Draw the computed position + if phantom_pos[0][2][0] is not '0': + self.areaview.add_phantom((int (phantom_pos[0][0][0]),int (phantom_pos[0][1][0]))) + if phantom_pos[1][2][0] is not '0': + self.areaview.add_phantom((int (phantom_pos[1][0][0]),int (phantom_pos[1][1][0]))) + + + def automatic_mode_for_recovery_test (self): + # Automatic mode : all beacons are used + self.rotate_beacons () + trust = 0 + select = random.randrange(1,4) + for b in self.areaview.area.beacons: + if b.id is select: + phantom_pos = self.call_algorithm(b.id,b.angle[1],1) + if phantom_pos[0][2][0] is not '0': + self.areaview.add_phantom((int (phantom_pos[0][0][0]),int (phantom_pos[0][1][0]))) + trust += 1 + if phantom_pos[1][2][0] is not '0': + self.areaview.add_phantom((int (phantom_pos[1][0][0]),int (phantom_pos[1][1][0]))) + trust += 1 + if trust != 0: + return trust + phantom_pos = self.call_algorithm(b.id,b.angle[2],2) + # Draw the computed position + if phantom_pos[0][2][0] is not '0': + print phantom_pos + self.areaview.add_phantom((int (phantom_pos[0][0][0]),int (phantom_pos[0][1][0]))) + trust += 1 + if phantom_pos[1][2][0] is not '0': + self.areaview.add_phantom((int (phantom_pos[1][0][0]),int (phantom_pos[1][1][0]))) + trust += 1 + return trust + + 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 + def exit (self): + self.p.kill() + self.quit() + + def recovery (self): + recovery = 0 + self.clear_phantoms() + while recovery == 0: + recovery = self.automatic_mode_for_recovery_test() + #self.update() + #print self.areaview.area.phantoms + self.update() + for o in self.areaview.area.obstacles: + #print "obstacle" + #print "o.pos[0] = %d - self.areaview.area.phantoms[0][0] = %d" %(o.pos[0],self.areaview.area.phantoms[0][0]) + dx1 = o.pos[0] - self.areaview.area.phantoms[0][0] + dy1 = o.pos[1] - self.areaview.area.phantoms[0][1] + dx2 = o.pos[0] - self.areaview.area.phantoms[1][0] + dy2 = o.pos[1] - self.areaview.area.phantoms[1][1] + total1 = dx1 * dx1 + dy1 * dy1 + total2 = dx2 * dx2 + dy2 * dy2 + x = random.randrange(100,2700) + y = random.randrange(100,1700) + o.pos = (x,y) + self.total += 1 + if total1 < 90000 or total2 < 90000: + #print "1 trouve" + self.valeur_ok+=1 + else: + #print "1 pas trouve" + self.valeur_ko+=1 + print "#######################################" + print self.areaview.area.phantoms + print "OK = %d" %(self.valeur_ok) + print "KO = %d" %(self.valeur_ko) + print "Total = %d" %(self.total) + print "New position (%d,%d) (%d,%d)" %(self.areaview.area.obstacles[0].pos[0],self.areaview.area.obstacles[0].pos[1],self.areaview.area.obstacles[1].pos[0],self.areaview.area.obstacles[1].pos[1]) + print "#######################################" + #time.sleep(10) + #self.clear_phantoms() + self.after (100,self.recovery) + + + + + +if __name__ == '__main__': + app = beacon_simu ((0, 0), (3000, 2000)) + app.mainloop () -- cgit v1.2.3