TkMobileRobotSimulator.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 # -*- Python -*-
4 
5 # @brief 2D mobile robot on tk canvas
6 # @date $Date$
7 # @author Norkai Ando <n-ando@aist.go.jp>
8 #
9 # Copyright (C) 2007
10 # Noriaki Ando
11 # Task-intelligence Research Group,
12 # Intelligent Systems Research Institute,
13 # National Institute of
14 # Advanced Industrial Science and Technology (AIST), Japan
15 # All rights reserved.
16 #
17 # $Id$
18 #
19 
20 # $Log$
21 #
22 
23 #from Tkinter import *
24 from Tix import *
25 import time
26 import math
27 
28 # Import RTM module
29 import RTC
30 import OpenRTM_aist
31 # This module's spesification
32 # <rtc-template block="module_spec">
33 tkmobilerobotsimulator_spec = ["implementation_id", "TkMobileRobotSimulator",
34  "type_name", "TkMobileRobotSimulator",
35  "description", "sample component for Python and Tkinter",
36  "version", "1.0",
37  "vendor", "Noriaki Ando, AIST",
38  "category", "example",
39  "activity_type", "DataFlowComponent",
40  "max_instance", "10",
41  "language", "Python",
42  "lang_type", "SCRIPT",
43  ""]
44 # </rtc-template>
45 
46 
48  def __init__(self, manager):
49  OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
50 
51  self.pos = []
52  self.vel = []
53  return
54 
55  def onInitialize(self):
56  self._d_vel = RTC.TimedFloatSeq(RTC.Time(0,0),[])
57  self._velIn = OpenRTM_aist.InPort("vel", self._d_vel)
58 
59  self._d_pos = RTC.TimedFloatSeq(RTC.Time(0,0),[])
60  self._posOut = OpenRTM_aist.OutPort("pos", self._d_pos)
61 
62  # Set InPort buffers
63  self.addInPort("vel",self._velIn)
64  self.addOutPort("pos",self._posOut)
65 
66  # Bind variables and configuration variable
67  return RTC.RTC_OK
68 
69  def onShutdown(self, ec_id):
70  return RTC.RTC_OK
71 
72  def onDeactivated(self, ec_id):
73  self.pos = []
74  self.vel = []
75  return RTC.RTC_OK
76 
77  def onExecute(self, ec_id):
78  if self._velIn.isNew():
79  self.vel = self._velIn.read().data
80  self._d_pos.data = self.pos
81  self._posOut.write()
82  time.sleep(0.01)
83  return RTC.RTC_OK
84 
85  def get_velocity(self):
86  return self.vel
87 
88  def set_position(self, pos):
89  self.pos = pos
90 
91 
92 class ToggleItem:
93  def __init__(self):
94  self.active = True
95  return
96 
97  def __del__(self):
98  self.delete()
99  return
100 
101  def activate(self):
102  self.active = True
103  self.draw()
104  return
105 
106  def deactivate(self):
107  self.active = False
108  self.delete()
109  return
110 
111  def toggle(self):
112  if self.active:
113  self.deactivate()
114  else:
115  self.activate()
116  return
117 
119  def __init__(self, canvas, text, x, y):
120  ToggleItem.__init__(self)
121  self.canvas = canvas
122  self.id = self.canvas.create_text(x, y, text=text)
123  self.text = text
124  self.x = x
125  self.y = y
126  self.draw_text(x, y, text)
127  return
128 
129  def draw(self):
130  if self.active == False: return
131  self.delete()
132  self.id = self.canvas.create_text(self.x, self.y, text=self.text)
133  return
134 
135  def draw_text(self, x, y, text):
136  self.x = x
137  self.y = y
138  self.text = text
139  self.draw()
140  return
141 
142  def delete(self):
143  self.canvas.delete(self.id)
144  return
145 
147  def __init__(self, simulator, robot, name, r):
148  ToggleItem.__init__(self)
149  self.canvas = simulator.get_canvas()
150  self.trans = simulator.get_translation()
151  self.robot = robot
152  self.name = name
153  self.r = r
154  self.id_circle = None
155  self.id_line0 = None
156  self.id_line1 = None
157  self.id_name = None
158  self.id_pos = None
159  self.llength = 50
160  return
161 
162  def draw(self):
163  if self.active == False: return
164  self.delete()
165  rx, ry, rt = self.robot.get_pos()
166 
167  tmp_x0 = - self.r
168  tmp_y0 = - self.r
169  tmp_x1 = self.r
170  tmp_y1 = self.r
171  x0, y0 = self.trans(tmp_x0, tmp_y0, rx, ry, 0)
172  x1, y1 = self.trans(tmp_x1, tmp_y1, rx, ry, 0)
173 
174  self.id_circle = self.canvas.create_oval(x0, y0, x1, y1,
175  width=2,
176  fill="", outline="#aaaaaa")
177  r = (x1 - x0)/2
178  xo = x0 + r
179  yo = y0 - r
180 
181  lx0 = xo + (r / math.sqrt(2))
182  ly0 = yo - (r / math.sqrt(2))
183  lx1 = lx0 + self.llength
184  ly1 = ly0 - self.llength
185 
186  self.id_line0 = self.canvas.create_line(lx0, ly0, lx1, ly1,
187  fill="#777777")
188  self.id_line1 = self.canvas.create_line(lx1, ly1, lx1 + 120, ly1,
189  fill="#777777")
190  self.id_name = self.canvas.create_text(lx1+120, ly1-8,
191  anchor=E, text=self.name)
192  pos_text = '(%5.2f, %5.2f, %5.2f)' % (rx, ry, (rt*180/math.pi)%360)
193  self.id_pos = self.canvas.create_text(lx1+120, ly1+8,
194  anchor=E, text=pos_text)
195  return
196 
197 
198  def delete(self):
199  if self.id_circle != None:
200  self.canvas.delete(self.id_circle)
201  if self.id_line0 != None:
202  self.canvas.delete(self.id_line0)
203  if self.id_line1 != None:
204  self.canvas.delete(self.id_line1)
205  if self.id_name != None:
206  self.canvas.delete(self.id_name)
207  if self.id_pos != None:
208  self.canvas.delete(self.id_pos)
209  return
210 
211 
213  def __init__(self, canvas, x0, y0, width, height, pitch, color, linewd):
214  ToggleItem.__init__(self)
215  self.canvas = canvas
216  self.x0 = x0
217  self.y0 = y0
218  self.width = width
219  self.height = height
220  self.pitch = pitch
221  self.color = color
222  self.linewd = linewd
223  self.idx = []
224  self.idy = []
225 
226  self.draw()
227  return
228 
229 
230  def draw(self):
231  if self.active == False: return
232  self.delete()
233 
234  x_start = int(self.x0 % self.pitch)
235  x_num = int((self.width - x_start) / self.pitch) + 1
236  for x in range(x_num):
237  x0 = x_start + self.pitch * x
238  id = self.canvas.create_line(x0, 0, x0, self.height,
239  fill=self.color, width=self.linewd)
240  self.idx.append(id)
241 
242  y_start = int(self.y0 % self.pitch)
243  y_num = int((self.height - y_start) / self.pitch) + 1
244  for y in range(y_num):
245  y0 = y_start + self.pitch * y
246  id = self.canvas.create_line(0, y0, self.width, y0,
247  fill=self.color, width=self.linewd)
248  self.idy.append(id)
249 
250  for i in self.idx:
251  self.canvas.tag_lower(i)
252  for i in self.idy:
253  self.canvas.tag_lower(i)
254  return
255 
256 
257  def delete(self):
258  for i in self.idx:
259  self.canvas.delete(i)
260  for i in self.idy:
261  self.canvas.delete(i)
262  return
263 
264 
265  def set_pitch(self, pitch):
266  self.pitch = pitch
267  self.draw()
268  return
269 
270 
272  def __init__(self, canvas, width, height):
273  ToggleItem.__init__(self)
274  self.x0 = width/2
275  self.y0 = height/2
276  self.width = width
277  self.height = height
278  self.canvas = canvas
279  self.id = [None] * 4
280  self.draw()
281  return
282 
283 
284  def draw(self):
285  if self.active == False: return
286  self.delete()
287  self.id[0] = self.canvas.create_line(0, self.height/2,
288  self.width, self.height/2)
289  self.id[1] = self.canvas.create_text(self.width - 10,
290  self.height/2 + 10,
291  text="x")
292  self.id[2] = self.canvas.create_line(self.width/2, 0,
293  self.width/2, self.height)
294  self.id[3] = self.canvas.create_text(self.width/2 + 10,
295  + 10, text="y")
296 
297  return
298 
299  def delete(self):
300  for i in self.id:
301  self.canvas.delete(i)
302  return
303 
304 
305 
307  def __init__(self, simulator):
308  self.simulator = simulator
309  self.tick = simulator.get_tick()
310  self.canvas = simulator.get_canvas()
311  self.trans = simulator.get_translation()
312  return
313 
314  def translate(self, x, y, dx, dy, dth):
315  return self.trans(x, y, dx, dy, dth)
316 
317  def get_tick(self):
318  return self.simulator.get_tick()
319 
320 
321 import tkSimpleDialog
322 
324  def __init__(self):
325  # robot's profile
326  self.name = ""
327  self.type = ""
328  self.description = ""
329  self.vendor = ""
330  # robot's parameter/input/output
331  self.param = []
332  self.input = []
333  self.output = []
334  # max length of label text
335  self.label_len = 0
336  # max length of unit text
337  self.unit_len = 0
338 
339  self.apply_param = None
340  self.apply_input = None
341  self.reset_output = None
342  return
343 
344  def set_profile(self, name, type, description, vendor):
345  self.name = name
346  self.type = type
347  self.description = description
348  self.vendor =vendor
349  return
350 
351  def append_parameter(self, label, variable, unit):
352  self.param.append({"label":label, "var":variable, "unit":unit})
353  self.label_len = max(len(label), self.label_len)
354  self.unit_len = max(len(unit), self.unit_len)
355  return
356 
357  def append_input(self, label, variable, unit):
358  self.input.append({"label":label, "var":variable, "unit":unit})
359  self.label_len = max(len(label), self.label_len)
360  self.unit_len = max(len(unit), self.unit_len)
361  return
362 
363  def append_output(self, label, variable, unit):
364  self.output.append({"label":label, "var":variable, "unit":unit})
365  self.label_len = max(len(label), self.label_len)
366  self.unit_len = max(len(unit), self.unit_len)
367  return
368 
369  def set_apply_param(self, func):
370  self.apply_param = func
371  return
372 
373  def set_apply_input(self, func):
374  self.apply_input = func
375  return
376 
377  def set_reset_output(self, func):
378  self.reset_output = func
379  return
380 
381  def pack(self):
382  f = Toplevel()
383  self.toplevel = f
384  f.title(self.name)
385  w0 = LabelFrame(f, label="Robot's Profile",
386  options="frame.anchor w frame.justify left")
387  prof_frame = w0.subwidget('frame')
388  self.profile_label(prof_frame)
389 
390  w1 = LabelFrame(f, label="Robot's Parameters")
391  param_frame = w1.subwidget('frame')
392  self.label_entries(param_frame, self.param)
393  if self.apply_param != None:
394  self.button(param_frame, "Apply", self.apply_param)
395 
396  w2 = LabelFrame(f, label="Robot's Input Values")
397  input_frame = w2.subwidget('frame')
398  self.label_entries(input_frame, self.input)
399  if self.apply_input != None:
400  self.button(input_frame, "Set", self.apply_input)
401 
402  w3 = LabelFrame(f, label="Robot's Output Values")
403  output_frame = w3.subwidget('frame')
404  self.label_entries(output_frame, self.output)
405  if self.reset_output != None:
406  self.button(output_frame, "Reset", self.reset_output)
407 
408 
409  for w in [w0, w1, w2, w3]:
410  w.pack(side=TOP, anchor=W, fill=X)
411  self.button(f, "OK", self.on_ok)
412 
413  return
414 
415  def on_ok(self):
416  self.toplevel.destroy()
417  return
418 
419 
420  def button(self, master, label, func):
421  bt = Button(master, text=label, command=func, width=10,
422  padx=3, pady=3)
423  bt.pack(side=TOP, padx=5, pady=5)
424  return
425 
426 
427  def profile_label(self, master):
428  t = ["Robot's name: ", "Robot's type: ", "Description: ", "Vendor: "]
429  for i in range(len(t)):
430  Label(master, text=t[i], anchor=W).grid(row=i, sticky=W,
431  padx=3, pady=3)
432  l = [self.name, self.type, self.description, self.vendor]
433  for i in range(len(l)):
434  Label(master, text=l[i], anchor=W).grid(row=i, column=1, sticky=W,
435  padx=3, pady=3)
436  return
437 
438 
439  def label_entry(self, master, label0, var, label1):
440  f = Frame(master)
441  l0 = Label(f, text=label0, width=self.label_len, justify=LEFT, anchor=W)
442  e = Entry(f, width=7, textvariable=var,
443  justify=RIGHT, relief=GROOVE, bd=2)
444  l1 = Label(f, text=label1, width=self.unit_len, justify=LEFT, anchor=W)
445  for w in [l0, e, l1]:
446  w.pack(side=LEFT, anchor=W, padx=3, pady=3)
447  return f
448 
449  def label_entries(self, f, props):
450  for p in props:
451  self.label_entry(f, p["label"], p["var"], p["unit"]).pack(side=TOP)
452  return
453 
454 
455 
457  def __init__(self, radius, wheeld, pos = (0, 0, math.pi/2), dt=0.1):
458  self.radius = radius
459  self.wheeld = wheeld
460  self.dt = dt
461  self.pre_x = pos[0]
462  self.pre_y = pos[1]
463  self.pre_t = pos[2]
464  self.pre_x_dot = 0
465  self.pre_y_dot = 0
466  self.pre_t_dot = 0
467  return
468 
469 
470  def set_wheel_radius(self, radius):
471  # wheel radius [m]
472  self.radius = radius
473  return
474 
475 
476  def set_wheel_distance(self, distance):
477  # distance between wheels [m]
478  self.wheeld = distance
479  return
480 
481 
482  def set_time_tick(self, tick):
483  # time tick for simulation [sec]
484  self.dt = tick
485  return
486 
487 
488  def set_pos(self, pos = (0, 0, math.pi/2)):
489  # x: pos[0] [m]
490  # y: pos[1] [m]
491  # theta: pos[2] [rad]
492  self.pre_x = pos[0]
493  self.pre_y = pos[1]
494  self.pre_t = pos[2]
495  return
496 
497 
498  def get_pos(self):
499  return self.pre_x, self.pre_y, self.pre_t
500 
501  def control(self, w1, w2):
502  # w1: [rad/s]
503  # w2: [rad/s]
504  x_dot = self.radius * (w1 + w2) * math.cos(self.pre_t)
505  y_dot = self.radius * (w1 + w2) * math.sin(self.pre_t)
506  t_dot = self.radius * (-w1 + w2) / self.wheeld
507 
508  x = (self.dt * (self.pre_x_dot + x_dot) / 2) + self.pre_x
509  y = (self.dt * (self.pre_y_dot + y_dot) / 2) + self.pre_y
510  theta = (self.dt * (self.pre_t_dot + t_dot) / 2) + self.pre_t
511 
512  self.pre_x = x
513  self.pre_y = y
514  self.pre_t = theta
515 
516  self.pre_x_dot = x_dot
517  self.pre_y_dot = y_dot
518  self.pre_t_dot = t_dot
519 
520  return x, y, theta
521 
522 
523 
525  count = 0
526  def __init__(self, simulator, radius=2, wheeld=20,
527  pos = (0, 0, math.pi/2)):
528  SimulatedObject.__init__(self, simulator)
529  self.tick = self.get_tick()
530  self.model = DiffMobileModel(radius, wheeld, pos, self.tick)
531  self.fig = [[10, 0], [5, 10], [-10, 10], [-10, -10], [5, -10]]
532  self.id = None
533  self.wl = 0.0
534  self.wr = 0.0
535  self.name = "DDMobileRobot" + str(self.__class__.count)
536  self.__class__.count += 1
537  self.comp = OpenRTM_aist.Manager.instance().createComponent("TkMobileRobotSimulator")
538 
539  # properties
540  self.rentry = StringVar()
541  self.rentry.set(radius)
542  self.dentry = StringVar()
543  self.dentry.set(wheeld)
544  # input variables
545  self.wlentry = StringVar()
546  self.wrentry = StringVar()
547  # output variables
548  self.xentry = StringVar()
549  self.yentry = StringVar()
550  self.tentry = StringVar()
551  return
552 
553 
554  def __del__(self):
555  try:
556  self.comp.exit()
557  del self.comp
558  except:
559  pass
560  self.delete()
561  return
562 
563 
564  def get_name(self):
565  return self.name
566 
567 
568  def set_pos(self, (x, y, th)):
569  self.model.set_pos((x, y, th))
570  return
571 
572 
573  def get_pos(self):
574  return self.model.get_pos()
575 
576 
577  def set_wheel_velocity(self, wl, wr):
578  self.wl = wl
579  self.wr = wr
580  self.wlentry.set('%5.2f'%self.wl)
581  self.wrentry.set('%5.2f'%self.wr)
582  return
583 
584 
585  def on_update(self):
586  self.model.set_time_tick(self.get_tick())
587  v = self.comp.get_velocity()
588  if len(v) == 2:
589  self.set_wheel_velocity(v[0], v[1])
590  self.x, self.y, self.th = self.model.control(self.wl, self.wr)
591  self.th_deg = (self.th * 180 / math.pi) % 360
592  self.comp.set_position((self.x, self.y, self.th_deg))
593  self.xentry.set('%5.2f'%self.x)
594  self.yentry.set('%5.2f'%self.y)
595  self.tentry.set('%5.2f'%self.th_deg)
596  self.draw()
597  return
598 
599 
600  def draw(self):
601  # converting actual coordinate system into display coordinate
602  # system, and drawing figures
603  robotfig = []
604  for pos in self.fig:
605  robotfig.append(self.translate(pos[0], pos[1],
606  self.x, self.y, self.th))
607  if self.id != None:
608  self.canvas.delete(self.id)
609  self.id = self.canvas.create_polygon(robotfig,
610  fill="#00aa00",
611  outline="#eeeeee")
612  return
613 
614 
615  def delete(self):
616  if self.id != None:
617  self.canvas.delete(self.id)
618  return
619 
620 
621  def property_page(self):
622  p = PropertyDialog()
623  p.set_profile(self.name, "DDMobileRobot",
624  "Differential Drive Mobile Robot", "AIST")
625  p.append_parameter("Wheel radius r: ", self.rentry, "[m]")
626  p.append_parameter("Wheel distance d: ", self.dentry, "[m]")
627  p.append_input("Angular velocity (LEFT) wl: ", self.wlentry, "[rad/s]")
628  p.append_input("Angular velocity (RIGHT) wr: ", self.wrentry, "[rad/s]")
629  p.append_output("Robot position x : ", self.xentry, "[m]")
630  p.append_output("Robot position y : ", self.yentry, "[m]")
631  p.append_output("Robot position th: ", self.tentry, "[deg]")
632  p.set_apply_param(self.on_apply_param)
633  p.set_apply_input(self.on_apply_input)
634  p.set_reset_output(self.on_reset_output)
635  p.pack()
636  return
637 
638 
639  def on_reset_output(self):
640  self.set_pos((0.0, 0.0, math.pi/2))
641  return
642 
643 
644  def on_apply_param(self):
645  r = float(self.rentry.get())
646  d = float(self.dentry.get())
647  self.model.set_wheel_radius(r)
648  self.model.set_wheel_distance(d)
649  return
650 
651 
652  def on_apply_input(self):
653  self.wl = float(self.wlentry.get())
654  self.wr = float(self.wrentry.get())
655  return
656 
657 
658 
659 
660 class TkMobileRobot(Frame):
661  def __init__(self, master=None, width=800, height=600):
662  Frame.__init__(self, master)
663 
664  # canvas properties
665  self.width = width
666  self.height = height
667  # zero of canvas
668  self.x0 = width/2
669  self.y0 = height/2
670 
671  self.wd = 150
672 
673  self.robots = {}
674 
675  self.robot = None
676  self.postext = None
677 
678  self.scale = 1.0
679  self.scale_var = DoubleVar()
680  self.scale_var.set(self.scale)
681 
682  self.grid_pitch = 50
683 
684  self.tick = 0.1
685  self.default_tick = 0.1
686  self.tickscale_var = DoubleVar()
687  self.tickscale_var.set(self.tick)
688 
689  self.axis_check = StringVar()
690  self.axis_check.set("on")
691  self.grid_check = StringVar()
692  self.grid_check.set("on")
693  self.rname_check = StringVar()
694  self.rname_check.set("on")
695  self.rnames = {}
696 
697  self.robot_kind_var = StringVar()
698  self.robot_factory = {"DDMobileRobot": DDMobileRobot}
699 
700 
701 
702  self.init()
703  self.pack()
704 
705 
706  self.after(20, self.on_update)
707  return
708 
709  def init(self):
710  self.canvas = Canvas(self, bg="#eeeeee",
711  width = self.width, height = self.height)
712  self.canvas.pack(side=LEFT)
713 
714  self.can_grid = CanvasGrid(self.canvas, self.x0, self.y0,
715  self.width, self.height, self.grid_pitch,
716  "#aaaaaa", 1)
717  self.can_axis = CanvasAxis(self.canvas, self.width, self.height)
718 
719  self.frame = Frame(self)
720  self.frame.pack(side=LEFT)
721 
722  # Screen control
723  self.scrctrl_frame = Frame(self.frame, width=self.wd, height=300,
724  relief=GROOVE, bd=2)
725  self.scrctrl_frame.pack(side=TOP, fill=X)
726  self.create_scale(self.scrctrl_frame)
728 
729  # Robot manager
730  self.robomgr_frame = Frame(self.frame, width=self.wd, height=300,
731  relief=GROOVE, bd=2)
732  self.robomgr_frame.pack(side=TOP)
735  return
736 
737 
738  def on_update(self):
739  for o in self.robots.keys():
740  self.robots[o].on_update()
741  for r in self.rnames.keys():
742  self.rnames[r].draw()
743  self.after(20, self.on_update)
744  return
745 
746 
747  def get_tick(self):
748  return self.tick
749 
750 
751  def get_canvas(self):
752  return self.canvas
753 
754 
755  def get_translation(self):
756  return self.real_to_canvas
757 
758 
759  #------------------------------------------------------------
760  # Scale control set
761  def create_scale(self, frame):
762  dummy = Frame(frame, width=self.wd)
763  dummy.pack(side=TOP)
764  sl = Scale(frame, from_=0, to=10, resolution=0.01,
765  label="Scale Factor", command=self.on_scale,
766  variable=self.scale_var, orient=HORIZONTAL)
767  bt = Button(frame, text="Reset Scale", command=self.reset_scale)
768  sl.pack(side=TOP, fill=X)
769  bt.pack(side=TOP, fill=X)
770 
771  sl = Scale(frame, from_=0.001, to=1, resolution=0.001,
772  label="Time tick [s]", command=self.on_tickchange,
773  variable=self.tickscale_var, orient=HORIZONTAL)
774  bt = Button(frame, text="Reset Tick", command=self.reset_tickscale)
775  sl.pack(side=TOP, fill=X)
776  bt.pack(side=TOP, fill=X)
777  return
778 
779 
780  def on_scale(self, val):
781  v = float(val)
782  if v == 0.0:
783  pitch = 0
784  else:
785  pitch = self.grid_pitch/v
786  self.scale = v
787  self.can_grid.set_pitch(pitch)
788  return
789 
790 
791  def reset_scale(self):
792  self.scale_var.set(1.)
793  pitch = self.grid_pitch/1.0
794  self.scale = 1.0
795  self.can_grid.set_pitch(pitch)
796  return
797 
798 
799  def on_tickchange(self, val):
800  v = self.tickscale_var.get()
801  if v == 0.0:
802  self.tick = 0
803  else:
804  self.tick = v
805  return
806 
807  def reset_tickscale(self):
808  self.tick = self.default_tick
809  self.tickscale_var.set(self.default_tick)
810  return
811  # end of Scale widget set
812  #------------------------------------------------------------
813 
814  #------------------------------------------------------------
815  # Canvas control set
816  def create_checkbutton(self, frame):
817  axis = Checkbutton(frame, text="Axis",
818  onvalue="on", offvalue="off",
819  justify=LEFT, anchor=W,
820  variable=self.axis_check,
821  command=self.can_axis.toggle)
822  grid = Checkbutton(frame, text="Grid",
823  onvalue="on", offvalue="off",
824  justify=LEFT, anchor=W,
825  variable=self.grid_check,
826  command=self.can_grid.toggle)
827  rname = Checkbutton(frame, text="Robots' name",
828  onvalue="on", offvalue="off",
829  justify=LEFT, anchor=W,
830  variable=self.rname_check,
831  command=self.on_rname_toggle)
832  for w in [axis, grid, rname]:
833  w.pack(side=TOP, anchor=W, fill=X)
834  return
835 
836 
837  def on_rname_toggle(self):
838  for r in self.rnames.keys():
839  self.rnames[r].toggle()
840  return
841 
842 
843  # end of Canvas control set
844  #------------------------------------------------------------
845 
846 
847  #------------------------------------------------------------
848  # Robot creator control set
849  def create_robotcreator(self, frame):
850  lb = Label(frame, text="Robot Type", anchor=W, justify=LEFT)
851 
852  om = OptionMenu(frame, label="Type: ", variable=self.robot_kind_var)
853  for opt in self.robot_factory.keys():
854  om.add_command(opt, label=opt)
855  self.robot_kind_var.set(self.robot_factory.keys()[0])
856 
857  creater = Button(frame, text="Create", command=self.create_robot)
858  deleter = Button(frame, text="Delete", command=self.delete_robot)
859 
860  om.pack(side=TOP, fill=X)
861  creater.pack(side=TOP, fill=X)
862  deleter.pack(side=TOP, fill=X)
863  return
864 
865 
866  def create_robotlist(self, frame):
867  f = Frame(frame, width=self.wd, height=200)
868  f.pack(side=TOP, fill=BOTH, expand=1)
869  ys = Scrollbar(f, orient = VERTICAL)
870  ys.grid(row = 0, column=1, sticky = N+S)
871  xs = Scrollbar(f, orient = HORIZONTAL)
872  xs.grid(row = 1, column=0, sticky = E+W)
873 
874  self.rlistbox = Listbox(f,
875  xscrollcommand = xs.set,
876  yscrollcommand = ys.set,
877  selectmode = 'single',
878  setgrid = TRUE,
879  height=20)
880  self.rlistbox.grid(row = 0, column = 0, sticky = N+S+E+W,)
881  xs['command']=self.rlistbox.xview
882  ys['command']=self.rlistbox.yview
883  self.rlistbox.bind("<Double-Button-1>", self.on_clickrlistbox)
884  return
885 
886 
887  def on_clickrlistbox(self, event):
888  index = self.rlistbox.curselection()
889  if len(index) > 0:
890  robot_name = self.rlistbox.get(index[0])
891  self.robots[robot_name].property_page()
892  return
893 
894 
895  def create_robot(self):
896  kind = self.robot_kind_var.get()
897  robot = self.robot_factory[kind](self)
898 
899  self.rlistbox.insert(END, robot.get_name())
900  self.rnames[robot.get_name()] = RobotTitle(self,
901  robot,
902  robot.get_name(),
903  20)
904  self.robots[robot.get_name()] = robot
905  return
906 
907 
908  def delete_robot(self):
909  index = self.rlistbox.curselection()
910  if len(index) > 0:
911  robot_name = self.rlistbox.get(index[0])
912  r = self.rnames.pop(robot_name)
913  del(r)
914  r = self.robots.pop(robot_name)
915  del(r)
916  self.rlistbox.delete(index)
917  return
918 
919  # end of Robot creator control set
920  #------------------------------------------------------------
921 
922 
923  #------------------------------------------------------------
924  #
925  def real_to_canvas(self, x, y, dx, dy, dt):
926  # Simulator coordinate system -> display coordinate system
927  # x, y: original position
928  # dx, dy, dt: translation and rotation vector
929  # translation and rotation
930  x_tmp = (math.cos(dt) * x - math.sin(dt) * y + dx)/self.scale
931  y_tmp = (math.sin(dt) * x + math.cos(dt) * y + dy)/self.scale
932  # align to canvas coordinate system (origin is center and y+ is upward)
933  xo = x_tmp + self.x0
934  yo = -y_tmp + self.y0
935  return xo, yo
936 
937 
939  profile = OpenRTM_aist.Properties(defaults_str=tkmobilerobotsimulator_spec)
940  manager.registerFactory(profile,
941  TkMobileRobotSimulator,
942  OpenRTM_aist.Delete)
943  return
944 
945 
946 def main():
947  m = TkMobileRobot(Tk())
948  m.master.title("Tk Mobile Robot Simulator")
949  mgr = OpenRTM_aist.Manager.init(sys.argv)
950  mgr.activateManager()
951  profile = OpenRTM_aist.Properties(defaults_str=tkmobilerobotsimulator_spec)
952  mgr.registerFactory(profile, TkMobileRobotSimulator, OpenRTM_aist.Delete)
953  mgr.runManager(True)
954  m.mainloop()
955 
956 if __name__ == '__main__':
957  main()
def insert(seq, elem, index)
Insert the element to the CORBA sequence.
The Properties class represents a persistent set of properties.
Definition: Properties.py:83
def __init__(self, canvas, x0, y0, width, height, pitch, color, linewd)
def addOutPort(self, name, outport)
Definition: RTObject.py:2765
def addInPort(self, name, inport)
Definition: RTObject.py:2721
def __init__(self, simulator, radius=2, wheeld=20, pos=(0, 0, math.pi/2))
InPort template class.
Definition: InPort.py:58
def append(dest, src)
Definition: NVUtil.py:386


openrtm_aist_python
Author(s): Shinji Kurihara
autogenerated on Mon Feb 28 2022 23:01:07