test-samplerobot.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 PKG = 'hrpsys'
00004 NAME = 'test-samplerobot'
00005 
00006 import imp  ## for rosbuild
00007 try:
00008     imp.find_module(PKG)
00009 except:
00010     import roslib; roslib.load_manifest(PKG)
00011 
00012 
00013 import os
00014 import sys
00015 import time
00016 import unittest
00017 import yaml
00018 
00019 import rostest
00020 from hrpsys import rtm
00021 from hrpsys.hrpsys_config import *
00022 import OpenHRP
00023 
00024 rtm.nsport = 2809
00025 
00026 class SampleRobot(HrpsysConfigurator):
00027     def getRTCList(self):
00028         return [
00029             ['seq', "SequencePlayer"],
00030             ['sh', "StateHolder"],
00031             ['fk', "ForwardKinematics"],
00032             ['log', "DataLogger"],
00033             ]
00034 
00035 
00036 
00037 class TestJointAngle(unittest.TestCase):
00038 
00039     @classmethod
00040     def setUpClass(self):
00041         h = SampleRobot()
00042         h.init(robotname="SampleRobot(Robot)0")
00043 
00044     def test_set_if_find_log(self):
00045         h = SampleRobot()
00046         h.findComps()
00047         print >>sys.stderr, "log=",h.log, "log_svc=",h.log_svc
00048         self.assertTrue(h.log)
00049         self.assertTrue(h.log_svc)
00050 
00051     def test_load_pattern(self):
00052         h = SampleRobot()
00053         h.findComps()
00054         from subprocess import check_output
00055         openhrp3_path = check_output(['rospack','find','openhrp3']).rstrip() # for rosbuild
00056         PKG_CONFIG_PATH=""
00057         if os.path.exists(os.path.join(openhrp3_path, "bin")) :
00058             PKG_CONFIG_PATH='PKG_CONFIG_PATH=%s/lib/pkgconfig:$PKG_CONFIG_PATH'%(openhrp3_path)
00059 
00060         cmd = "%s pkg-config openhrp3.1 --variable=idl_dir"%(PKG_CONFIG_PATH)
00061         os.path.join(check_output(cmd, shell=True).rstrip(), "../sample/controller/SampleController/etc/Sample")
00062         h.loadPattern(os.path.join(check_output(cmd, shell=True).rstrip(), "../sample/controller/SampleController/etc/Sample"), 1)
00063         self.assertEqual(h.waitInterpolation(), None)
00064 
00065     def test_set_joint_angles(self):
00066         h = SampleRobot()
00067         h.findComps()
00068         self.assertTrue(h.setJointAngles(h.getJointAngles(),1))
00069         self.assertEqual(h.waitInterpolation(), None)
00070 
00071         import random
00072         a = [(360*random.random()-180) for i in xrange(len(h.getJointAngles()))]
00073         self.assertTrue(h.setJointAngles(a,2))
00074         self.assertEqual(h.waitInterpolation(), None)
00075 
00076         a = [0 for i in xrange(len(h.getJointAngles()))]
00077         self.assertTrue(h.setJointAngles(a,2))
00078         self.assertEqual(h.waitInterpolation(), None)
00079 
00080 #unittest.main()
00081 if __name__ == '__main__':
00082     rostest.run(PKG, NAME, TestJointAngle, sys.argv)
00083 
00084 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:19