TkJoyStickComp.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- Python -*-
3 
4 import sys
5 import time
6 sys.path.append(".")
7 
8 # Import RTM module
9 import OpenRTM_aist
10 import RTC
11 
12 # for convert()
13 import math
14 
15 # This module's spesification
16 # <rtc-template block="module_spec">
17 tkjoystick_spec = ["implementation_id", "TkJoyStick",
18  "type_name", "TkJoyStick",
19  "description", "Sample component for MobileRobotCanvas component",
20  "version", "1.0",
21  "vendor", "Noriaki Ando and Shinji Kurihara",
22  "category", "example",
23  "activity_type", "DataFlowComponent",
24  "max_instance", "10",
25  "language", "Python",
26  "lang_type", "SCRIPT",
27  ""]
28 # </rtc-template>
29 
30 import tkjoystick
31 
32 class Position:
33  def __init__(self, x = 0.0, y = 0.0, r = 0.0, th = 0.0):
34  self.x = x
35  self.y = y
36  self.r = r
37  self.th = th
38 
39 position = Position()
40 #tkJoyCanvas = tkjoystick.TkJoystick()
41 
42 
43 class TkJoyStick(OpenRTM_aist.DataFlowComponentBase):
44  def __init__(self, manager):
45  OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
46 
47  self._d_pos = RTC.TimedFloatSeq(RTC.Time(0,0),[])
48  self._posOut = OpenRTM_aist.OutPort("pos", self._d_pos)
49  self._d_vel = RTC.TimedFloatSeq(RTC.Time(0,0),[])
50  self._velOut = OpenRTM_aist.OutPort("vel", self._d_vel)
51 
52 
53  # Set OutPort buffers
54  self.registerOutPort("pos",self._posOut)
55  self.registerOutPort("vel",self._velOut)
56 
57  self._k = 1.0
58  self.x = 0.0
59  self.y = 0.0
60 
61 
62  def onInitialize(self):
63  # Bind variables and configuration variable
64 
65  return RTC.RTC_OK
66 
67 
68  def onShutdown(self, ec_id):
69  return RTC.RTC_OK
70 
71 
72  def onExecute(self, ec_id):
73  self._d_pos.data = [self.x, self.y]
74  self._d_vel.data = self.convert(self.x, self.y)
75  self._posOut.write()
76  self._velOut.write()
77 
78  return RTC.RTC_OK
79 
80  """
81  \brief Canvas$B$N%G!<%?$r(BMobileRobotCanvas$BMQ$N%G!<%?$KJQ49$9$k!#(B
82  """
83  def convert(self, x, y):
84  _th = math.atan2(y,x)
85  _v = self._k * math.hypot(x, y)
86  _vl = _v * math.cos(_th - (math.pi/4.0))
87  _vr = _v * math.sin(_th - (math.pi/4.0))
88  print(x, y, _vl, _vr)
89  return [_vl, _vr]
90 
91  def set_pos(self, pos, pol):
92  self.x = pos[0]
93  self.y = pos[1]
94  self.r = pol[0]
95  self.th = pol[1]
96 
97 
98 
99 #def MyModuleInit(manager):
100 # profile = OpenRTM_aist.Properties(defaults_str=tkjoystick_spec)
101 # manager.registerFactory(profile,
102 # TkJoyStick,
103 # OpenRTM_aist.Delete)
104 #
105 # # Create a component
106 # comp = manager.createComponent("TkJoyStick")
107 
108 
109 
110 def main():
111  tkJoyCanvas = tkjoystick.TkJoystick()
112  tkJoyCanvas.master.title("TkJoystick")
113  mgr = OpenRTM_aist.Manager.init(sys.argv)
114  mgr.activateManager()
115 
116  # Register component
117  profile = OpenRTM_aist.Properties(defaults_str=tkjoystick_spec)
118  mgr.registerFactory(profile,
119  TkJoyStick,
120  OpenRTM_aist.Delete)
121  # Create a component
122  comp = mgr.createComponent("TkJoyStick")
123 
124  tkJoyCanvas.set_on_update(comp.set_pos)
125  mgr.runManager(True)
126  tkJoyCanvas.mainloop()
127 
128 if __name__ == "__main__":
129  main()
130 
def __init__(self, x=0.0, y=0.0, r=0.0, th=0.0)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05