Fix launch generation and enable unstable rtc euslisp unittest on travis rostest

.travis.yml

@@ -20,7 +20,7 @@ env:
   matrix:
     - ROS_DISTRO=hydro  USE_DEB=true
     - ROS_DISTRO=hydro  USE_DEB=false   NOT_TEST_INSTALL=true
-    - ROS_DISTRO=hydro  USE_DEB=false   NOT_TEST_INSTALL=true EXTRA_DEB="ros-hydro-roseus ros-hydro-euscollada ros-hydro-pr2eus" CATKIN_PARALLEL_JOBS='-p1'
+    - ROS_DISTRO=hydro  USE_DEB=source  NOT_TEST_INSTALL=true EXTRA_DEB="ros-hydro-roseus ros-hydro-euscollada ros-hydro-pr2eus" ROS_PARALLEL_JOBS="-j1 -l1" IS_EUSLISP_TRAVIS_TEST="true"
     - ROS_DISTRO=hydro  USE_DEB=true    NOT_TEST_INSTALL=true INSTALL_SRC="https://github.com/start-jsk/rtmros_tutorials" TEST_PKGS="hrpsys_ros_bridge_tutorials" INSTALL_SRC_SECURE="[email protected]:start-jsk/rtmros_hrp2" TEST_PKGS_SECURE="jsk_hrp2_ros_bridge"
     - ROS_DISTRO=hydro  USE_DEB=source  NOT_TEST_INSTALL=true
     - ROS_DISTRO=indigo USE_DEB=true
@@ -43,7 +43,7 @@ before_script:
   - export REPOSITORY_NAME=`basename $PWD`
   - if [ "${INSTALL_SRC}" != "" ] ;then sudo apt-get install python-yaml; rm .rosinstall; for src in $INSTALL_SRC; do name=`basename $src`; python -c "import yaml;print yaml.dump([{'git':{'uri':'$src','local-name':'$name'}}], default_flow_style=False)" >> .rosinstall; done; cat .rosinstall; export USE_DEB=false; fi; # set USE_DEB false to enable .rosinstall
 script:
-  - export ROS_PARALLEL_JOBS="-j2 -l2"
+  - if [ "${IS_EUSLISP_TRAVIS_TEST}" != "true" ] ; then export ROS_PARALLEL_JOBS="-j2 -l2" ; fi
   - if [ "${TEST_TYPE}" == "" ] ; then source .travis/travis.sh; else source ./.travis_test.sh ; fi
 after_success:
   - TRAVIS_JOB_SUBNUMBER="${TRAVIS_JOB_NUMBER##*.}"

hrpsys_ros_bridge/catkin.cmake

@@ -152,7 +152,12 @@ endif()
 compile_openhrp_model(${_OPENHRP3_MODEL_DIR}/PA10/pa10.main.wrl)
 compile_openhrp_model(${_OPENHRP3_MODEL_DIR}/sample1.wrl SampleRobot
   --conf-dt-option "0.002"
-  --simulation-timestep-option "0.002")
+  --simulation-timestep-option "0.002"
+  --conf-file-option "abc_leg_offset: 0,0.09,0"
+  --conf-file-option "end_effectors: lleg,LLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, rleg,RLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, larm,LARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708, rarm,RARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708,"
+  --conf-file-option "collision_pair: RARM_WRIST_P:WAIST LARM_WRIST_P:WAIST RARM_WRIST_P:RLEG_HIP_R LARM_WRIST_P:LLEG_HIP_R RARM_WRIST_R:RLEG_HIP_R LARM_WRIST_R:LLEG_HIP_R"
+  --conf-file-option "pdgains_sim.file_name: ${hrpsys_PREFIX}/share/hrpsys/samples/SampleRobot/SampleRobot.PDgain.dat"
+  )
 generate_default_launch_eusinterface_files("\$(find openhrp3)/share/OpenHRP-3.1/sample/model/PA10/pa10.main.wrl" hrpsys_ros_bridge)
 generate_default_launch_eusinterface_files("\$(find openhrp3)/share/OpenHRP-3.1/sample/model/sample1.wrl" hrpsys_ros_bridge SampleRobot)
 execute_process(COMMAND sed -i s@pa10\(Robot\)0@HRP1\(Robot\)0@ ${PROJECT_SOURCE_DIR}/launch/pa10.launch)
@@ -201,6 +206,10 @@ add_rostest(test/test-import-python.test)
 # call catkin depends
 find_package(catkin COMPONENTS roseus  QUIET)
 if(roseus_FOUND)
-  generate_eusdoc(euslisp/rtm-ros-robot-interface.l)
-  add_rostest(test/hrpsys-samples/test_samplerobot_euslisp_unittests.launch)
+  if ("true" STREQUAL "$ENV{IS_EUSLISP_TRAVIS_TEST}")
+    message("Execute rostest for euslisp unittest")
+    add_rostest(test/hrpsys-samples/test_samplerobot_euslisp_unittests.launch)
+  else()
+    generate_eusdoc(euslisp/rtm-ros-robot-interface.l)
+  endif()
 endif()

hrpsys_ros_bridge/cmake/compile_robot_model.cmake

@@ -198,12 +198,13 @@ macro (generate_default_launch_eusinterface_files wrlfile project_pkg_name)
   set(${_sname}_generated_launch_euslisp_files)
   #   generate hrpsys_config.py to use unstable RTCs
   set(ROSBRIDGE_ARGS "    <arg name=\"BASE_LINK\" default=\"WAIST_LINK0\" />\n")
+  set(USE_UNSTABLE_RTC "false")
+  set(STARTUP_ARGS "    <arg name=\"HRPSYS_PY_PKG\" default=\"${PROJECT_PKG_NAME}\" if=\"$(arg USE_UNSTABLE_RTC)\"/>\n    <arg name=\"HRPSYS_PY_NAME\" default=\"${_sname}_hrpsys_config.py\" if=\"$(arg USE_UNSTABLE_RTC)\"/>")
   if ("${ARGV3}" STREQUAL "--use-unstable-hrpsys-config")
-    set(ROSBRIDGE_ARGS "    <arg name=\"USE_WALKING\" default=\"true\" />\n    <arg name=\"USE_IMPEDANCECONTROLLER\" default=\"true\" />\n    <arg name=\"USE_EMERGENCYSTOPPER\" default=\"true\" />${ROSBRIDGE_ARGS}")
+    set(USE_UNSTABLE_RTC "true")
     set(STARTUP_ARGS "    <arg name=\"HRPSYS_PY_ARGS\" default=\"--use-unstable-rtc\" />")
   elseif ("${ARGV3}" STREQUAL "--use-robot-hrpsys-config")
-    set(ROSBRIDGE_ARGS "    <arg name=\"USE_WALKING\" default=\"true\" />\n    <arg name=\"USE_IMPEDANCECONTROLLER\" default=\"true\" />\n    <arg name=\"USE_EMERGENCYSTOPPER\" default=\"true\" />${ROSBRIDGE_ARGS}")
-    set(STARTUP_ARGS "    <arg name=\"HRPSYS_PY_PKG\" default=\"${PROJECT_PKG_NAME}\" />\n    <arg name=\"HRPSYS_PY_NAME\" default=\"${_sname}_hrpsys_config.py\" />")
+    set(USE_UNSTABLE_RTC "true")
   endif()
   #  generate startup.launch
   configure_file(${hrpsys_ros_bridge_PACKAGE_PATH}/scripts/default_robot_startup.launch.in ${PROJECT_SOURCE_DIR}/launch/${_sname}_startup.launch)

hrpsys_ros_bridge/scripts/default_robot.launch.in

@@ -9,6 +9,7 @@
   <arg name="SIMULATOR_NAME" default="@ROBOT@(Robot)0" />
   <arg name="corbaport" default="15005" />
   <arg name="hrpsys_precreate_rtc" default="HGcontroller"/>
+  <arg name="USE_UNSTABLE_RTC" default="@USE_UNSTABLE_RTC@"/>
   <include file="$(find @PROJECT_PKG_NAME@)/launch/@robot@_startup.launch" >
     <arg name="KILL_SERVERS" default="$(arg KILL_SERVERS)" />
     <arg name="NOSIM" value="$(arg NOSIM)" />
@@ -18,11 +19,13 @@
     <arg name="corbaport" default="$(arg corbaport)" />
     <arg name="GUI" default="$(arg GUI)" />
     <arg name="hrpsys_precreate_rtc" default="$(arg hrpsys_precreate_rtc)" />
+    <arg name="USE_UNSTABLE_RTC" default="$(arg USE_UNSTABLE_RTC)"/>
   </include>
   <include file="$(find @PROJECT_PKG_NAME@)/launch/@robot@_ros_bridge.launch" >
     <arg name="SIMULATOR_NAME" value="$(arg SIMULATOR_NAME)" />
     <arg name="RUN_RVIZ" default="$(arg RUN_RVIZ)" />
     <arg name="corbaport" default="$(arg corbaport)" />
+    <arg name="USE_UNSTABLE_RTC" default="$(arg USE_UNSTABLE_RTC)"/>
   </include>
 
   <sphinxdoc><![CDATA[

hrpsys_ros_bridge/scripts/default_robot_ros_bridge.launch.in

@@ -1,6 +1,7 @@
 <launch>
   <arg name="RUN_RVIZ" default="true" />
   <arg name="SIMULATOR_NAME" default="@ROBOT@(Robot)0" />
+  <arg name="USE_UNSTABLE_RTC" default="@USE_UNSTABLE_RTC@"/>
   <rosparam command="load"
             file="$(find @PROJECT_PKG_NAME@)/models/@ROBOT@_controller_config.yaml" />
 
@@ -11,7 +12,11 @@
     <arg name="COLLADA_FILE" value="$(find @PROJECT_PKG_NAME@)/models/@[email protected]" />
     <arg name="CONF_FILE" value="$(find @PROJECT_PKG_NAME@)/models/@[email protected]" />
     <arg name="corbaport" default="$(arg corbaport)" />
+    <arg name="USE_WALKING" default="true" if="$(arg USE_UNSTABLE_RTC)"/>
+    <arg name="USE_IMPEDANCECONTROLLER" default="true" if="$(arg USE_UNSTABLE_RTC)" />
+    <arg name="USE_EMERGENCYSTOPPER" default="true" if="$(arg USE_UNSTABLE_RTC)" />
 @ROSBRIDGE_ARGS@
+
   </include>
 
   <group if="$(arg RUN_RVIZ)" >

hrpsys_ros_bridge/scripts/default_robot_startup.launch.in

@@ -8,6 +8,7 @@
   <arg name="PROJECT_FILE" default="$(find @PROJECT_PKG_NAME@)/models/@[email protected]" unless="$(arg NOSIM)"/>
   <arg name="corbaport" default="15005" />
   <arg name="hrpsys_precreate_rtc" default="HGcontroller"/>
+  <arg name="USE_UNSTABLE_RTC" default="@USE_UNSTABLE_RTC@"/>
   <param name="use_sim_time" value="true" />
   <include file="$(find hrpsys_tools)/launch/hrpsys.launch">
     <arg name="PROJECT_FILE" value="$(arg PROJECT_FILE)"/>

hrpsys_ros_bridge/src/hrpsys_ros_bridge/samplerobot_hrpsys_config.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+
+pkg = 'hrpsys'
+import imp
+imp.find_module(pkg)
+
+from hrpsys.hrpsys_config import *
+import OpenHRP
+
+class SampleRobotHrpsysConfigurator(HrpsysConfigurator):
+    def getRTCList (self):
+        return self.getRTCListUnstable()
+    def init (self, robotname="SampleRobot", url=""):
+        HrpsysConfigurator.init(self, robotname, url)
+        print "initialize rtc parameters"
+        self.setStAbcParameters()
+
+    def defJointGroups (self):
+        rleg_6dof_group = ['rleg', ['RLEG_HIP_R', 'RLEG_HIP_P', 'RLEG_HIP_Y', 'RLEG_KNEE', 'RLEG_ANKLE_P', 'RLEG_ANKLE_R']]
+        lleg_6dof_group = ['lleg', ['LLEG_HIP_R', 'LLEG_HIP_P', 'LLEG_HIP_Y', 'LLEG_KNEE', 'LLEG_ANKLE_P', 'LLEG_ANKLE_R']]
+        torso_group = ['torso', ['WAIST_P', 'WAIST_R', 'CHEST']]
+        head_group = ['head', []]
+        rarm_group = ['rarm', ['RARM_SHOULDER_P', 'RARM_SHOULDER_R', 'RARM_SHOULDER_Y', 'RARM_ELBOW', 'RARM_WRIST_Y', 'RARM_WRIST_P', 'RARM_WRIST_R']]
+        larm_group = ['larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R']]
+        self.Groups = [rleg_6dof_group, lleg_6dof_group, torso_group, head_group, rarm_group, larm_group]
+
+    def setStAbcParameters (self):
+        # ST parameters
+        stp=self.st_svc.getParameter()
+        stp.st_algorithm=OpenHRP.StabilizerService.EEFMQP
+        #   eefm st params
+        stp.eefm_leg_inside_margin=71.12*1e-3
+        stp.eefm_leg_outside_margin=71.12*1e-3
+        stp.eefm_leg_front_margin=182.0*1e-3
+        stp.eefm_leg_rear_margin=72.0*1e-3
+        stp.eefm_k1=[-1.39899,-1.39899]
+        stp.eefm_k2=[-0.386111,-0.386111]
+        stp.eefm_k3=[-0.175068,-0.175068]
+        stp.eefm_rot_damping_gain=20*1.6*10 # Stiff parameter for simulation
+        stp.eefm_pos_damping_gain=[3500*50, 3500*50, 3500*1.0*5] # Stiff parameter for simulation
+        #   tpcc st params
+        stp.k_tpcc_p=[0.2, 0.2]
+        stp.k_tpcc_x=[4.0, 4.0]
+        stp.k_brot_p=[0.0, 0.0]
+        stp.k_brot_tc=[0.1, 0.1]
+        self.st_svc.setParameter(stp)
+
+    def __init__(self, robotname=""):
+        HrpsysConfigurator.__init__(self)
+        self.defJointGroups()
+
+if __name__ == '__main__':
+    hcf = SampleRobotHrpsysConfigurator()
+    if len(sys.argv) > 2 :
+        hcf.init(sys.argv[1], sys.argv[2])
+    elif len(sys.argv) > 1 :
+        hcf.init(sys.argv[1])
+    else :
+        hcf.init()

hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-auto-balancer.l

@@ -246,8 +246,9 @@
   (let ((fs-list (send *ri* :get-remaining-foot-step-sequence)))
     (while fs-list
       (setq fs-list (send *ri* :get-remaining-foot-step-sequence))
-      (send *irtviewer* :draw-objects :flush nil)
-      (send *ri* :draw-remaining-foot-step-sequence (send *irtviewer* :viewer) :flush t)
+      (unless (or (null x::*display*) (= x::*display* 0))
+        (send *irtviewer* :draw-objects :flush nil)
+        (send *ri* :draw-remaining-foot-step-sequence (send *irtviewer* :viewer) :flush t))
       (send *ri* :angle-vector (send *sr* :angle-vector) (* 1e3 (send (send *ri* :get-gait-generator-param) :default_step_time)))
       (send *ri* :wait-interpolation))
     t))
@@ -290,7 +291,7 @@
   (send *ri* :set-gait-generator-param :toe-angle 0 :heel-angle 0 :default-step-time 1.0 :default-step-height 0.05)
   )
 
-(defun samplerobot-auto-balancer-demo19 ()
+(defun samplerobot-auto-balancer-demo19-old ()
   (print "13. Overwrite footsteps during walking.")
   (send *ri* :set-foot-steps-no-wait
         (list (make-coords :coords (send *sr* :rleg :end-coords :copy-worldcoords) :name :rleg)

hrpsys_ros_bridge/test/hrpsys-samples/samplerobot-unittest.l

@@ -21,12 +21,20 @@
           (assert (,func)))))
 
 ;; Test demos
-(dolist (funcs (remove-if-not #'(lambda (x) ;; find demo functions
-                                  (and (substringp (format nil "~A-demo" *test-prefix*) (string-downcase x))
-                                       (not (string= (format nil "~A-demo" *test-prefix*) (string-downcase x)))))
-                              (functions)))
-  (eval `(deftest ,(read-from-string (format nil "test-~A" funcs))
-           (assert (,funcs)))))
+;; Test function should be
+;;   [test-prefix]-demo[number]
+;;   for example, samplerobot-sequence-player-demo5
+(let* ((demo-func-name (format nil "~A-demo" *test-prefix*))
+       (funcs (remove-if-not #'(lambda (x) ;; find demo functions
+                                 (and (substringp demo-func-name (string-downcase x))
+                                      (not (string= demo-func-name (string-downcase x))) ;; "[test-prefix]-demo" is total demo.
+                                      (numberp (read-from-string (subseq (string-downcase x) (length demo-func-name)))) ;; Find numbered-demo functions like "samplerobot-sequence-player-demo5"
+                                      ))
+                             (functions))))
+  (dolist (funcs (sort funcs #'< #'(lambda (x) (read-from-string (subseq (string-downcase x) (length demo-func-name)))))) ;; Sort by demo number
+    (eval `(deftest ,(read-from-string (format nil "test-~A" funcs))
+             (assert (,funcs))))
+    ))
 
 (run-all-tests)
-(exit 0)
+(exit 0)

hrpsys_ros_bridge/test/hrpsys-samples/test_samplerobot_euslisp_unittests.launch

@@ -10,6 +10,7 @@
     <arg name="GUI" default="$(arg GUI)" />
     <arg name="RUN_RVIZ" default="$(arg RUN_RVIZ)" />
     <arg name="corbaport" default="2809" />
+    <arg name="USE_UNSTABLE_RTC" default="true"/>
   </include>
 
   <test test-name="test_samplerobot_0_data_logger" pkg="roseus" type="roseus"
@@ -24,4 +25,16 @@
         args="$(find hrpsys_ros_bridge)/test/hrpsys-samples/samplerobot-unittest.l $(find hrpsys_ros_bridge)/test/hrpsys-samples/samplerobot-collision-detector.l"
         time-limit="300"
         retry="1" />
+  <test test-name="test_samplerobot_3_remove_force_sensor_offset" pkg="roseus" type="roseus"
+        args="$(find hrpsys_ros_bridge)/test/hrpsys-samples/samplerobot-unittest.l $(find hrpsys_ros_bridge)/test/hrpsys-samples/samplerobot-remove-force-offset.l"
+        time-limit="300"
+        retry="1" />
+  <test test-name="test_samplerobot_4_impedance_controller" pkg="roseus" type="roseus"
+        args="$(find hrpsys_ros_bridge)/test/hrpsys-samples/samplerobot-unittest.l $(find hrpsys_ros_bridge)/test/hrpsys-samples/samplerobot-impedance-controller.l"
+        time-limit="300"
+        retry="1" />
+  <test test-name="test_samplerobot_5_auto_balancer" pkg="roseus" type="roseus"
+        args="$(find hrpsys_ros_bridge)/test/hrpsys-samples/samplerobot-unittest.l $(find hrpsys_ros_bridge)/test/hrpsys-samples/samplerobot-auto-balancer.l"
+        time-limit="300"
+        retry="1" />
 </launch>