TkJoyStickComp.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- Python -*-
00003 
00004 import sys
00005 import time
00006 sys.path.append(".")
00007 
00008 # Import RTM module
00009 import OpenRTM_aist
00010 import RTC
00011 
00012 # for convert()
00013 import math
00014 
00015 # This module's spesification
00016 # <rtc-template block="module_spec">
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 # </rtc-template>
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 #tkJoyCanvas = tkjoystick.TkJoystick()
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                 # Set OutPort buffers
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                 # Bind variables and configuration variable
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 #def MyModuleInit(manager):
00100 #    profile = OpenRTM_aist.Properties(defaults_str=tkjoystick_spec)
00101 #    manager.registerFactory(profile,
00102 #                            TkJoyStick,
00103 #                            OpenRTM_aist.Delete)
00104 #
00105 #    # Create a component
00106 #    comp = manager.createComponent("TkJoyStick")
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         # Register component
00117         profile = OpenRTM_aist.Properties(defaults_str=tkjoystick_spec)
00118         mgr.registerFactory(profile,
00119                             TkJoyStick,
00120                             OpenRTM_aist.Delete)
00121         # Create a component
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 


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