40 from hrpsys
import rtm
42 from hironx_ros_bridge
import hironx_client
as hironx
44 _ARMGROUP_TESTED =
'larm' 45 _GOINITIAL_TIME_MIDSPEED = 3
46 _JOINT_TESTED = (14,
'LARM_JOINT5')
47 _PKG =
'nextage_ros_bridge' 48 _POSE_LARM_JOINT5 = [-0.0017695671419774017, 0.00019009187609567157,
49 -0.99999841624735, 0.3255751238415409,
50 0.00012113080903022877, 0.9999999746368694,
51 0.00018987782289436872, 0.18236314012331808,
52 0.9999984269784911, -0.00012079461563276815,
53 -0.0017695901230784174, 1.037624522677471,
55 _INDICES_POSITION = 3, 7, 11
60 Test the derived methods of hrpsys_config.HrpsysConfigurator that are 61 overridden in hironx_client.HIRONX. 63 So far this test can be run by: 65 $ rosrun nextage_ros_bridge test_hironx_derivedmethods_rostest.py 67 #TODO: rostest (ie. boot from .launch file) not passing with this script 68 # for some reasons. Unless this test requires other nodes running, 69 # we might not need to run this from .launch? 71 #TODO: Merge with the existing unittest scripts in hironx_ros_bridge/test 77 modelfile =
'/opt/jsk/etc/NEXTAGE/model/main.wrl' 79 robotname =
"RobotHardware0" 82 self._robot.init(robotname=robotname, url=modelfile)
87 cls._robot.goOffPose()
93 self.assertFalse(
True)
98 self.assertFalse(
True)
101 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
102 pos = self._robot.getCurrentPose(_JOINT_TESTED[1])
103 rospy.loginfo(
'test_getCurrentPose compare pos={},' +
104 '\n\tPOSE_LARM_JOINT5={}'.format(pos, _POSE_LARM_JOINT5))
105 self.assertEqual(pos, _POSE_LARM_JOINT5)
108 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
109 pose = self._robot.getCurrentPose(_JOINT_TESTED[1])
110 position_from_getcurrentposition = self._robot.getCurrentPosition(
112 position_from_pose = [i
for j,
113 i
in enumerate(pose)
if j
in _INDICES_POSITION]
114 rospy.loginfo(
'test_getCurrentPosition compare position_from_getcurrentposition={},' +
115 '\n\tposition_from_pose={}'.format(
116 position_from_getcurrentposition, position_from_pose))
117 self.assertEqual(position_from_getcurrentposition, position_from_pose)
120 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
121 rot_matrix = self._robot.getCurrentRotation(_JOINT_TESTED[1])
125 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
126 rpy = self._robot.getCurrentRPY(_JOINT_TESTED[1])
130 self._robot.goInitial(_GOINITIAL_TIME_MIDSPEED)
131 joint_angles = self._robot.getJointAngles()
134 self.assertEqual(self._robot.InitialPose[3][5],
135 joint_angles[_JOINT_TESTED[0]])
137 if __name__ ==
'__main__':
139 rostest.rosrun(_PKG,
'test_hironx_derivedmethods',
140 TestHironxDerivedmethodsFromHrpsys)
def test_getCurrentPose(self)
def test_getCurrentPosition(self)
def test_setJointAngle_beyond_pi(self)
def test_getCurrentRotation(self)
def test_getJointAngles(self)
def test_getCurrentRPY(self)
def test_setJointAngle_within_pi(self)