NXTRTC_callback.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding:utf-8 -*-
3 # -*- Python -*-
4 
5 import sys
6 import time
7 sys.path.append(".")
8 
9 # Import RTM module
10 import RTC
11 import OpenRTM_aist
12 
13 
14 # import NXTBrick class
15 import NXTBrick
16 
17 
18 # This module's spesification
19 # <rtc-template block="module_spec">
20 nxtrtc_spec = ["implementation_id", "NXTRTC",
21  "type_name", "NXTRTC",
22  "description", "NXT sample component",
23  "version", "0.1",
24  "vendor", "AIST",
25  "category", "example",
26  "activity_type", "DataFlowComponent",
27  "max_instance", "10",
28  "language", "Python",
29  "lang_type", "SCRIPT",
30  "conf.default.map", "A,B",
31  ""]
32 
33 # </rtc-template>
34 
35 # @class CallBackClass
36 # @brief callback class
37 #
38 # when data is written in the buffer of InPort,
39 # it is called.
41  def __init__(self, nxtbrick_, map_):
42  self._nxtbrick = nxtbrick_
43  self._map = map_
44  self._mapping = {'A':0,'B':1,'C':2}
45 
46  def __call__(self, pData):
47  vel_ = [0,0,0]
48  vel_[self._mapping[self._map[0][0]]] = pData.data[0]
49  vel_[self._mapping[self._map[0][1]]] = pData.data[1]
50  # set velocity
51  self._nxtbrick.setMotors(vel_)
52 
53 
55  def __init__(self, manager):
56  OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
57 
58  # initialize of configuration-data.
59  # <rtc-template block="configurations">
60  self._map = [['A', 'B']]
61  self._nxtbrick = None
62  self._mapping = {'A':0,'B':1,'C':2}
63 
64  def onInitialize(self):
65  # DataPorts initialization
66  # <rtc-template block="data_ports">
67  self._d_vel = RTC.TimedFloatSeq(RTC.Time(0,0),[])
68  self._velIn = OpenRTM_aist.InPort("vel", self._d_vel)
69  self.addInPort("vel",self._velIn)
70  self._d_pos = RTC.TimedFloatSeq(RTC.Time(0,0),[])
71  self._posOut = OpenRTM_aist.OutPort("pos", self._d_pos)
72  self.addOutPort("pos",self._posOut)
73  self._d_sens = RTC.TimedFloatSeq(RTC.Time(0,0),[])
74  self._sensOut = OpenRTM_aist.OutPort("sens", self._d_sens)
75  self.addOutPort("sens",self._sensOut)
76 
77  # Bind variables and configuration variable
78  # <rtc-template block="bind_config">
79  self.bindParameter("map", self._map, "A,B")
80 
81  # create NXTBrick object
82  try:
83  print "Connecting to NXT brick ...."
85  print "Connection established."
86  except:
87  print "NXTBrick connection failed."
88  return RTC.RTC_ERROR
89 
90  # set callback class
91  self._velIn.setOnWrite(CallBackClass(self._ntxbrick,self._map))
92 
93  return RTC.RTC_OK
94 
95  def onFinalize(self):
96  self._nxtbrick.close()
97 
98  def onActivated(self, ec_id):
99  # reset NXTBrick's position.
100  self._nxtbrick.resetPosition()
101 
102  return RTC.RTC_OK
103 
104 
105  def onDeactivated(self, ec_id):
106  # reset NXTBrick's position.
107  self._nxtbrick.resetPosition()
108 
109  return RTC.RTC_OK
110 
111 
112  def onExecute(self, ec_id):
113  # get sensor data.
114  sensor_ = self._nxtbrick.getSensors()
115  if sensor_:
116  self._d_sens.data = [sensor_[3]]
117  # write sensor data to outport.
118  self._sensOut.write()
119 
120  # get position data.
121  position_ = self._nxtbrick.getMotors()
122  if position_:
123  self._d_pos.data = [position_[self._mapping[self._map[0][0]]][9],position_[self._mapping[self._map[0][1]]][9]]
124  # write position data to outport.
125  self._posOut.write()
126 
127  return RTC.RTC_OK
128 
129 
130 
131 def NXTRTC_callbackInit(manager):
132  profile = OpenRTM_aist.Properties(defaults_str=nxtrtc_spec)
133  manager.registerFactory(profile,
134  NXTRTC,
135  OpenRTM_aist.Delete)
136 
137 
138 def MyModuleInit(manager):
139  NXTRTC_callbackInit(manager)
140 
141  # Create a component
142  comp = manager.createComponent("NXTRTC")
143 
144 
145 
146 def main():
147  mgr = OpenRTM_aist.Manager.init(len(sys.argv), sys.argv)
148  #mgr = OpenRTM_aist.Manager.init(sys.argv)
149  mgr.setModuleInitProc(MyModuleInit)
150  mgr.activateManager()
151  mgr.runManager()
152 
153 if __name__ == "__main__":
154  main()
155 
156 
def bindParameter(self, param_name, var, def_val, trans=None)
template <typename vartype>=""> bool bindParameter(const char* param_name, VarType& var...
Definition: RTObject.py:2588
The Properties class represents a persistent set of properties.
Definition: Properties.py:83
def addOutPort(self, name, outport)
Definition: RTObject.py:2765
def addInPort(self, name, inport)
Definition: RTObject.py:2721
InPort template class.
Definition: InPort.py:58


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