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/simu/simulator.py | 436 --------------------------------------- 1 file changed, 436 deletions(-) delete mode 100644 digital/beacon/simu/simulator.py (limited to 'digital/beacon/simu/simulator.py') diff --git a/digital/beacon/simu/simulator.py b/digital/beacon/simu/simulator.py deleted file mode 100644 index 7e8b4e1c..00000000 --- a/digital/beacon/simu/simulator.py +++ /dev/null @@ -1,436 +0,0 @@ -# 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