00001
00002
00003
00004 import sys
00005 import time
00006 sys.path.append(".")
00007
00008
00009 import OpenRTM_aist
00010 import RTC
00011
00012
00013 import math
00014
00015
00016
00017 tkjoystick_spec = ["implementation_id", "TkJoyStick",
00018 "type_name", "TkJoyStick",
00019 "description", "Sample component for MobileRobotCanvas component",
00020 "version", "1.0",
00021 "vendor", "Noriaki Ando and Shinji Kurihara",
00022 "category", "example",
00023 "activity_type", "DataFlowComponent",
00024 "max_instance", "10",
00025 "language", "Python",
00026 "lang_type", "SCRIPT",
00027 ""]
00028
00029
00030 import tkjoystick
00031
00032 class Position:
00033 def __init__(self, x = 0.0, y = 0.0, r = 0.0, th = 0.0):
00034 self.x = x
00035 self.y = y
00036 self.r = r
00037 self.th = th
00038
00039 position = Position()
00040
00041
00042
00043 class TkJoyStick(OpenRTM_aist.DataFlowComponentBase):
00044 def __init__(self, manager):
00045 OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
00046
00047 self._d_pos = RTC.TimedFloatSeq(RTC.Time(0,0),[])
00048 self._posOut = OpenRTM_aist.OutPort("pos", self._d_pos)
00049 self._d_vel = RTC.TimedFloatSeq(RTC.Time(0,0),[])
00050 self._velOut = OpenRTM_aist.OutPort("vel", self._d_vel)
00051
00052
00053
00054 self.registerOutPort("pos",self._posOut)
00055 self.registerOutPort("vel",self._velOut)
00056
00057 self._k = 1.0
00058 self.x = 0.0
00059 self.y = 0.0
00060
00061
00062 def onInitialize(self):
00063
00064
00065 return RTC.RTC_OK
00066
00067
00068 def onShutdown(self, ec_id):
00069 return RTC.RTC_OK
00070
00071
00072 def onExecute(self, ec_id):
00073 self._d_pos.data = [self.x, self.y]
00074 self._d_vel.data = self.convert(self.x, self.y)
00075 self._posOut.write()
00076 self._velOut.write()
00077
00078 return RTC.RTC_OK
00079
00080 """
00081 \brief Canvas$B$N%G!<%?$r(BMobileRobotCanvas$BMQ$N%G!<%?$KJQ49$9$k!#(B
00082 """
00083 def convert(self, x, y):
00084 _th = math.atan2(y,x)
00085 _v = self._k * math.hypot(x, y)
00086 _vl = _v * math.cos(_th - (math.pi/4.0))
00087 _vr = _v * math.sin(_th - (math.pi/4.0))
00088 print x, y, _vl, _vr
00089 return [_vl, _vr]
00090
00091 def set_pos(self, pos, pol):
00092 self.x = pos[0]
00093 self.y = pos[1]
00094 self.r = pol[0]
00095 self.th = pol[1]
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 def main():
00111 tkJoyCanvas = tkjoystick.TkJoystick()
00112 tkJoyCanvas.master.title("TkJoystick")
00113 mgr = OpenRTM_aist.Manager.init(sys.argv)
00114 mgr.activateManager()
00115
00116
00117 profile = OpenRTM_aist.Properties(defaults_str=tkjoystick_spec)
00118 mgr.registerFactory(profile,
00119 TkJoyStick,
00120 OpenRTM_aist.Delete)
00121
00122 comp = mgr.createComponent("TkJoyStick")
00123
00124 tkJoyCanvas.set_on_update(comp.set_pos)
00125 mgr.runManager(True)
00126 tkJoyCanvas.mainloop()
00127
00128 if __name__ == "__main__":
00129 main()
00130