tkjoystick.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: euc-jp -*-
00003 
00004 #
00005 # @brief tk joystick
00006 # @date $Date$
00007 # @author Norkai Ando <n-ando@aist.go.jp>
00008 #
00009 # Copyright (C) 2007
00010 #     Noriaki Ando
00011 #     Task-intelligence Research Group,
00012 #     Intelligent Systems Research Institute,
00013 #     National Institute of
00014 #         Advanced Industrial Science and Technology (AIST), Japan
00015 #     All rights reserved.
00016 #
00017 # $Id$
00018 #
00019 
00020 # $Log$
00021 #
00022 
00023 from Tkinter import *
00024 import time
00025 import math
00026 import mutex
00027 import sys
00028 
00029 class ToggleItem:
00030     def __init__(self):
00031         self.active = True
00032 
00033     def activate(self):
00034         self.active = True
00035         self.draw()
00036 
00037     def deactivate(self):
00038         self.active = False
00039         self.delete()
00040 
00041     def toggle(self):
00042         if self.active:
00043             self.deactivate()
00044         else:
00045             self.activate()
00046 
00047 
00048 class CanvasText(ToggleItem):
00049     def __init__(self, canvas, text, x, y):
00050         ToggleItem.__init__(self)
00051         self.canvas = canvas
00052         self.id = self.canvas.create_text(x, y, text=text)
00053         self.text = text
00054         self.x = x
00055         self.y = y
00056         self.draw_text(x, y, text)
00057 
00058     def draw(self):
00059         if self.active == False: return
00060         self.delete()
00061         self.id = self.canvas.create_text(self.x, self.y, text=self.text)
00062 
00063     def draw_text(self, x, y, text):
00064         self.x = x
00065         self.y = y
00066         self.text = text
00067         self.draw()
00068 
00069     def delete(self):
00070         self.canvas.delete(self.id)
00071 
00072 
00073 class CanvasAxis(ToggleItem):
00074     def __init__(self, canvas, width, height):
00075         ToggleItem.__init__(self)
00076         self.x0 = width/2
00077         self.y0 = height/2
00078         self.width = width
00079         self.height = height
00080         self.canvas = canvas
00081         self.id = [None] * 4
00082         self.draw()
00083 
00084     def draw(self):
00085         if self.active == False: return
00086         self.delete()
00087         self.id[0] = self.canvas.create_line(0, self.height/2, 
00088                                              self.width, self.height/2)
00089         self.id[1] = self.canvas.create_text(self.width - 10,
00090                                              self.height/2 + 10,
00091                                              text="x")
00092         self.id[2] = self.canvas.create_line(self.width/2, 0, 
00093                                              self.width/2, self.height)
00094         self.id[3] = self.canvas.create_text(self.width/2 + 10,
00095                                              + 10, text="y")
00096         return
00097 
00098     def delete(self):
00099         for i in self.id:
00100             self.canvas.delete(i)
00101 
00102 class CanvasCircle(ToggleItem):
00103     def __init__(self, canvas, x, y, width, height, pitch):
00104         ToggleItem.__init__(self)
00105         self.canvas = canvas
00106         self.x = x
00107         self.y = y
00108         self.width = width
00109         self.height = height
00110         self.pitch = pitch
00111         self.id = []
00112         self.draw()
00113 
00114     def draw(self):
00115         if self.active == False: return
00116         self.delete()
00117         if self.pitch == 0:
00118             circnum = 0
00119         else:
00120             circnum = max(self.width, self.height) / self.pitch
00121         circrange = range(int(circnum))
00122         circrange.reverse()
00123         for i in circrange:
00124             x0 = self.x - self.pitch * i
00125             y0 = self.y - self.pitch * i
00126             x1 = self.x + self.pitch * i
00127             y1 = self.y + self.pitch * i
00128             if i % 2 == 0:
00129                 color = "#dddddd"
00130             else:
00131                 color = "#eeeeee"
00132             id = self.canvas.create_oval(x0, y0, x1, y1, fill=color,
00133                                               outline="#ffffff")
00134             self.id.append(id)
00135         id = self.id
00136         id.reverse()
00137         for i in id:
00138             self.canvas.tag_lower(i)
00139 
00140     def delete(self):
00141         for c in self.id:
00142             self.canvas.delete(c)
00143 
00144     def set_pitch(self, pitch):
00145         self.pitch = pitch
00146         self.draw()
00147 
00148 class CanvasLine(ToggleItem):
00149     def __init__(self, canvas, x0, y0, x1, y1, color, width):
00150         ToggleItem.__init__(self)
00151         self.canvas = canvas
00152         self.x0 = x0
00153         self.y0 = y0
00154         self.x1 = y1
00155         self.y1 = y1
00156         self.color = color
00157         self.width = width
00158         self.id = self.canvas.create_line(self.x0, self.y0, self.x1, self.y1,
00159                                           fill=self.color, width=self.width)
00160         self.draw()
00161 
00162     def draw(self):
00163         if self.active == False: return
00164         self.delete()
00165         self.id = self.canvas.create_line(self.x0, self.y0, self.x1, self.y1,
00166                                           fill=self.color, width=self.width)
00167     def draw_line(self, x0, y0, x1, y1):
00168         self.x0 = x0
00169         self.y0 = y0
00170         self.x1 = x1
00171         self.y1 = y1
00172         self.draw()
00173 
00174     def delete(self):
00175         self.canvas.delete(self.id)
00176 
00177 class Stick:
00178     def __init__(self, canvas, x, y, r, **key):
00179         self.canvas = canvas
00180         # ジョイスティックID
00181         self.key = key
00182         self.id = self.canvas.create_oval(x-r, y-r, x+r, y+r, **key)
00183         # 中心からジョイスティックへの線ID
00184         self.line = None
00185         # (x,y) テキスト表示ID
00186         self.xy_text = None
00187         # (r,th) テキスト表示ID
00188         self.pol_text = None
00189 
00190         # 画面座標系から画面ジョイスティック座標系へのオフセット
00191         self.offsetx = x
00192         self.offsety = y
00193 
00194         # 画面ジョイスティック座標系でのジョイスティック位置
00195         self.x = 0
00196         self.y = 0
00197 
00198         # ジョイスティック座標系でのジョイスティック位置
00199         self.pos_x = 0
00200         self.pos_y = 0
00201 
00202         self.coffx = 0
00203         self.coffy = 0
00204 
00205         # ジョイスティックへのバインド
00206         self.make_binds()
00207         # テキスト描画
00208 
00209     def set_on_drag_start(self, func):
00210         self.on_drag_start = func
00211 
00212     def set_on_dragging(self, func):
00213         self.on_dragging = func
00214 
00215     def set_on_drag_end(self, func):
00216         self.on_drag_end = func
00217 
00218     def make_binds(self):
00219         self.canvas.tag_bind(self.id, '<1>', self.drag_start)
00220         self.canvas.tag_bind(self.id, '<Button1-Motion>', self.dragging)
00221         self.canvas.tag_bind(self.id, '<Button1-ButtonRelease>', self.drag_end)
00222         
00223     def drag_start(self, event):
00224         # クリック位置のオフセットを計算
00225         x1 = event.x - self.offsetx
00226         y1 = event.y - self.offsety
00227         self.coffx = x1 - self.x
00228         self.coffy = y1 - self.y
00229 
00230         self.calc_pol(self.pos_x, self.pos_y)
00231 #        self.draw_text()
00232 
00233         # コールバック
00234         if self.on_drag_start != None:
00235             self.on_drag_start((self.pos_x, self.pos_y), (self.r, self.th))
00236         
00237     def dragging(self, event):
00238         # ドラッグの移動量
00239         x1 = event.x - self.offsetx
00240         y1 = event.y - self.offsety
00241         dx = (x1 - self.x) - self.coffx
00242         dy = (y1 - self.y) - self.coffy
00243 
00244         # 円を移動
00245         self.canvas.move(self.id, dx, dy)
00246         self.canvas.tag_raise(self.id)
00247         # ジョイスティック位置を計算
00248         self.x = x1 - self.coffx
00249         self.y = y1 - self.coffy
00250         self.pos_x = self.x
00251         self.pos_y = -self.y 
00252         self.calc_pol(self.pos_x, self.pos_y)
00253 
00254         # コールバック
00255         if self.on_dragging != None:
00256             self.on_dragging((self.pos_x, self.pos_y), (self.r, self.th))
00257 
00258     def drag_end(self, event):
00259         x1 = event.x - self.offsetx
00260         y1 = event.y - self.offsety
00261         # 戻すための移動量
00262         dx = x1 - self.coffx
00263         dy = y1 - self.coffy
00264         self.canvas.move(self.id, -dx, -dy)
00265         self.x = 0
00266         self.y = 0
00267         self.pos_x = 0
00268         self.pos_y = 0
00269         self.calc_pol(self.pos_x, self.pos_y)
00270         # コールバック
00271         if self.on_drag_end != None:
00272             self.on_drag_end((self.pos_x, self.pos_y), (self.r, self.th))
00273 
00274     def calc_pol(self, x, y):
00275         r = math.hypot(x, y)
00276         if r == 0:
00277             th = 720.0
00278         elif y >= 0.0:
00279             th = math.acos((x * 1.0) / (r * 1.0)) * 180.0 / math.pi
00280         elif y < 0.0:
00281             th = math.acos((x * 1.0) / (r * 1.0)) * -180.0 / math.pi
00282         self.r = r
00283         self.th = th
00284         return (r, th)
00285 
00286     def get_pos(self):
00287         return self.pos_x, self.pos_y, self.r, self.th
00288 
00289 
00290 class TkJoystick(Frame):
00291     def __init__(self, r=10, width=300, height=300, master=None):
00292         Frame.__init__(self, master)
00293 
00294         self.pos_x = 0.0
00295         self.pos_y = 0.0
00296         self.pol_r = 0.0
00297         self.pol_th = 0.0
00298 
00299         self.stick_r = r
00300 
00301         self.width = width
00302         self.height = height
00303 
00304         self.wd = 160
00305 
00306         self.x0 = width/2
00307         self.y0 = height/2
00308 
00309         self.xentry = ""
00310         self.yentry = ""
00311         self.rentry = ""
00312         self.thentry = ""
00313 
00314         # scale variable
00315         self.scale_var = DoubleVar()
00316         self.scale_var.set(1.0)
00317 
00318         # checkbutton variables
00319         self.vline_check = StringVar()
00320         self.vline_check.set("on")
00321         self.axis_check = StringVar()
00322         self.axis_check.set("on")
00323         self.circ_check = StringVar()
00324         self.circ_check.set("on")
00325         self.xy_check = StringVar()
00326         self.xy_check.set("on")
00327         self.pol_check = StringVar()
00328         self.pol_check.set("on")
00329 
00330         # x-y entry variables
00331         self.xentry = StringVar()
00332         self.xentry.set("0.0")
00333         self.yentry = StringVar()
00334         self.yentry.set("0.0")
00335 
00336         # polarpos entry variables
00337         self.rentry = StringVar()
00338         self.rentry.set("0.0")
00339         self.thentry = StringVar()
00340         self.thentry.set("0.0")
00341 
00342         # text position
00343         self.xytext_x = self.width - 50
00344         self.xytext_y = self.height - 25
00345         self.poltext_x = self.width - 60
00346         self.poltext_y = self.height - 10
00347 
00348         # vector line
00349         self.vline_color = "#aaaaaa"
00350         self.vline_width = 2
00351         self.default_pitch = 50
00352 
00353         # callback function
00354         self.on_update = None
00355 
00356         self.init()
00357         self.pack()
00358 
00359 
00360     def init(self):
00361         self.canvas = Canvas(self, bg="white", \
00362                                  width = self.width, height = self.height)
00363         self.canvas.pack(side=LEFT)
00364 
00365         # coaxial pattern
00366         self.can_circle = CanvasCircle(self.canvas, 
00367                                    self.x0, self.y0,
00368                                    self.width, self.height, pitch=50)
00369 
00370         # axis
00371         self.can_axis = CanvasAxis(self.canvas, self.width, self.height)
00372 
00373         # x-y text
00374         text = 'x: %4d, y: %4d' % (0, 0)
00375         self.can_xytext = CanvasText(self.canvas, text,
00376                                      self.xytext_x, self.xytext_y)
00377 
00378         # polar position text
00379         text = 'r: %4.2f, th: NaN' % (0.0)
00380         self.can_poltext = CanvasText(self.canvas, text,
00381                                       self.poltext_x, self.poltext_y)
00382                            
00383         self.can_vline = CanvasLine(self.canvas, self.x0, self.y0,
00384                                     self.x0, self.y0,
00385                                     self.vline_color, self.vline_width)
00386 
00387         # joystick circle
00388         self.stick = Stick(self.canvas, self.width/2, self.height/2,
00389                            self.stick_r,
00390                            fill="#999999", width=1)
00391         self.stick.set_on_drag_start(self.on_pos_update)
00392         self.stick.set_on_dragging(self.on_pos_update)
00393         self.stick.set_on_drag_end(self.on_pos_update)
00394 
00395 
00396         # right side widgets
00397         self.frame = Frame(self, height=self.height, width=self.wd)
00398         self.frame.pack(side=LEFT, fill=Y, padx=3, pady=3)
00399         self.create_scale(self.frame).pack(side=TOP, fill=Y)
00400         self.create_checkbutton(self.frame).pack(side=TOP, fill=Y)
00401         self.create_pollabel(self.frame).pack(side=BOTTOM, fill=Y)
00402         self.create_xylabel(self.frame).pack(side=BOTTOM, fill=Y)
00403         return
00404 
00405     def create_scale(self, frame):
00406         f = Frame(frame, bd=2, relief=GROOVE)
00407         dummy = Frame(f, width=self.wd)
00408         dummy.pack(side=TOP)
00409         sl = Scale(f, from_=0, to=10, resolution=0.01,
00410                    label="Scale Factor", command=self.on_scale,
00411                    variable=self.scale_var, orient=HORIZONTAL)
00412         bt = Button(f, text="Reset Scale", command=self.reset_scale)
00413         sl.pack(side=TOP, fill=X)
00414         bt.pack(side=TOP, fill=X)
00415         return f
00416 
00417     def on_scale(self, val):
00418         v =  float(val)
00419         if v == 0.0:
00420             pitch = 0
00421         else:
00422             pitch = self.default_pitch/v
00423         self.can_circle.set_pitch(pitch)
00424 
00425     def create_checkbutton(self, frame):
00426         f = Frame(frame, bd=2, relief=GROOVE)
00427         dummy = Frame(f, width=self.wd)
00428         dummy.pack(side=TOP)
00429         vec = Checkbutton(f, text="Vector Line",
00430                            onvalue="on", offvalue="off",
00431                            justify=LEFT, anchor=W,
00432                            variable=self.vline_check,
00433                            command=self.can_vline.toggle)
00434         axis = Checkbutton(f, text="Axis",
00435                            onvalue="on", offvalue="off",
00436                            justify=LEFT, anchor=W,
00437                            variable=self.axis_check,
00438                            command=self.can_axis.toggle)
00439         circ = Checkbutton(f, text="Background",
00440                            onvalue="on", offvalue="off",
00441                            justify=LEFT, anchor=W,
00442                            variable=self.circ_check,
00443                            command=self.can_circle.toggle)
00444         xy   = Checkbutton(f, text="X-Y position", 
00445                            onvalue="on", offvalue="off",
00446                            justify=LEFT, anchor=W,
00447                            variable=self.xy_check,
00448                            command=self.can_xytext.toggle)
00449         pol  = Checkbutton(f, text="Polar postion",
00450                            onvalue="on", offvalue="off",
00451                            justify=LEFT, anchor=W,
00452                            variable=self.pol_check,
00453                            command=self.can_poltext.toggle)
00454         for w in [vec, axis, circ, xy, pol]:
00455             w.pack(side=TOP, anchor=W, fill=X)
00456         return f
00457 
00458     def create_xylabel(self, frame):
00459         f = Frame(frame)
00460         dummy = Frame(f, width=self.wd)
00461         dummy.pack(side=TOP)
00462         xl = Label(f, text="x: ", width=3)
00463         xe = Entry(f, width=7, textvariable=self.xentry,
00464                    justify=RIGHT, relief=GROOVE)
00465         yl = Label(f, text="y: ", width=3)
00466         ye = Entry(f, width=7, textvariable=self.yentry,
00467                    justify=RIGHT, relief=GROOVE)
00468 
00469         for w in [ye, yl, xe, xl]:
00470             w.pack(side=RIGHT, fill=X)
00471         return f
00472 
00473     def create_pollabel(self, frame):
00474         f = Frame(frame, width=self.wd)
00475         dummy = Frame(f, width=self.wd)
00476         dummy.pack(side=TOP)
00477         rl = Label(f, text="r: ", width=3)
00478         re = Entry(f, width=7, textvariable=self.rentry,
00479                    justify=RIGHT, relief=GROOVE)
00480         thl = Label(f, text="th:", width=3)
00481         the = Entry(f, width=7, textvariable=self.thentry,
00482                     justify=RIGHT, relief=GROOVE)
00483         for w in [the, thl, re, rl]:
00484             w.pack(side=RIGHT, fill=X)
00485         return f
00486 
00487     def reset_scale(self):
00488         self.scale_var.set(1.)
00489         pitch = self.default_pitch/1.0
00490         self.can_circle.set_pitch(pitch)
00491         
00492     def get_pos(self):
00493         return self.pos_x, self.pos_y, self.pol_r, self.pol_th
00494 
00495     def on_pos_update(self, pos, pol):
00496         self.pos_x = pos[0] * self.scale_var.get()
00497         self.pos_y = pos[1] * self.scale_var.get()
00498         self.pol_r = pol[0] * self.scale_var.get()
00499         self.pol_th = pol[1]
00500 
00501         xt = '%4d' % (self.pos_x)
00502         yt = '%4d' % (self.pos_y)
00503         rt = '%5.2f' % (self.pol_r)
00504         if pol[1] > 360: tht = 'NaN'
00505         else: tht = '%5.2f' % self.pol_th
00506 
00507         self.xentry.set(xt)
00508         self.yentry.set(yt)
00509         self.rentry.set(rt)
00510         self.thentry.set(tht)
00511 
00512         text = 'x: %4d, y: %4d' % (pos[0], pos[1])
00513         self.can_xytext.draw_text(self.xytext_x, self.xytext_y, text)
00514         if pol[1] > 360:
00515             text = 'r: %5.2f, th: NaN' % (pol[0])
00516         else:
00517             text = 'r: %5.2f, th: %5.2f' % (pol[0], pol[1])
00518         self.can_poltext.draw_text(self.poltext_x, self.poltext_y, text)
00519 
00520         self.can_vline.draw_line(self.x0, self.y0,
00521                                  self.x0 + pos[0],
00522                                  self.y0 - pos[1])
00523 
00524         if self.on_update != None:
00525             self.on_update((self.pos_x, self.pos_y),
00526                            (self.pol_r, self.pol_th))
00527 
00528     def set_on_update(self, func):
00529         self.on_update = func
00530 
00531 def test ():
00532     m = TkJoystick()
00533 
00534     while 1:
00535         m.update()
00536         print m.get_pos()
00537 
00538 
00539 if  __name__ == '__main__': test()


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:57