[ros_bridge][py] Allow passing RTC list from hironx.py client.

hironx_ros_bridge/scripts/hironx.py

@@ -56,12 +56,16 @@
                  ' this script, but can use RTM. To use ROS, do not forget' \
                  ' to run rosbridge. How to do so? --> http://wiki.ros.org/rtmros_nextage/Tutorials/Operating%20Hiro%2C%20NEXTAGE%20OPEN'
 
+# The default RTCs for Hironx
+RTC_LIST = 'seq, sh, fk, ic, el, sc, log'
+
 if __name__ == '__main__':
     parser = argparse.ArgumentParser(description='hiro command line interpreters')
     parser.add_argument('--host', help='corba name server hostname')
     parser.add_argument('--port', help='corba name server port number')
     parser.add_argument('--modelfile', help='robot model file nmae')
     parser.add_argument('--robot', help='robot modlule name (RobotHardware0 for real robot, Robot()')
+    parser.add_argument('--rtcs', help="RT components to activate. If nothing passed then default value will be used. Example: '{}'".format(RTC_LIST))
     args, unknown = parser.parse_known_args()
     unknown = [u for u in unknown if u[:2] != '__'] # filter out ros arguments
 
@@ -73,13 +77,15 @@
         args.robot = "RobotHardware0" if args.host else "HiroNX(Robot)0"
     if not args.modelfile:
         args.modelfile = "/opt/jsk/etc/HIRONX/model/main.wrl" if args.host else ""
+    if not args.rtcs:
+        args.rtcs = RTC_LIST
 
     # support old style format
     if len(unknown) >= 2:
         args.robot = unknown[0]
         args.modelfile = unknown[1]
     robot = hiro = hironx_client.HIRONX()
-    robot.init(robotname=args.robot, url=args.modelfile)
+    robot.init(robotname=args.robot, url=args.modelfile, rtcs=args.rtcs)
 
     # ROS Client
     try:

hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py

@@ -307,7 +307,34 @@ class via the link above; nicely formatted api doc web page
 
     HandGroups = {'rhand': [2, 3, 4, 5], 'lhand': [6, 7, 8, 9]}
 
-    RtcList = []
+    # This shouldn't be accessed once turned to True during `init` method.
+    is_rtc_activated = False
+
+    # Default set of RTCs to be activated.
+    _RTC_list = [
+            ['seq', "SequencePlayer"],
+            ['sh', "StateHolder"],
+            ['fk', "ForwardKinematics"],
+            ['ic', "ImpedanceController"],
+            ['el', "SoftErrorLimiter"],
+            ['sc', "ServoController"],
+            ['log', "DataLogger"],
+        ]
+
+    # All available RTCs. This list is meant to be immutable.
+    RTCs_all_available = [
+            ['seq', "SequencePlayer"],
+            ['sh', "StateHolder"],
+            ['fk', "ForwardKinematics"],
+            ['ic', "ImpedanceController"],
+            ['el', "SoftErrorLimiter"],
+            ['co', "CollisionDetector"],
+            ['sc', "ServoController"],
+            ['log', "DataLogger"],
+        ]
+
+    # List of the name of RT Components that hrpsys requires at minimum.
+    _RTC_NAME_MINREQ = ['seq', 'sh', 'fk']
 
     # servo controller (grasper)
     sc = None
@@ -319,7 +346,7 @@ class via the link above; nicely formatted api doc web page
                                "the function call was successful, since not " +
                                "all methods internally called return status")
 
-    def init(self, robotname="HiroNX(Robot)0", url=""):
+    def init(self, robotname="HiroNX(Robot)0", url="", rtcs=None):
         '''
         Calls init from its superclass, which tries to connect RTCManager,
         looks for ModelLoader, and starts necessary RTC components. Also runs
@@ -328,6 +355,10 @@ def init(self, robotname="HiroNX(Robot)0", url=""):
 
         @type robotname: str
         @type url: str
+        @type rtcs: [str]
+        @param rtcs: List of abbreviated RTC names.
+
+            example: ['seq', 'sh',,,]
         '''
         # reload for hrpsys 315.1.8
         print(self.configurator_name + "waiting ModelLoader")
@@ -353,7 +384,7 @@ def init(self, robotname="HiroNX(Robot)0", url=""):
         self.sensors = self.getSensors(url)
 
         # all([rtm.findRTC(rn[0], rtm.rootnc) for rn in self.getRTCList()]) # not working somehow...
-        if set([rn[0] for rn in self.getRTCList()]).issubset(set([x.name() for x in self.ms.get_components()])) :
+        if set([rn[0] for rn in self.getRTCList(rtcs)]).issubset(set([x.name() for x in self.ms.get_components()])) :
             print(self.configurator_name + "hrpsys components are already created and running")
             self.findComps(max_timeout_count=0, verbose=True)
         else:
@@ -434,34 +465,71 @@ def goInitial(self, tm=7, wait=True, init_pose_type=0):
                 self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
         return ret
 
-    def getRTCList(self):
+    def getRTCList(self, rtcs_str=None):
         '''
+        @summary: Return the list of activated RT components. As opposed to
+                  its naming, this also:
+                      1) activate an rmfo (stands for "remove force offset")
+                         RTC.
+                      2) selectively activate RTCs passed by rtcs_str. This is
+                         possible ONLY during the initialization process done
+                         by `init` method.
         @see: HrpsysConfigurator.getRTCList
 
+        @type rtcs_str: str
+        @param rtcs_str: A single str for a set of abbreviated names of RTCs,
+                         each of which is comma-separated. This is possible
+                         ONLY during the initialization process done by
+                         `init` method.
+            example: "seq, sh, fk, ic, el, sc, log"
         @rtype [[str]]
-        @rerutrn List of available components. Each element consists of a list
+        @return List of available components. Each element consists of a list
                  of abbreviated and full names of the component.
+        @raise TypeError: When rtcs_str isn't a string.
+        @raise ValueError: When rtcs_str does not contain minimum
+                           required RTCs.
         '''
-        rtclist = [
-            ['seq', "SequencePlayer"],
-            ['sh', "StateHolder"],
-            ['fk', "ForwardKinematics"],
-            ['ic', "ImpedanceController"],
-            ['el', "SoftErrorLimiter"],
-            # ['co', "CollisionDetector"],
-            ['sc', "ServoController"],
-            ['log', "DataLogger"],
-            ]
-        if hasattr(self, 'rmfo'):
+        if rtcs_str:
+            if self.is_rtc_activated:
+                print('RTCs are already activated. Skipping the passed request: {}'.format(rtcs_str))
+            else:
+                if not isinstance(rtcs_str, basestring):
+                    raise TypeError('rtcs_str needs to be string.')
+                # Set a new list of RTCs
+                new_rtcs = []
+                # Separate by comma and remove whitespace.
+                rtcs_req_list = [x.strip() for x in rtcs_str.split(",")]
+                # Check if minimum required RTCs are passed.
+                if not all(x in rtcs_req_list for x in self._RTC_NAME_MINREQ):
+                    raise ValueError('{} are required at minimum'.format(
+                        self._RTC_NAME_MINREQ))
+
+                # Create a new list of requested RTC with the name of
+                # implementations.
+                for rtc_requested in rtcs_req_list:
+                    for elem in self.RTCs_all_available:
+                        if elem[0] == rtc_requested:
+                            new_rtcs.append(elem)
+                            break
+                self._RTC_list = new_rtcs
+                self.is_rtc_activated = True
+
+        is_rmfo_initiated = False
+        # For some reason using built-in "any" method yields
+        # `TypeError: 'module' object is not callable`, so do the iteration.
+        for rtc_list in self._RTC_list:
+            if 'rmfo' in rtc_list:
+                is_rmfo_initiated = True
+        if hasattr(self, 'rmfo') and not is_rmfo_initiated:
             self.ms.load("RemoveForceSensorLinkOffset")
             self.ms.load("AbsoluteForceSensor")
             if "RemoveForceSensorLinkOffset" in self.ms.get_factory_names():
-                rtclist.append(['rmfo', "RemoveForceSensorLinkOffset"])
+                self._RTC_list.append(['rmfo', "RemoveForceSensorLinkOffset"])
             elif "AbsoluteForceSensor" in self.ms.get_factory_names():
-                rtclist.append(['rmfo', "AbsoluteForceSensor"])
+                self._RTC_list.append(['rmfo', "AbsoluteForceSensor"])
             else:
                 print "Component rmfo is not loadable."
-        return rtclist
+        return self._RTC_list
 
     # hand interface
     # effort: 1~100[%]

hironx_ros_bridge/test/test-hironx.test

@@ -25,4 +25,7 @@
         args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
   <test type="test_hironx_ik_noinit.py" pkg="hironx_ros_bridge" test-name="test_hironx_ik_noinit" time-limit="3000"
         args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
+  <test type="test_hironx_client.py" pkg="hironx_ros_bridge"
+        test-name="test_hironx_client" time-limit="3000"
+        args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
 </launch>

hironx_ros_bridge/test/test_hironx_client.py

@@ -0,0 +1,108 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2017, TORK
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of TORK (Tokyo Opensource Robotics Kyokai Association). 
+# nor the names of its contributors may be used to endorse or promote products
+# derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from hironx_ros_bridge.hironx_client import HIRONX
+from test_hironx import TestHiro
+
+PKG = 'hironx_ros_bridge'
+
+
+class TestHiroClient(TestHiro):
+
+    _RTC_LIST_CUSTOM = [
+            ['seq', "SequencePlayer"],
+            ['sh', "StateHolder"],
+            ['fk', "ForwardKinematics"],
+            ['rmfo', 'RemoveForceSensorLinkOffset']
+        ]
+
+    def _compare_2dlist(self, twodim_list_a, twodim_list_b):
+        '''
+        Compare the first element in all elements of the 2nd list.
+        E.g. For [['a0', 'a1'], ['b0', 'b1'],..., ['n0', 'n1']], this method
+             checks a0, b0, n0
+        @rtype bool
+        '''
+        return set([a[0] for a in twodim_list_a]) == set(
+            [b[0] for b in twodim_list_b])
+
+    def test_getRTCList(self):
+        '''
+        Depending on the hrpsys version, different RTC implementation can be
+        returned, e.g. for "rmfo", older returns AbsoluteForceSensor while
+        newer does RemoveForceSensorLinkOffset. So in this testcase we only
+        check the first element of the returned list (e.g. "rmfo" instead of
+        its implementation).
+        '''
+        self.assertTrue(
+            self._compare_2dlist(
+                self.robot.getRTCList(),
+                # Accessing a private member var only for testing purpose. 
+                self.robot._RTC_list))
+
+    def test_getRTCList_customrtcs_args_correct(self):
+        '''
+        Test when the RTC list was passed from the client.
+
+        Because this uses HIRONX.init(), which is already done in the
+        superclass, HIRONX class instance is re-generated within this method,
+        which is not elegant but as of now I can't think of a better way.
+        '''
+        self.robot = HIRONX()
+        # Passing 1st elems from _RTC_LIST_CUSTOM, to init method that calls
+        # internally getRTCList.
+        self.robot.init(rtcs='seq, sh, fk')
+        self.assertTrue(
+            self._compare_2dlist(
+                self.robot.getRTCList(), self._RTC_LIST_CUSTOM))
+
+    def test_getRTCList_customrtcs_args_wrong(self):
+        '''
+        Test when the RTC list was passed from the client, in wrong format.
+        '''
+        # Passing the list of RTCs falling short of requirement.
+        self.assertRaises(
+            ValueError, self.robot.getRTCList, rtcs_str='seq, sh')
+
+        # Passing 1st elems from _RTC_LIST_CUSTOM,
+        # but list is not the right type of arg.
+        ## http://stackoverflow.com/a/6103930/577001
+        self.assertRaises(
+            TypeError, lambda: self.robot.getRTCList,
+                rtcs_str=['seq', 'sh', 'fk', 'el', 'co', 'log'])
+
+if __name__ == '__main__':
+    import rostest
+    rostest.rosrun(PKG, 'test_hronx_client', TestHiroClient)