Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 import unittest
00038
00039 import rospy
00040 from hrpsys import rtm
00041
00042 from hironx_ros_bridge import hironx_client as hironx
00043
00044 _ARMGROUP_TESTED = 'larm'
00045 _GOINITIAL_TIME_MIDSPEED = 3
00046 _JOINT_TESTED = (14, 'LARM_JOINT5')
00047 _PKG = 'nextage_ros_bridge'
00048 _POSE_LARM_JOINT5 = [-0.0017695671419774017, 0.00019009187609567157,
00049 -0.99999841624735, 0.3255751238415409,
00050 0.00012113080903022877, 0.9999999746368694,
00051 0.00018987782289436872, 0.18236314012331808,
00052 0.9999984269784911, -0.00012079461563276815,
00053 -0.0017695901230784174, 1.037624522677471,
00054 0.0, 0.0, 0.0, 1.0]
00055 _INDICES_POSITION = 3, 7, 11
00056
00057
00058 class TestHironxDerivedmethodsFromHrpsys(unittest.TestCase):
00059 '''
00060 Test the derived methods of hrpsys_config.HrpsysConfigurator that are
00061 overridden in hironx_client.HIRONX.
00062
00063 So far this test can be run by:
00064
00065 $ rosrun nextage_ros_bridge test_hironx_derivedmethods_rostest.py
00066
00067 #TODO: rostest (ie. boot from .launch file) not passing with this script
00068 # for some reasons. Unless this test requires other nodes running,
00069 # we might not need to run this from .launch?
00070
00071 #TODO: Merge with the existing unittest scripts in hironx_ros_bridge/test
00072 '''
00073
00074 @classmethod
00075 def setUpClass(self):
00076
00077 modelfile = '/opt/jsk/etc/HIRONX/model/main.wrl'
00078 rtm.nshost = 'nxc100'
00079 robotname = "RobotHardware0"
00080
00081 self._robot = hironx.HIRONX()
00082 self._robot.init(robotname=robotname, url=modelfile)
00083
00084
00085 @classmethod
00086 def tearDownClass(cls):
00087 cls._robot.goOffPose()
00088 True
00089
00090 def test_setJointAngle_within_pi(self):
00091
00092
00093 self.assertFalse(True)
00094
00095 def test_setJointAngle_beyond_pi(self):
00096
00097
00098 self.assertFalse(True)
00099
00100 def test_getCurrentPose(self):
00101 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00102 pos = self._robot.getCurrentPose(_JOINT_TESTED[1])
00103 rospy.loginfo('test_getCurrentPose compare pos={},' +
00104 '\n\tPOSE_LARM_JOINT5={}'.format(pos, _POSE_LARM_JOINT5))
00105 self.assertEqual(pos, _POSE_LARM_JOINT5)
00106
00107 def test_getCurrentPosition(self):
00108 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00109 pose = self._robot.getCurrentPose(_JOINT_TESTED[1])
00110 position_from_getcurrentposition = self._robot.getCurrentPosition(
00111 _JOINT_TESTED[1])
00112 position_from_pose = [i for j,
00113 i in enumerate(pose) if j in _INDICES_POSITION]
00114 rospy.loginfo('test_getCurrentPosition compare position_from_getcurrentposition={},' +
00115 '\n\tposition_from_pose={}'.format(
00116 position_from_getcurrentposition, position_from_pose))
00117 self.assertEqual(position_from_getcurrentposition, position_from_pose)
00118
00119 def test_getCurrentRotation(self):
00120 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00121 rot_matrix = self._robot.getCurrentRotation(_JOINT_TESTED[1])
00122
00123
00124 def test_getCurrentRPY(self):
00125 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00126 rpy = self._robot.getCurrentRPY(_JOINT_TESTED[1])
00127
00128
00129 def test_getJointAngles(self):
00130 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
00131 joint_angles = self._robot.getJointAngles()
00132
00133
00134 self.assertEqual(self._robot.InitialPose[3][5],
00135 joint_angles[_JOINT_TESTED[0]])
00136
00137 if __name__ == '__main__':
00138 import rostest
00139 rostest.rosrun(_PKG, 'test_hironx_derivedmethods',
00140 TestHironxDerivedmethodsFromHrpsys)