summaryrefslogtreecommitdiff
path: root/digital/beacon/src/simulator.py
diff options
context:
space:
mode:
Diffstat (limited to 'digital/beacon/src/simulator.py')
-rw-r--r--digital/beacon/src/simulator.py436
1 files changed, 436 insertions, 0 deletions
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 ()