Merge pull request #291 from k-okada/add_test_for_287

add test code to check #287 problem

.travis.yml

@@ -33,6 +33,7 @@ before_install: # Use this to prepare the system to install prerequisites or dep
   # Define some config vars
   - export CI_SOURCE_PATH=$(pwd)
   - export REPOSITORY_NAME=${PWD##*/}
+  - export ROS_PARALLEL_JOBS="-j2 -l2"
   - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"
   - sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list'
   - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
@@ -81,13 +82,13 @@ before_script: # Use this to prepare your build for testing e.g. copy database c
   - if [ $BUILDER == rosbuild ]; then rospack profile              ; fi
 script: # All commands must exit with code 0 on success. Anything else is considered failure.
   # for catkin
-  - if [ $BUILDER == catkin ]; then catkin_make -j8 -l8            ; fi
+  - if [ $BUILDER == catkin ]; then catkin_make ${ROS_PARALLEL_JOBS}            ; fi
   - if [ $BUILDER == catkin ]; then export TARGET_PKG=`find build/$REPOSITORY_NAME -name Makefile -print |  sed s@.*/\\\\\([^\/]*\\\\\)/Makefile@\\\1@g` ; fi
   # - if [ $BUILDER == catkin ]; then source devel/setup.sh; export EXIT_STATUS=0; for pkg in $TARGET_PKG; do (roscd $pkg; [ "`find . -iname '*.test'`" == "" ] && echo "[$pkg] No tests ware found!!!"  || find . -iname "*.test" -print0 | xargs -0 -n1 rostest || export EXIT_STATUS=$?;) done; [ $EXIT_STATUS == 0 ] ; fi
   - export EXIT_STATUS=$?
-  - if [ $BUILDER == catkin ]; then catkin_make test --pkg $TARGET_PKG -j8 -l8  ; export EXIT_STATUS=$?; fi
+  - if [ $BUILDER == catkin ]; then catkin_make test --pkg $TARGET_PKG ${ROS_PARALLEL_JOBS}  ; export EXIT_STATUS=$?; fi
   - if [ $EXIT_STATUS != 0 -a $BUILDER == catkin ]; then find build -name LastTest.log -exec echo "==== {} ====" \; -exec cat {} \;  ; fi
-  - if [ $BUILDER == catkin ]; then catkin_make -j8 -l8 install            ; fi
+  - if [ $BUILDER == catkin ]; then catkin_make ${ROS_PARALLEL_JOBS} install            ; fi
   - if [ $BUILDER == catkin ]; then rm -fr devel src build                 ; fi
   - if [ $BUILDER == catkin ]; then source install/setup.bash              ; fi
   - if [ $BUILDER == catkin ]; then export EXIT_STATUS=0; for pkg in $TARGET_PKG; do [ "`find install/share/$pkg -iname '*.test'`" == "" ] && echo "[$pkg] No tests ware found!!!"  || find install/share/$pkg -iname "*.test" -print0 | xargs -0 -n1 rostest || export EXIT_STATUS=$?; done; [ $EXIT_STATUS == 0 ] ; fi

hironx_ros_bridge/test/test-hironx-ros-bridge.test

@@ -16,5 +16,6 @@
   <param name="joint_states_test/test_duration" value="5.0" />
 
   <!-- test test-name="joint_states_test" pkg="rostest" type="hztest" name="joint_states_test" / -->
-  <test type="test_hironx_ros_bridge.py" pkg="hironx_ros_bridge" test-name="test_hironx_ros_bridge" time-limit="1000" />
+  <test type="test_hironx_ros_bridge.py" pkg="hironx_ros_bridge" test-name="test_hironx_ros_bridge" time-limit="1000"
+        args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" />
 </launch>

hironx_ros_bridge/test/test_hironx_ros_bridge.py

@@ -10,6 +10,7 @@
     import roslib; roslib.load_manifest(PKG)
     import hironx_ros_bridge
 
+from hironx_ros_bridge import hironx_client as hironx
 import rospy, actionlib, math, numpy
 import tf
 from tf.transformations import quaternion_matrix, euler_from_matrix
@@ -44,6 +45,9 @@ def __init__(self, *args, **kwargs):
         self.filename_base = tempfile.mkstemp()[1]
         self.filenames = []
 
+        self.robot = hironx.HIRONX()
+        self.robot.init()
+
     @classmethod
     def setUpClass(self):
 
@@ -201,6 +205,37 @@ def test_RArm(self):
                                                              [ 0, 1, 0],
                                                              [ 1, 0, 0]]), decimal=2)
 
+    def test_tf_and_controller(self):
+        goal = self.goal_RArm()
+        for av in [[  25,-139,-157,  45,   0,   0]]:
+            goal = self.setup_Positions(goal, [av])
+            self.rarm.send_goal_and_wait(goal)
+            # check if tf and current link is same
+            rospy.sleep(1)
+            now = rospy.Time.now()
+            self.listener.waitForTransform("WAIST", "RARM_JOINT5_Link", now, rospy.Duration(1.0))
+            (pos_tf, rot_tf) = self.listener.lookupTransform("WAIST", "RARM_JOINT5_Link", now)
+            rot_tf = quaternion_matrix(rot_tf)[0:3,0:3]
+            pos_c = self.robot.getCurrentPosition('RARM_JOINT5','WAIST')
+            rot_c = self.robot.getCurrentRotation('RARM_JOINT5','WAIST')
+            numpy.testing.assert_array_almost_equal(pos_tf, pos_c, decimal=3)
+            numpy.testing.assert_array_almost_equal(rot_tf, rot_c, decimal=2)
+
+        goal = self.goal_LArm()
+        for av in [[  25,-139,-157,  45,   0,   0]]:
+            goal = self.setup_Positions(goal, [av])
+            self.larm.send_goal_and_wait(goal)
+            # check if tf and current link is same
+            rospy.sleep(1)
+            now = rospy.Time.now()
+            self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
+            (pos_tf, rot_tf) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
+            rot_tf = quaternion_matrix(rot_tf)[0:3,0:3]
+            pos_c = self.robot.getCurrentPosition('LARM_JOINT5','WAIST')
+            rot_c = self.robot.getCurrentRotation('LARM_JOINT5','WAIST')
+            numpy.testing.assert_array_almost_equal(pos_tf, pos_c, decimal=3)
+            numpy.testing.assert_array_almost_equal(rot_tf, rot_c, decimal=2)
+
     def test_Torso(self):
         goal = self.goal_Torso()
         goal = self.setup_Positions(goal, [[   0.0],