diff --git a/.travis.yml b/.travis.yml
index e595b9b5..e30bbd68 100644
--- a/.travis.yml
+++ b/.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
diff --git a/hironx_ros_bridge/test/test-hironx-ros-bridge.test b/hironx_ros_bridge/test/test-hironx-ros-bridge.test
index d8da7f83..b51a53e5 100644
--- a/hironx_ros_bridge/test/test-hironx-ros-bridge.test
+++ b/hironx_ros_bridge/test/test-hironx-ros-bridge.test
@@ -16,5 +16,6 @@
-
+
diff --git a/hironx_ros_bridge/test/test_hironx_ros_bridge.py b/hironx_ros_bridge/test/test_hironx_ros_bridge.py
index 559776a7..9465c58f 100755
--- a/hironx_ros_bridge/test/test_hironx_ros_bridge.py
+++ b/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],