forked from fkanehiro/hrpsys-base
-
Notifications
You must be signed in to change notification settings - Fork 0
/
test-samplerobot.py
executable file
·84 lines (64 loc) · 2.36 KB
/
test-samplerobot.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#!/usr/bin/env python
PKG = 'hrpsys'
NAME = 'test-samplerobot'
import imp ## for rosbuild
try:
imp.find_module(PKG)
except:
import roslib; roslib.load_manifest(PKG)
import os
import sys
import time
import unittest
import yaml
import rostest
from hrpsys import rtm
from hrpsys.hrpsys_config import *
import OpenHRP
rtm.nsport = 2809
class SampleRobot(HrpsysConfigurator):
def getRTCList(self):
return [
['seq', "SequencePlayer"],
['sh', "StateHolder"],
['fk', "ForwardKinematics"],
['log', "DataLogger"],
]
class TestJointAngle(unittest.TestCase):
@classmethod
def setUpClass(self):
h = SampleRobot()
h.init(robotname="SampleRobot(Robot)0")
def test_set_if_find_log(self):
h = SampleRobot()
h.findComps()
print >>sys.stderr, "log=",h.log, "log_svc=",h.log_svc
self.assertTrue(h.log)
self.assertTrue(h.log_svc)
def test_load_pattern(self):
h = SampleRobot()
h.findComps()
from subprocess import check_output
openhrp3_path = check_output(['rospack','find','openhrp3']).rstrip() # for rosbuild
PKG_CONFIG_PATH=""
if os.path.exists(os.path.join(openhrp3_path, "bin")) :
PKG_CONFIG_PATH='PKG_CONFIG_PATH=%s/lib/pkgconfig:$PKG_CONFIG_PATH'%(openhrp3_path)
cmd = "%s pkg-config openhrp3.1 --variable=idl_dir"%(PKG_CONFIG_PATH)
os.path.join(check_output(cmd, shell=True).rstrip(), "../sample/controller/SampleController/etc/Sample")
h.loadPattern(os.path.join(check_output(cmd, shell=True).rstrip(), "../sample/controller/SampleController/etc/Sample"), 1)
self.assertEqual(h.waitInterpolation(), None)
def test_set_joint_angles(self):
h = SampleRobot()
h.findComps()
self.assertTrue(h.setJointAngles(h.getJointAngles(),1))
self.assertEqual(h.waitInterpolation(), None)
import random
a = [(360*random.random()-180) for i in xrange(len(h.getJointAngles()))]
self.assertTrue(h.setJointAngles(a,2))
self.assertEqual(h.waitInterpolation(), None)
a = [0 for i in xrange(len(h.getJointAngles()))]
self.assertTrue(h.setJointAngles(a,2))
self.assertEqual(h.waitInterpolation(), None)
#unittest.main()
if __name__ == '__main__':
rostest.run(PKG, NAME, TestJointAngle, sys.argv)