From 4c71191d2f653d9d3f043aa01c132bf5df3354c7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sun, 30 Nov 2014 19:01:04 -0800 Subject: [PATCH 1/4] add test code to check #287 problem --- .../test/test_hironx_ros_bridge.py | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/hironx_ros_bridge/test/test_hironx_ros_bridge.py b/hironx_ros_bridge/test/test_hironx_ros_bridge.py index 559776a7..d3923548 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.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", "LARM_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('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], From 7e48276050cab98999be6ac07d4076d500900e23 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 6 Jan 2015 12:49:38 +0900 Subject: [PATCH 2/4] add ROS_PARALLEL_JOBS to -j2 --- .travis.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 From c7300aecc8b7719cde1d242bb2ec92ce5c4eee13 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 6 Jan 2015 04:01:02 -0800 Subject: [PATCH 3/4] need to setup omniserver for new test code --- hironx_ros_bridge/test/test-hironx-ros-bridge.test | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 @@ - + From d6d7b534651eb04408f348caae0710181d76be1e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 6 Jan 2015 09:58:04 -0800 Subject: [PATCH 4/4] fix typo --- hironx_ros_bridge/test/test_hironx_ros_bridge.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hironx_ros_bridge/test/test_hironx_ros_bridge.py b/hironx_ros_bridge/test/test_hironx_ros_bridge.py index d3923548..9465c58f 100755 --- a/hironx_ros_bridge/test/test_hironx_ros_bridge.py +++ b/hironx_ros_bridge/test/test_hironx_ros_bridge.py @@ -224,12 +224,12 @@ def test_tf_and_controller(self): goal = self.goal_LArm() for av in [[ 25,-139,-157, 45, 0, 0]]: goal = self.setup_Positions(goal, [av]) - self.rarm.send_goal_and_wait(goal) + 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", "RARM_JOINT5_Link", now) + (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')