NXTRTC20.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 NXTBrick20
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 
36  def __init__(self, manager):
37  OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
38 
39  # initialize of configuration-data.
40  # <rtc-template block="configurations">
41  self._map = [['A', 'B']]
42  self._nxtbrick = None
43  self._mapping = {'A':0,'B':1,'C':2}
44 
45  def onInitialize(self):
46  # DataPorts initialization
47  # <rtc-template block="data_ports">
48  self._d_vel = RTC.TimedFloatSeq(RTC.Time(0,0),[])
49  self._velIn = OpenRTM_aist.InPort("vel", self._d_vel)
50  self.addInPort("vel",self._velIn)
51  self._d_pos = RTC.TimedFloatSeq(RTC.Time(0,0),[])
52  self._posOut = OpenRTM_aist.OutPort("pos", self._d_pos)
53  self.addOutPort("pos",self._posOut)
54  self._d_sens = RTC.TimedFloatSeq(RTC.Time(0,0),[])
55  self._sensOut = OpenRTM_aist.OutPort("sens", self._d_sens)
56  self.addOutPort("sens",self._sensOut)
57 
58  # Bind variables and configuration variable
59  # <rtc-template block="bind_config">
60  self.bindParameter("map", self._map, "A,B")
61 
62  # create NXTBrick object
63  try:
64  print "Connecting to NXT brick ...."
66  print "Connection established."
67  except:
68  print "NXTBrick connection failed."
69  return RTC.RTC_ERROR
70 
71  return RTC.RTC_OK
72 
73  def onFinalize(self):
74  self._nxtbrick.close()
75 
76  def onActivated(self, ec_id):
77  self._nxtbrick.setMotors([0,0,0])
78  # reset NXTBrick's position.
79  self._nxtbrick.resetPosition()
80  return RTC.RTC_OK
81 
82  def onDeactivated(self, ec_id):
83  self._nxtbrick.setMotors([0,0,0])
84  # reset NXTBrick's position.
85  self._nxtbrick.resetPosition()
86 
87  return RTC.RTC_OK
88 
89  def onExecute(self, ec_id):
90  try:
91  # check new data.
92  if self._velIn.isNew():
93  # read velocity data from inport.
94  self._d_vel = self._velIn.read()
95 
96  vel_ = [0,0,0]
97  vel_[self._mapping[self._map[0][0]]] = self._d_vel.data[0]
98  vel_[self._mapping[self._map[0][1]]] = self._d_vel.data[1]
99  # set velocity
100  self._nxtbrick.setMotors(vel_)
101  else:
102  print "buffer empty"
103 
104  # get sensor data.
105  sensor_ = self._nxtbrick.getSensors()
106  if sensor_:
107  self._d_sens.data = sensor_
108  # write sensor data to outport.
109  self._sensOut.write()
110 
111  # get position data.
112  position_ = self._nxtbrick.getMotors()
113  if position_:
114  self._d_pos.data = \
115  [position_[self._mapping[self._map[0][0]]], \
116  position_[self._mapping[self._map[0][1]]]]
117  # write position data to outport.
118  self._posOut.write()
119  except:
120  print sys.exc_info()[1]
121 
122  return RTC.RTC_OK
123 
124 
125 
126 def NXTRTCInit(manager):
127  profile = OpenRTM_aist.Properties(defaults_str=nxtrtc_spec)
128  manager.registerFactory(profile,
129  NXTRTC,
130  OpenRTM_aist.Delete)
131 
132 
133 def MyModuleInit(manager):
134  NXTRTCInit(manager)
135 
136  # Create a component
137  comp = manager.createComponent("NXTRTC")
138 
139 
140 
141 def main():
142  mgr = OpenRTM_aist.Manager.init(sys.argv)
143  mgr.setModuleInitProc(MyModuleInit)
144  mgr.activateManager()
145  mgr.runManager()
146 
147 if __name__ == "__main__":
148  main()
149 
150 
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