From 0541637350c1b0c3ec9fdef462df9cc4bb290797 Mon Sep 17 00:00:00 2001 From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp> Date: Tue, 25 Apr 2017 15:39:18 -0700 Subject: [PATCH] [ros_bridge][py] Allow passing RTC list from hironx.py client. --- hironx_ros_bridge/scripts/hironx.py | 15 ++++++++- .../src/hironx_ros_bridge/hironx_client.py | 9 +++++- hironx_ros_bridge/test/test_hironx_client.py | 32 ++++++++++++++++--- 3 files changed, 50 insertions(+), 6 deletions(-) diff --git a/hironx_ros_bridge/scripts/hironx.py b/hironx_ros_bridge/scripts/hironx.py index d2602670..575cea69 100755 --- a/hironx_ros_bridge/scripts/hironx.py +++ b/hironx_ros_bridge/scripts/hironx.py @@ -56,12 +56,23 @@ ' 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"],] + 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.') args, unknown = parser.parse_known_args() unknown = [u for u in unknown if u[:2] != '__'] # filter out ros arguments @@ -73,13 +84,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: 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 a240ec0a..46446d9d 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +++ b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py @@ -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=""): + def init(self, robotname="HiroNX(Robot)0", url="", rtcs=_RTClist): ''' Calls init from its superclass, which tries to connect RTCManager, looks for ModelLoader, and starts necessary RTC components. Also runs @@ -337,6 +337,11 @@ def init(self, robotname="HiroNX(Robot)0", url=""): @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'. + + example: [['seq', "SequencePlayer"], ['sh', "StateHolder"],,,] ''' # reload for hrpsys 315.1.8 print(self.configurator_name + "waiting ModelLoader") @@ -361,6 +366,8 @@ def init(self, robotname="HiroNX(Robot)0", url=""): # HrpsysConfigurator.init(self, robotname=robotname, url=url) self.sensors = self.getSensors(url) + if rtcs: + self._RTClist = 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") diff --git a/hironx_ros_bridge/test/test_hironx_client.py b/hironx_ros_bridge/test/test_hironx_client.py index fa37b667..c129c455 100755 --- a/hironx_ros_bridge/test/test_hironx_client.py +++ b/hironx_ros_bridge/test/test_hironx_client.py @@ -33,6 +33,7 @@ # 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' @@ -40,8 +41,7 @@ class TestHiroClient(TestHiro): - def test_getRTCList(self): - RTC_LIST = [ + _RTC_LIST = [ ['seq', "SequencePlayer"], ['sh', "StateHolder"], ['fk', "ForwardKinematics"], @@ -50,8 +50,32 @@ def test_getRTCList(self): # ['co', "CollisionDetector"], ['sc', "ServoController"], ['log', "DataLogger"], - ] - self.assertListEqual(self.robot.getRTCList(), RTC_LIST) + ] + + _RTC_LIST_CUSTOM = [ + ['seq', "SequencePlayer"], + ['sh', "StateHolder"], + ['fk', "ForwardKinematics"], + ['el', "SoftErrorLimiter"], + ['co', "CollisionDetector"], + ['log', "DataLogger"], + ] + + def test_getRTCList(self): + self.assertListEqual(self.robot.getRTCList(), self._RTC_LIST) + + def test_getRTCList_customrtcs(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() + self.robot.init(rtcs=self._RTC_LIST_CUSTOM) + + self.assertListEqual(self.robot.getRTCList(), self._RTC_LIST_CUSTOM) if __name__ == '__main__': import rostest