FSM4RTC implementation (#8)

* import FSM4RTC idls from http://svn.openrtm.org/OpenRTM-aist/branches/FSM4RTC

* add event handler for FSM events

* fix typo

* add script to compile openrtm-aist fsm4rtc branch for testing

* implement setter and getter for property: dynamic

* add utility method to get ExtendedFsmService

* getter and setter for dynamic was there at node.py

* add utility script to run fsm4rtc test component

* fix idl prefix issue

* increase portability

* raise exception when observer service not found

* dirty fix for openrtm-aist get_sdo_service bug

* revert change made by accident

Author: yosuke

idl/ComponentObserver.idl

@@ -19,6 +19,8 @@
 #include "SDOPackage.idl"
 #include "RTC.idl"
 
+#pragma prefix "omg.org"
+
 /*!
  * @if jp
  * @brief コンポーネント状態オブザーバインターフェース
@@ -43,7 +45,7 @@
  * @since 1.1
  * @endif
  */
-module OpenRTM
+module RTC
 {
   /*!
    * @if jp
@@ -277,10 +279,18 @@ module OpenRTM
      *
      * @endif
      */
-    HEARTBEAT,
+    RTC_HEARTBEAT,
+    EC_HEARTBEAT,
+    
+    FSM_PROFILE,
+    FSM_STATUS,
+    FSM_STRUCTURE,
+    
+    USER_DEFINED,
 
     STATUS_KIND_NUM
   };
+  #pragma version StatusKind 1.0
 
   /*!
    * @if jp
@@ -492,5 +502,6 @@ module OpenRTM
      */
     oneway void update_status(in StatusKind status_kind, in string hint);
   };
+  #pragma version ComponentObserver 1.0
 
 };

idl/ExtendedFsmService.idl

@@ -0,0 +1,37 @@
+#ifndef _EXTENDED_FSM_SERVICE_IDL_
+#define _EXTENDED_FSM_SERVICE_IDL_
+
+#include <RTC.idl>
+
+#pragma prefix "omg.org"
+
+module RTC
+{
+    struct FsmEventProfile
+    {
+        string name;
+        string data_type;
+    };
+    #pragma version FsmEventProfile 1.0
+    typedef sequence<FsmEventProfile> FsmEventProfileList;
+
+    struct FsmStructure
+    {
+        string name;
+        string structure;
+        FsmEventProfileList event_profiles;
+        NVList properties;
+    };
+    #pragma version FsmStructure 1.0
+
+    interface ExtendedFsmService : SDOPackage::SDOService
+    {
+        string get_current_state();
+        ReturnCode_t set_fsm_structure(in FsmStructure fsm_structure);
+        ReturnCode_t get_fsm_structure(out FsmStructure fsm_structure);
+    };
+    #pragma version ExtendedFsmService 1.0
+};
+
+#endif // _EXTENDED_FSM_SERVICE_IDL_
+

rtctree/component.py

@@ -67,9 +67,12 @@ class Component(TreeNode):
       one of Component.CFG_UPDATE_SET, Component.CFG_UPDATE_PARAM,
       Component.CFG_UPDATE_PARAM_IN_ACTIVE, Component.CFG_ADD_SET,
       Component.CFG_REMOVE_SET and Component.CFG_ACTIVATE_SET.
-    - heartbeat(time)
-      A heartbeat was received from the component. The time the beat was
-      received is passed.
+    - heartbeat(type, time)
+      A heartbeat was received from the component or from the execution context.
+      The time the beat was received is passed.
+    - fsm_event(type, hint)
+      A change in the FSM status has occurred. The type of the event and the
+      content of the event is passed.
 
     To explain the usage of Component node, we first launch example components:
     >>> import subprocess, shlex
@@ -245,7 +248,7 @@ def __init__(self, name=None, parent=None, obj=None, *args, **kwargs):
         super(Component, self).__init__(name=name, parent=parent,
                                         *args, **kwargs)
         self._set_events(['rtc_status', 'component_profile', 'ec_event',
-            'port_event', 'config_event', 'heartbeat'])
+            'port_event', 'config_event', 'heartbeat', 'fsm_event'])
         self._reset_data()
         self._parse_profile()
 
@@ -852,6 +855,22 @@ def state_string(self):
         '''The state of this component as a colourised string.'''
         return self.get_state_string()
 
+    ###########################################################################
+    # FSM4RTC
+
+    def get_extended_fsm_service(self):
+        '''Get a reference to the ExtendedFsmService.
+
+        @return A reference to the ExtendedFsmService object
+        @raises InvalidSdoServiceError
+
+        '''
+        with self._mutex:
+            try:
+                return self._obj.get_sdo_service(RTC.ExtendedFsmService._NP_RepositoryId)._narrow(RTC.ExtendedFsmService)
+            except:
+                raise exceptions.InvalidSdoServiceError('ExtendedFsmService')
+
     ###########################################################################
     # Port management
 
@@ -1002,7 +1021,7 @@ def add_logger(self, cb, level='NORMAL', filters='ALL'):
 
         '''
         with self._mutex:
-            obs = rtctree.sdo.RTCLogger(self, cb)
+            obs = sdo.RTCLogger(self, cb)
             uuid_val = uuid.uuid4()
             intf_type = obs._this()._NP_RepositoryId
             props = {'logger.log_level': level,
@@ -1130,11 +1149,15 @@ def _config_event(self, name, event):
 
     def _enable_dynamic(self, enable=True):
         if enable:
-            obs = rtctree.sdo.RTCObserver(self)
+            obs = sdo.RTCObserver(self)
             uuid_val = uuid.uuid4().get_bytes()
             intf_type = obs._this()._NP_RepositoryId
             props = utils.dict_to_nvlist({'heartbeat.enable': 'YES',
                 'heartbeat.interval': '1.0',
+                'rtc_heartbeat.enable': 'YES',
+                'rtc_heartbeat.interval': '1.0',
+                'ec_heartbeat.enable': 'YES',
+                'ec_heartbeat.interval': '1.0',
                 'observed_status': 'ALL'})
             sprof = SDOPackage.ServiceProfile(id=uuid_val,
                     interface_type=intf_type, service=obs._this(),
@@ -1147,6 +1170,8 @@ def _enable_dynamic(self, enable=True):
                 self._obs_id = uuid_val
                 # If we could set an observer, the component is alive
                 self._last_heartbeat = time.time()
+            else:
+                raise exceptions.InvalidSdoServiceError('Observer')
         else: # Disable
             conf = self.object.get_configuration()
             res = conf.remove_service_profile(self._obs_id)
@@ -1214,10 +1239,14 @@ def _get_ec_state(self, ec):
         else:
             return self.CREATED
 
-    def _heartbeat(self):
-        # Received a heart beat
+    def _heartbeat(self, kind):
+        # Received a heart beat signal
         self._last_heartbeat = time.time()
-        self._call_cb('heartbeat', self._last_heartbeat)
+        self._call_cb('heartbeat', (kind, self._last_heartbeat))
+
+    def _fsm_event(self, kind, hint):
+        # Received a fsm event
+        self._call_cb('fsm_event', (kind, hint))
 
     def _parse_configuration(self):
         # Parse the component's configuration sets

rtctree/exceptions.py

@@ -287,4 +287,11 @@ def __str__(self):
         return 'Not a CORBA CosNaming.Name: {0}'.format(self.args[0])
 
 
+class InvalidSdoServiceError(RtcTreeError):
+    '''Could not find specified SDO service.'''
+    def __str__(self):
+        return 'Invalid SDO service: {0}'.format(self.args[0])
+
+
+
 # vim: set expandtab tabstop=8 shiftwidth=4 softtabstop=4 textwidth=79

rtctree/sdo.py

@@ -19,9 +19,10 @@
 
 
 from rtctree.rtc import OpenRTM__POA
+from rtctree.rtc import RTC__POA
 
 
-class RTCObserver(OpenRTM__POA.ComponentObserver):
+class RTCObserver(RTC__POA.ComponentObserver):
     def __init__(self, target):
         self._tgt = target
 
@@ -77,8 +78,10 @@ def update_status(self, kind, hint):
             elif event == 'ACTIVATE_CONFIG_SET':
                 event = self._tgt.CFG_ACTIVATE_SET
             self._tgt._config_event(arg, event)
-        elif kind == 'HEARTBEAT':
-            self._tgt._heartbeat()
+        elif kind == 'HEARTBEAT' or kind == 'RTC_HEARTBEAT' or kind == 'EC_HEARTBEAT':
+            self._tgt._heartbeat(kind)
+        elif kind == 'FSM_PROFILE' or kind == 'FSM_STATUS' or kind == 'FSM_STRUCTURE':
+            self._tgt._fsm_event(kind, hint)
 
 
 class RTCLogger(OpenRTM__POA.Logger):

test/run-fsm4rtc.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+
+BASEDIR=$(dirname $(readlink -f "$0"))
+
+FSM4RTC_HOME="$BASEDIR/FSM4RTC/OpenRTM-aist"
+
+$FSM4RTC_HOME/examples/SimpleIO/ConsoleInComp -f $BASEDIR/rtc.conf -o "logger.log_level:DEBUG" -o "sdo.service.provider.enabled_services:ALL" -o "manager.local_service.modules:$FSM4RTC_HOME/src/ext/sdo/fsm4rtc_observer/.libs/ComponentObserverConsumer.so(ComponentObserverConsumerInit),$FSM4RTC_HOME/src/ext/sdo/extended_fsm/.libs/ExtendedFsmServiceProvider.so(ExtendedFsmServiceProviderInit)"
+

test/setup-fsm4rtc.patch

@@ -0,0 +1,36 @@
+Index: OpenRTM-aist/src/lib/rtm/FsmActionListener.h
+===================================================================
+--- OpenRTM-aist/src/lib/rtm/FsmActionListener.h	(リビジョン 2806)
++++ OpenRTM-aist/src/lib/rtm/FsmActionListener.h	(作業コピー)
+@@ -29,9 +29,7 @@
+ 
+ namespace RTC
+ {
+-  struct FsmStructure
+-  {
+-  };
++  struct FsmStructure;
+   /*!
+    * @if jp
+    *
+Index: OpenRTM-aist/src/lib/rtm/SdoServiceAdmin.cpp
+===================================================================
+--- OpenRTM-aist/src/lib/rtm/SdoServiceAdmin.cpp	(リビジョン 2806)
++++ OpenRTM-aist/src/lib/rtm/SdoServiceAdmin.cpp	(作業コピー)
+@@ -82,6 +82,7 @@
+     
+     // If types include '[Aa][Ll][Ll]', all types enabled in this RTC
+     ::coil::vstring activeProviderTypes;
++/*
+     for (size_t i(0); i < enabledProviderTypes.size(); ++i)
+       {
+         std::string tmp(enabledProviderTypes[i]);
+@@ -100,6 +101,8 @@
+               }
+           }
+       }
++*/
++    activeProviderTypes = availableProviderTypes;
+ 
+     SdoServiceProviderFactory& factory(SdoServiceProviderFactory::instance());
+     for (size_t i(0); i < activeProviderTypes.size(); ++i)

test/setup-fsm4rtc.sh

@@ -0,0 +1,19 @@
+#!/bin/bash
+
+BASEDIR=$(dirname $(readlink -f "$0"))
+
+sudo apt install subversion autoconf autotools-bin libpoco-dev
+if [ ! -d "$BASEDIR/FSM4RTC" ]; then
+    svn co http://svn.openrtm.org/OpenRTM-aist/branches/FSM4RTC $BASEDIR/FSM4RTC
+fi
+cd $BASEDIR/FSM4RTC
+svn update
+patch -p0 -E < $BASEDIR/setup-fsm4rtc.patch
+cd OpenRTM-aist
+./autogen.sh 
+./configure
+make -j4
+
+## To launch the test component:
+# cd src/ext/sdo/fsm4rtc_observer/test/
+# ./test.sh