diff --git a/hironx_ros_bridge/scripts/hironx.py b/hironx_ros_bridge/scripts/hironx.py index 575cea69..373808da 100755 --- a/hironx_ros_bridge/scripts/hironx.py +++ b/hironx_ros_bridge/scripts/hironx.py @@ -56,15 +56,7 @@ ' 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') @@ -72,7 +64,7 @@ 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 diff --git a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py index 46446d9d..9f135146 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +++ b/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,39 @@ 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 rtcs_str.split(","): + for elem in self._RTC_list: + if elem[0] == rtc_requested: + print('[getRTCList] RTC activated: {}'.format(elem)) + new_rtcs.append(elem) + 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[%]