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 from gazebo_msgs.msg import LinkStates
00040 from geometry_msgs.msg import Pose
00041 import rospy
00042
00043
00044 class TestNxoGazebo(unittest.TestCase):
00045 LINKS = ['ground_plane::link', 'NextageOpen::WAIST',
00046 'NextageOpen::CHEST_JOINT0_Link',
00047 'NextageOpen::HEAD_JOINT0_Link', 'NextageOpen::HEAD_JOINT1_Link',
00048 'NextageOpen::LARM_JOINT0_Link', 'NextageOpen::LARM_JOINT1_Link', 'NextageOpen::LARM_JOINT2_Link', 'NextageOpen::LARM_JOINT3_Link', 'NextageOpen::LARM_JOINT4_Link', 'NextageOpen::LARM_JOINT5_Link',
00049 'NextageOpen::RARM_JOINT0_Link', 'NextageOpen::RARM_JOINT1_Link', 'NextageOpen::RARM_JOINT2_Link', 'NextageOpen::RARM_JOINT3_Link', 'NextageOpen::RARM_JOINT4_Link', 'NextageOpen::RARM_JOINT5_Link']
00050 POSES_INIT = [
00051 [[ 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 1.0]],
00052 [[ 0.00869031777881, 0.00936108107894, 0.970000019992], [ 4.52986285804e-06, 9.1893585999e-07, 0.000198866987408, 0.999999980215]],
00053 [[ 0.00869026831415, 0.00936080520858, 0.969999965706], [ 2.94622369018e-06, 1.3456210695e-06, -0.000126180352162, 0.999999992034]],
00054 [[ 0.00869141396909, 0.00935955870271, 1.53949992447], [ 8.45267344191e-07, 7.30178323418e-07, -0.000131509094738, 0.999999991352]],
00055 [[ 0.00869143715228, 0.0093595715095, 1.53949989454], [ 1.01216879393e-06, 4.83962982629e-06, -0.000131441661779, 0.999999991349]],
00056 [[ 0.00872762107774, 0.154360006834, 1.34029621598], [ -0.130527499716, 3.7821173448e-05, 2.6239331832e-05, 0.991444688169]],
00057 [[ 0.00872766863833, 0.154360191609, 1.34029623983], [ -0.130527069459, 0.000269559123027, -4.63162615923e-06, 0.991444709227]],
00058 [[ 0.00858791298884, 0.181417572639, 1.07422723747], [ -0.091005332606, -0.710566966204, 0.0935703234198, 0.691416813906]],
00059 [[ 0.00940755565144, 0.173655626629, 1.04526064458], [ -0.090885098394, -0.710582204533, 0.0934535149189, 0.691432766103]],
00060 [[ 0.244320109519, 0.175331850808, 1.05145851947], [ -0.0909400891469, -0.710174638341, 0.0934000106546, 0.691851372228]],
00061 [[ 0.335517794548, 0.163801033044, 1.00834272469], [ -0.0908880315601, -0.710167606022, 0.0934534767984, 0.69185821145]],
00062 [[ 0.00865434250886, -0.135639383827, 1.34029577734], [ 0.13052792502, 9.10844525273e-06, -0.000308631661141, 0.991444585165]],
00063 [[ 0.00865441900623, -0.135638679716, 1.34029592469], [ 0.130523056036, 0.000264943619984, -0.000273859879479, 0.991445201032]],
00064 [[ 0.00848275607192, -0.162697817784, 1.07422672949], [ 0.0908048466902, -0.71057338265, -0.093765722566, 0.691410107649]],
00065 [[ 0.0093050984273, -0.154936315414, 1.04526041866], [ 0.0907054333018, -0.710585857087, -0.0936678744598, 0.691423599085]],
00066 [[ 0.244217095143, -0.156743001062, 1.05144546797], [ 0.0907579071769, -0.710197835911, -0.0936164077221, 0.691822234651]],
00067 [[ 0.335421275762, -0.145261511825, 1.0083303359], [ 0.0907167631829, -0.710192142524, -0.0936591102354, 0.691827695777]]]
00068
00069 def _cb_gz_linkstates(self, data):
00070 self._linkstates = data
00071
00072 def __init__(self, *args, **kwargs):
00073 super(TestNxoGazebo, self).__init__(*args, **kwargs)
00074 rospy.init_node('test_nxo_gazebo')
00075 rospy.loginfo("need to wait for finishing go_initial.py (https://github.com/tork-a/rtmros_nextage/pull/223/files#diff-16b25951a50b1e80569929d32a09102bR14)")
00076 rospy.sleep(3+4+3)
00077 rospy.sleep(5)
00078 rospy.loginfo("start test")
00079 self._subscriber_gz_linkstates = rospy.Subscriber('/gazebo/link_states', LinkStates, self._cb_gz_linkstates)
00080
00081 @classmethod
00082 def setUpClass(self):
00083 rospy.sleep(5)
00084
00085 @classmethod
00086 def tearDownClass(self):
00087 True
00088
00089 def test_go_initial(self):
00090 '''Check if arms are moved to init pose'''
00091 _ORDER_PERMISSIBLE = 1
00092
00093 self.assertEqual(sorted(self.LINKS), sorted(self._linkstates.name))
00094
00095
00096 i = 0
00097 for pose in self._linkstates.pose:
00098 rospy.loginfo("check pose " + self.LINKS[i])
00099
00100
00101 self.assertAlmostEqual(self.POSES_INIT[i][0][0], pose.position.x, places = _ORDER_PERMISSIBLE)
00102 self.assertAlmostEqual(self.POSES_INIT[i][0][1], pose.position.y, places = _ORDER_PERMISSIBLE)
00103 self.assertAlmostEqual(self.POSES_INIT[i][0][2], pose.position.z, places = _ORDER_PERMISSIBLE)
00104 self.assertAlmostEqual(self.POSES_INIT[i][1][0], pose.orientation.x, places = _ORDER_PERMISSIBLE)
00105 self.assertAlmostEqual(self.POSES_INIT[i][1][1], pose.orientation.y, places = _ORDER_PERMISSIBLE)
00106 self.assertAlmostEqual(self.POSES_INIT[i][1][2], pose.orientation.z, places = _ORDER_PERMISSIBLE)
00107 self.assertAlmostEqual(self.POSES_INIT[i][1][3], pose.orientation.w, places = _ORDER_PERMISSIBLE)
00108 i += 1
00109
00110 if __name__ == '__main__':
00111 import rostest
00112 rostest.rosrun('nextage_gazebo', 'test_nxo_gz', TestNxoGazebo)