summaryrefslogtreecommitdiffhomepage
path: root/digital/beacon/simu/simulator.py
diff options
context:
space:
mode:
authorFlorent Duchon2012-03-13 22:50:23 +0100
committerFlorent Duchon2012-03-18 16:30:27 +0100
commit8ac7f52728a8ec2a09a6eeebc3cf5d4afff9b485 (patch)
tree0cdca38087cdbfae887e1c4148c4a065bcfae383 /digital/beacon/simu/simulator.py
parent1355d019cafedbb3d6b191156b80a857c6a618d1 (diff)
digital/beacon, digital/lol: beacon folders reorganisation
Diffstat (limited to 'digital/beacon/simu/simulator.py')
-rw-r--r--digital/beacon/simu/simulator.py436
1 files changed, 0 insertions, 436 deletions
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 ()