[WIP][ros_bridge] Fix to tork-a/rtmros_nextage#308.

hironx_ros_bridge/scripts/hironx.py

@@ -56,23 +56,15 @@
                  ' 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'
 
-RTC_LIST = [
-            ['seq', "SequencePlayer"],
-            ['sh', "StateHolder"],
-            ['fk', "ForwardKinematics"],
-            ['ic', "ImpedanceController"],
-            ['el', "SoftErrorLimiter"],
-            # ['co', "CollisionDetector"],
-            ['sc', "ServoController"],
-            ['log', "DataLogger"],]
+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.')
+    parser.add_argument('--rtcs', help="RT components to activate. If nothing passed then default value will be used. Example: ['seq', 'sh', 'fk', 'ic', 'el', 'sc', 'log']")
     args, unknown = parser.parse_known_args()
     unknown = [u for u in unknown if u[:2] != '__'] # filter out ros arguments
 

hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py

@@ -307,7 +307,7 @@ class via the link above; nicely formatted api doc web page
 
     HandGroups = {'rhand': [2, 3, 4, 5], 'lhand': [6, 7, 8, 9]}
 
-    _RTClist = [
+    _RTC_list = [
             ['seq', "SequencePlayer"],
             ['sh', "StateHolder"],
             ['fk', "ForwardKinematics"],
@@ -328,7 +328,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="", rtcs=_RTClist):
+    def init(self, robotname="HiroNX(Robot)0", url="", rtcs=_RTC_list):
         '''
         Calls init from its superclass, which tries to connect RTCManager,
         looks for ModelLoader, and starts necessary RTC components. Also runs
@@ -337,11 +337,10 @@ def init(self, robotname="HiroNX(Robot)0", url="", rtcs=_RTClist):
 
         @type robotname: str
         @type url: str
-        @type rtcs: [[str, str]]
-        @param rtcs: List of list of RTC names. Each inner list consists of
-            'SHORTENED' name and the 'FULLNAME'.
+        @type rtcs: [str]
+        @param rtcs: List of abbreviated RTC names.
 
-            example: [['seq', "SequencePlayer"], ['sh', "StateHolder"],,,]
+            example: ['seq', 'sh',,,]
         '''
         # reload for hrpsys 315.1.8
         print(self.configurator_name + "waiting ModelLoader")
@@ -367,7 +366,10 @@ def init(self, robotname="HiroNX(Robot)0", url="", rtcs=_RTClist):
         self.sensors = self.getSensors(url)
 
         if rtcs:
-            self._RTClist = rtcs
+            # convert the list of abbreviated RTC names to the one that
+            # getRTCList wants.
+            self.getRTCList(rtcs)
+
         # 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()])) :
             print(self.configurator_name + "hrpsys components are already created and running")
@@ -450,24 +452,42 @@ 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):
         '''
         @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.
+            example: "seq, sh, fk, ic, el, sc, log"
         @rtype [[str]]
         @rerutrn List of available components. Each element consists of a list
                  of abbreviated and full names of the component.
         '''
+        if rtcs_str:
+            print('[getRTCList][DEBUG] In rtcs_str')
+            # Set a new list of RTCs
+            new_rtcs = []
+            for rtc_requested in [x.strip() for x in rtcs_str.split(",")]:
+                print('[DEBUG][getRTCList] RTC requested: {}'.format(rtc_requested))
+                for elem in self._RTC_list:
+                    print('[DEBUG][getRTCList] RTC examined: {}'.format(elem[0]))
+                    if elem[0] == rtc_requested:
+                        print('[getRTCList] to be activated: {}'.format(elem))
+                        new_rtcs.append(elem)
+                        break
+            self._RTC_list = new_rtcs
+
         if hasattr(self, 'rmfo'):
             self.ms.load("RemoveForceSensorLinkOffset")
             self.ms.load("AbsoluteForceSensor")
             if "RemoveForceSensorLinkOffset" in self.ms.get_factory_names():
-                self._RTClist.append(['rmfo', "RemoveForceSensorLinkOffset"])
+                self._RTC_list.append(['rmfo', "RemoveForceSensorLinkOffset"])
             elif "AbsoluteForceSensor" in self.ms.get_factory_names():
-                self._RTClist.append(['rmfo', "AbsoluteForceSensor"])
+                self._RTC_list.append(['rmfo', "AbsoluteForceSensor"])
             else:
                 print "Component rmfo is not loadable."
-        return self._RTClist
+        return self._RTC_list
 
     # hand interface
     # effort: 1~100[%]