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

샘플 코드의 설명

여기로부터, 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()


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