39 from gazebo_msgs.msg
import LinkStates
40 from geometry_msgs.msg
import Pose
45 LINKS = [
'ground_plane::link',
'NextageOpen::WAIST',
46 'NextageOpen::CHEST_JOINT0_Link',
47 'NextageOpen::HEAD_JOINT0_Link',
'NextageOpen::HEAD_JOINT1_Link',
48 'NextageOpen::LARM_JOINT0_Link',
'NextageOpen::LARM_JOINT1_Link',
'NextageOpen::LARM_JOINT2_Link',
'NextageOpen::LARM_JOINT3_Link',
'NextageOpen::LARM_JOINT4_Link',
'NextageOpen::LARM_JOINT5_Link',
49 'NextageOpen::RARM_JOINT0_Link',
'NextageOpen::RARM_JOINT1_Link',
'NextageOpen::RARM_JOINT2_Link',
'NextageOpen::RARM_JOINT3_Link',
'NextageOpen::RARM_JOINT4_Link',
'NextageOpen::RARM_JOINT5_Link']
51 [[ 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 1.0]],
52 [[ 0.00869031777881, 0.00936108107894, 0.970000019992], [ 4.52986285804e-06, 9.1893585999e-07, 0.000198866987408, 0.999999980215]],
53 [[ 0.00869026831415, 0.00936080520858, 0.969999965706], [ 2.94622369018e-06, 1.3456210695e-06, -0.000126180352162, 0.999999992034]],
54 [[ 0.00869141396909, 0.00935955870271, 1.53949992447], [ 8.45267344191e-07, 7.30178323418e-07, -0.000131509094738, 0.999999991352]],
55 [[ 0.00869143715228, 0.0093595715095, 1.53949989454], [ 1.01216879393e-06, 4.83962982629e-06, -0.000131441661779, 0.999999991349]],
56 [[ 0.00872762107774, 0.154360006834, 1.34029621598], [ -0.130527499716, 3.7821173448e-05, 2.6239331832e-05, 0.991444688169]],
57 [[ 0.00872766863833, 0.154360191609, 1.34029623983], [ -0.130527069459, 0.000269559123027, -4.63162615923e-06, 0.991444709227]],
58 [[ 0.00858791298884, 0.181417572639, 1.07422723747], [ -0.091005332606, -0.710566966204, 0.0935703234198, 0.691416813906]],
59 [[ 0.00940755565144, 0.173655626629, 1.04526064458], [ -0.090885098394, -0.710582204533, 0.0934535149189, 0.691432766103]],
60 [[ 0.244320109519, 0.175331850808, 1.05145851947], [ -0.0909400891469, -0.710174638341, 0.0934000106546, 0.691851372228]],
61 [[ 0.335517794548, 0.163801033044, 1.00834272469], [ -0.0908880315601, -0.710167606022, 0.0934534767984, 0.69185821145]],
62 [[ 0.00865434250886, -0.135639383827, 1.34029577734], [ 0.13052792502, 9.10844525273e-06, -0.000308631661141, 0.991444585165]],
63 [[ 0.00865441900623, -0.135638679716, 1.34029592469], [ 0.130523056036, 0.000264943619984, -0.000273859879479, 0.991445201032]],
64 [[ 0.00848275607192, -0.162697817784, 1.07422672949], [ 0.0908048466902, -0.71057338265, -0.093765722566, 0.691410107649]],
65 [[ 0.0093050984273, -0.154936315414, 1.04526041866], [ 0.0907054333018, -0.710585857087, -0.0936678744598, 0.691423599085]],
66 [[ 0.244217095143, -0.156743001062, 1.05144546797], [ 0.0907579071769, -0.710197835911, -0.0936164077221, 0.691822234651]],
67 [[ 0.335421275762, -0.145261511825, 1.0083303359], [ 0.0907167631829, -0.710192142524, -0.0936591102354, 0.691827695777]]]
73 super(TestNxoGazebo, self).
__init__(*args, **kwargs)
74 rospy.init_node(
'test_nxo_gazebo')
75 rospy.loginfo(
"need to wait for finishing go_initial.py (https://github.com/tork-a/rtmros_nextage/pull/223/files#diff-16b25951a50b1e80569929d32a09102bR14)")
78 rospy.loginfo(
"start test")
90 '''Check if arms are moved to init pose''' 91 _ORDER_PERMISSIBLE = 1
93 self.assertEqual(sorted(self.
LINKS), sorted(self._linkstates.name))
97 for pose
in self._linkstates.pose:
98 rospy.loginfo(
"check pose " + self.
LINKS[i])
101 self.assertAlmostEqual(self.
POSES_INIT[i][0][0], pose.position.x, places = _ORDER_PERMISSIBLE)
102 self.assertAlmostEqual(self.
POSES_INIT[i][0][1], pose.position.y, places = _ORDER_PERMISSIBLE)
103 self.assertAlmostEqual(self.
POSES_INIT[i][0][2], pose.position.z, places = _ORDER_PERMISSIBLE)
104 self.assertAlmostEqual(self.
POSES_INIT[i][1][0], pose.orientation.x, places = _ORDER_PERMISSIBLE)
105 self.assertAlmostEqual(self.
POSES_INIT[i][1][1], pose.orientation.y, places = _ORDER_PERMISSIBLE)
106 self.assertAlmostEqual(self.
POSES_INIT[i][1][2], pose.orientation.z, places = _ORDER_PERMISSIBLE)
107 self.assertAlmostEqual(self.
POSES_INIT[i][1][3], pose.orientation.w, places = _ORDER_PERMISSIBLE)
110 if __name__ ==
'__main__':
112 rostest.rosrun(
'nextage_gazebo',
'test_nxo_gz', TestNxoGazebo)
def __init__(self, args, kwargs)
def test_go_initial(self)
def _cb_gz_linkstates(self, data)
_subscriber_gz_linkstates