add interactive mode to hrpsys_tools_config.py

hrpsys_tools/catkin.cmake

@@ -4,7 +4,7 @@ project(hrpsys_tools)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS openrtm_aist_python rostest)
+find_package(catkin REQUIRED COMPONENTS rostest)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -134,5 +134,6 @@ install(DIRECTORY scripts DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SO
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
-install(DIRECTORY test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
+install(DIRECTORY test samples DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
 add_rostest(test/test-hrpsys-config.test)
+add_rostest(test/test-pa10.test)

hrpsys_tools/samples/pa10.launch

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="GUI" default="true" />
+  <arg name="corbaport" default="15005" />
+  <include file="$(find hrpsys_tools)/launch/hrpsys.launch" >
+    <arg name="MODEL_FILE" value="$(find openhrp3)/share/OpenHRP-3.1/sample/model/sample1.wrl" />
+    <arg name="CONF_FILE" value="$(find  hrpsys)/share/hrpsys/samples/PA10/PA10.conf" />
+    <arg name="PROJECT_FILE" value="$(find  hrpsys)/share/hrpsys/samples/PA10/PA10simulation.xml" />
+    <arg name="SIMULATOR_NAME" value="PA10Controller(Robot)0" />
+    <arg name="GUI" default="$(arg GUI)" />
+    <arg name="corbaport" default="$(arg corbaport)" />
+  </include>
+</launch>

hrpsys_tools/scripts/hrpsys_tools_config.py

@@ -3,15 +3,46 @@
 import OpenRTM_aist.RTM_IDL # for catkin
 import sys
 
-import hrpsys
+from hrpsys import rtm
 from hrpsys.hrpsys_config import *
 import OpenHRP
-#from hrpsys.hrpsys_config import HrpsysConfigurator
 
 # copy from hrpsys/lib/python2.7/dist-packages/hrpsys_config.py
+import argparse, code
 if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description="hrpsys command line interpreters, try `./hrpsys_tools_config.py--host xxxx --port xxxx  -i` or `ipython ./hrpsys_tools_config.py -- --host xxxx --port xxxx` for robot debugging, we recommend to use ipython")
+    parser.add_argument('--host', help='corba name server hostname')
+    parser.add_argument('--port', help='corba name server port number')
+    parser.add_argument('-i', help='interactive mode',  action='store_true')
+    parser.add_argument('-c', help='execute command',  nargs='*')
+    args, unknown = parser.parse_known_args()
+
+    if args.i: # interactive
+        sys.argv.remove('-i')
+    if args.c:
+        sys.argv.remove('-c')
+        [sys.argv.remove(a) for a in args.c] # remove command from sys.argv
+    if args.host:
+        rtm.nshost = args.host; sys.argv = [sys.argv[0]] + sys.argv[3:]
+    if args.port:
+        rtm.nsport = args.port; sys.argv = [sys.argv[0]] + sys.argv[3:]
+
+    # ipython ./hrpsys_tools_config.py -i -- --port 2809
     hcf = HrpsysConfigurator()
-    if len(sys.argv) > 2 :
+    if args.i or '__IPYTHON__' in vars(__builtins__):
+        hcf.waitForModelLoader()
+        if len(sys.argv) > 1 and not sys.argv[1].startswith('-'):
+            hcf.waitForRTCManagerAndRoboHardware(robotname=sys.argv[1])
+            sys.argv = [sys.argv[0]] + sys.argv[2:]
+        hcf.findComps()
+        print >> sys.stderr, "[hrpsys.py] #\n[hrpsys.py] # use `hcf` as robot interface, for example hcf.getJointAngles()\n[hrpsys.py] #"
+        while args.c != None:
+            print >> sys.stderr, ">>", args.c[0]
+            exec(args.c[0])
+            args.c.pop(0)
+        if not (args.i and '__IPYTHON__' in vars(__builtins__)):
+            code.interact(local=locals()) #drop in shell if invoke from python, or ipython without -i option
+    elif len(sys.argv) > 2 :
         hcf.init(sys.argv[1], sys.argv[2])
     elif len(sys.argv) > 1 :
         hcf.init(sys.argv[1])

hrpsys_tools/test/test-hrpsys-config.test

@@ -14,6 +14,9 @@
 
 <!-- end of include -->
 
+  <test test-name="hrpsys_tool_config_interactive" pkg="hrpsys_tools"
+        type="hrpsys_tools_config.py" args='--host 127.0.0.1 --port 2809 -i -c "import rostest, unittest" "rostest.rosrun(\"hrpsys_tools\", \"interactive\", unittest.TestCase)" "quit()" "quit()"' />
+
   <test test-name="configure_host_port" pkg="hrpsys_tools"
         type="test-hrpsys-config.py" args="--host 127.0.0.1 --port 2809" />
 </launch>

hrpsys_tools/test/test-pa10.test

@@ -0,0 +1,18 @@
+<launch>
+  <include file="$(find hrpsys_tools)/samples/pa10.launch" >
+    <arg name="GUI" default="false" />
+    <arg name="corbaport" default="2809" />
+    <!--
+        use 2809 corbaport (this is default CORBA port, but rtm-ros-robtics uses 15005 for default...) for testing without dependency on rtmlaunch.
+        if you have some troubles, please restart omniNames
+          sudo /etc/init.d/omniorb4-nameserver restart
+          sudo pkill -KILL omniNames
+          rosrun openrtm_aist rtm-naming 2809
+     -->
+  </include>
+
+  <!-- need time.sleep(1): need code that waht for all component in serialization is actually activated.... -->
+  <test test-name="hrpsys_tool_config_interactive" pkg="hrpsys_tools"
+        type="hrpsys_tools_config.py" args='--host 127.0.0.1 --port 2809 -i PA10Controller\(Robot\)0 -c "import time" "time.sleep(5)" "hcf.setJointAngles([10,10,10,10,10,10,10,10,10], 1)" "print hcf.getJointAngles()" "import rostest, unittest" "rostest.rosrun(\"hrpsys_tools\", \"interactive\", unittest.TestCase)" "quit()"' />
+
+</launch>