샘플 코드 설명

샘플 코드의 설명
샘플 코드의 설명(콜백 오브젝트의 사용 예)

샘플 코드의 설명

여기로부터, NXTRTC.py 에 방금전 작성한 NXTBrick.py의 기능을 작성해 갑니다.

NXTBrick.py의 import NXTBrick 클래스를 사용하기 위해서 NXBrick.py를 import 합니다. import의 기술 방법은, 아래와 같이 확장자(extension)(.py)를 제외한 파일명을 지정합니다.
```
 import NXTBrick
```

onInitialize(self)의 구현 onInitialize()에서 NXTBrick 클래스의 인스턴스화를 실시합니다. NXTBrick 클래스의 인스턴스화 과정에서 에러가 발생했을 경우, RTC_ERROR가 돌아와 종료 상태로 천이 합니다.
```
        # create NXTBrick object
        try:
            self._nxtbrick = NXTBrick.NXTBrick()
        except:
            print "NXTBrick create failed."""
            return RTC.RTC_ERROR
```

onActivated(self, ec_id), onDeactivated(self, ec_id)의 구현 onActivated(), onDeactivated()에서는 NXTBrick 클래스의 resetPosition() 메소드를 부릅니다.
```
        self._nxtbrick.resetPosition() 
```

onExecute(self, ec_id)의 구현 onExecute()에서는 아래와 같은 처리를 실시하고 있습니다.

데이터 InPort로부터의 속도의 읽기.
Configuration 기능을 이용해 NXT의 어느 포트에 속도 출력을 실시하는지를 결정.
NXTBrick의 setMotors() 메소드에서 모터에 속도를 출력.
NXTBrick의 getSensors()에서 NTX의 초음파 센서값을 취득해, 데이터가 있으면 OutPort으로 출력.
NXTBrick의 getMotors()에서 NTX의 회전 각도[deg]를 취득해, 데이터가 있으면 OutPort으로 출력.

상기를 정리한 샘플 코드는 아래와 같습니다.


 #!/usr/bin/env python
 # -*- coding:shift_jis -*-
 # -*- Python -*-
 
 import sys
 import time
 sys.path.append(".")
 
 # Import RTM module
 import OpenRTM
 import RTC
 
 
 # import NXTBrick class
 import NXTBrick
 
 
 # This module's spesification
 # <rtc-template block="module_spec">
 nxtrtc_spec = ["implementation_id", "NXTRTC", 
 "type_name", "NXTRTC", 
 "description", "NXT sample component", 
 "version", "0.1", 
 "vendor", "AIST", 
 "category", "example", 
 "activity_type", "DataFlowComponent", 
 "max_instance", "10", 
 "language", "Python", 
 "lang_type", "SCRIPT",
 "conf.default.map", "A,B",
 ""]
 
 # </rtc-template>
 
 class NXTRTC(OpenRTM.DataFlowComponentBase):
 def __init__(self, manager):
 OpenRTM.DataFlowComponentBase.__init__(self, manager)
 
 # DataPorts initialization
 # <rtc-template block="data_ports">
 self._d_vel = RTC.TimedFloatSeq(RTC.Time(0,0),[])
 self._velIn = OpenRTM.InPort("vel", self._d_vel, OpenRTM.RingBuffer(8))
 self.registerInPort("vel",self._velIn)
 self._d_pos = RTC.TimedFloatSeq(RTC.Time(0,0),[])
 self._posOut = OpenRTM.OutPort("pos", self._d_pos, OpenRTM.RingBuffer(8))
 self.registerOutPort("pos",self._posOut)
 self._d_sens = RTC.TimedFloatSeq(RTC.Time(0,0),[])
 self._sensOut = OpenRTM.OutPort("sens", self._d_sens, OpenRTM.RingBuffer(8))
 self.registerOutPort("sens",self._sensOut)
 
 # initialize of configuration-data.
 # <rtc-template block="configurations">
 self._map = [['A', 'B']]
 self._nxtbrick = None
 self._mapping = {'A':0,'B':1,'C':2}
 
 def onInitialize(self):
 # Bind variables and configuration variable
 # <rtc-template block="bind_config">
 self.bindParameter("map", self._map, "A,B")
 
 # create NXTBrick object
 try:
 print "Connecting to NXT brick ...."
 self._nxtbrick = NXTBrick.NXTBrick()
 print "Connection established."
 except:
 print "NXTBrick connection failed."
 return RTC.RTC_ERROR
 
 return RTC.RTC_OK
 
 def onFinalize(self):
 self._nxtbrick.close()
 
 
 def onActivated(self, ec_id):
 # reset NXTBrick's position.
 self._nxtbrick.resetPosition()
 
 return RTC.RTC_OK
 
 def onDeactivated(self, ec_id):
 # reset NXTBrick's position.
 self._nxtbrick.resetPosition()
 
 return RTC.RTC_OK
 
 def onExecute(self, ec_id):
 cnt = 0
 # check new data.
 if self._velIn.isNew():
 # read velocity data from inport.
 self._d_vel = self._velIn.read()
 vel_ = [0,0,0]
 vel_[self._mapping[self._map[0][0]]] = self._d_vel.data[0]
 vel_[self._mapping[self._map[0][1]]] = self._d_vel.data[1]
 # set velocity
 self._nxtbrick.setMotors(vel_)
 
 # get sensor data.
 sensor_ = self._nxtbrick.getSensors()
 if sensor_:
 self._d_sens.data = sensor_
 self._sensOut.write()
 
 # get position data.
 position_ = self._nxtbrick.getMotors()
 if position_:
 self._d_pos.data = \
 [position_[self._mapping[self._map[0][0]]][9], \
 position_[self._mapping[self._map[0][1]]][9]]
 # write position data to outport.
 self._posOut.write()
 
 return RTC.RTC_OK
 
 
 
 def MyModuleInit(manager):
 profile = OpenRTM.Properties(defaults_str=nxtrtc_spec)
 manager.registerFactory(profile,
 NXTRTC,
 OpenRTM.Delete)
 
 # Create a component
 comp = manager.createComponent("NXTRTC")
 
 
 
 def main():
 mgr = OpenRTM.Manager.init(len(sys.argv), sys.argv)
 #mgr = OpenRTM.Manager.init(sys.argv)
 mgr.setModuleInitProc(MyModuleInit)
 mgr.activateManager()
 mgr.runManager()
 
 if __name__ == "__main__":
 main()


샘플 코드의 설명(콜백 오브젝트의 사용 예)

이 샘플에서는 상기의 샘플에 콜백 OnWrite를 추가해, InPort의 버퍼에 데이터가 써졌을 때에 모터에의 속도를 출력하도록 확장합니다.

콜백 클래스
아래와 같이 콜백 클래스를 기술합니다.


 # @class CallBackClass
 # @brief callback class
 #
 # when data is written in the buffer of InPort,
 # it is called.
 class CallBackClass:
 def __init__(self, nxtbrick_, map_):
 self._nxtbrick = nxtbrick_
 self._map = map_
 self._mapping = {'A':0,'B':1,'C':2}
 
 def __call__(self, pData):
 vel_ = [0,0,0]
 vel_[self._mapping[self._map[0][0]]] = pData.data[0]
 vel_[self._mapping[self._map[0][1]]] = pData.data[1]
 # set velocity
 self._nxtbrick.setMotors(vel_)


이 클래스에서는 NTXBrick 클래스의 오브젝트와 컨피그레이션 변수 map를 인수로 합니다.

콜백 클래스의 등록
아래와 같이, setOnWrite() 메소드에서 콜백 오브젝트를 등록합니다.

   # set callback class
   self._velIn.setOnWrite(CallBackClass(self._ntxbrick,self._map))

setOnWrite()에 의해, InPort에 데이터가 써졌을 때에, CallBackClass의 call() 메소드가 불리게 됩니다.

위의 내용을 정리한 샘플 코드는 아래와 같습니다.


 #!/usr/bin/env python
 # -*- coding:shift_jis -*-
 # -*- Python -*-
 
 import sys
 import time
 sys.path.append(".")
 
 # Import RTM module
 import OpenRTM
 import RTC
 
 
 # import NXTBrick class
 import NXTBrick
 
 
 # This module's spesification
 # <rtc-template block="module_spec">
 nxtrtc_spec = ["implementation_id", "NXTRTC", 
 "type_name", "NXTRTC", 
 "description", "NXT sample component", 
 "version", "0.1", 
 "vendor", "AIST", 
 "category", "example", 
 "activity_type", "DataFlowComponent", 
 "max_instance", "10", 
 "language", "Python", 
 "lang_type", "SCRIPT",
 "conf.default.map", "A,B",
 ""]
 
 # </rtc-template>
 
 # @class CallBackClass
 # @brief callback class
 #
 # when data is written in the buffer of InPort,
 # it is called.
 class CallBackClass:
 def __init__(self, nxtbrick_, map_):
 self._nxtbrick = nxtbrick_
 self._map = map_
 self._mapping = {'A':0,'B':1,'C':2}
 
 def __call__(self, pData):
 vel_ = [0,0,0]
 vel_[self._mapping[self._map[0][0]]] = pData.data[0]
 vel_[self._mapping[self._map[0][1]]] = pData.data[1]
 # set velocity
 self._nxtbrick.setMotors(vel_)
 
 
 class NXTRTC(OpenRTM.DataFlowComponentBase):
 def __init__(self, manager):
 OpenRTM.DataFlowComponentBase.__init__(self, manager)
 
 # DataPorts initialization
 # <rtc-template block="data_ports">
 self._d_vel = RTC.TimedFloatSeq(RTC.Time(0,0),[])
 self._velIn = OpenRTM.InPort("vel", self._d_vel, OpenRTM.RingBuffer(8))
 self.registerInPort("vel",self._velIn)
 self._d_pos = RTC.TimedFloatSeq(RTC.Time(0,0),[])
 self._posOut = OpenRTM.OutPort("pos", self._d_pos, OpenRTM.RingBuffer(8))
 self.registerOutPort("pos",self._posOut)
 self._d_sens = RTC.TimedFloatSeq(RTC.Time(0,0),[])
 self._sensOut = OpenRTM.OutPort("sens", self._d_sens, OpenRTM.RingBuffer(8))
 self.registerOutPort("sens",self._sensOut)
 
 # initialize of configuration-data.
 # <rtc-template block="configurations">
 self._map = [['A', 'B']]
 self._nxtbrick = None
 self._mapping = {'A':0,'B':1,'C':2}
 
 def onInitialize(self):
 # Bind variables and configuration variable
 # <rtc-template block="bind_config">
 self.bindParameter("map", self._map, "A,B")
 
 # create NXTBrick object
 try:
 print "Connecting to NXT brick ...."
 self._nxtbrick = NXTBrick.NXTBrick()
 print "Connection established."
 except:
 print "NXTBrick connection failed."
 return RTC.RTC_ERROR
 
 # set callback class
 self._velIn.setOnWrite(CallBackClass(self._ntxbrick,self._map))
 
 return RTC.RTC_OK
 
 def onFinalize(self):
 self._nxtbrick.close()
 
 def onActivated(self, ec_id):
 # reset NXTBrick's position.
 self._nxtbrick.resetPosition()
 
 return RTC.RTC_OK
 
 
 def onDeactivated(self, ec_id):
 # reset NXTBrick's position.
 self._nxtbrick.resetPosition()
 
 return RTC.RTC_OK
 
 
 def onExecute(self, ec_id):
 # get sensor data.
 sensor_ = self._nxtbrick.getSensors()
 if sensor_:
 self._d_sens.data = [sensor_[3]]
 # write sensor data to outport.
 self._sensOut.write()
 
 # get position data.
 position_ = self._nxtbrick.getMotors()
 if position_:
 self._d_pos.data = [position_[self._mapping[self._map[0][0]]][9],position_[self._mapping[self._map[0][1]]][9]]
 # write position data to outport.
 self._posOut.write()
 
 return RTC.RTC_OK
 
 
 
 def MyModuleInit(manager):
 profile = OpenRTM.Properties(defaults_str=nxtrtc_spec)
 manager.registerFactory(profile,
 NXTRTC,
 OpenRTM.Delete)
 
 # Create a component
 comp = manager.createComponent("NXTRTC")
 
 
 
 def main():
 mgr = OpenRTM.Manager.init(len(sys.argv), sys.argv)
 #mgr = OpenRTM.Manager.init(sys.argv)
 mgr.setModuleInitProc(MyModuleInit)
 mgr.activateManager()
 mgr.runManager()
 
 if __name__ == "__main__":
 main()


처음의 예에서는 모터에의 출력, 센서값의 읽기, 모터 엔코더의 읽기를 모두 동일 루프로 동기적으로 하고 있었습니다만, 콜백을 사용하는 것으로 데이터가 오자 마자 모터에 값을 출력할 수 있게 됩니다.