Go to the documentation of this file.00001
00002
00003
00004 PKG = 'hironx_ros_bridge'
00005
00006 try:
00007 import roslib
00008 import hironx_ros_bridge
00009 except:
00010 import roslib; roslib.load_manifest(PKG)
00011 import hironx_ros_bridge
00012
00013 from hironx_ros_bridge import hironx_client as hironx
00014 from hrpsys.hrpsys_config import euler_from_matrix
00015 from hrpsys import rtm
00016
00017 import os
00018 import unittest
00019 import time
00020 import tempfile
00021
00022 import math
00023 import random
00024 import numpy
00025
00026 from rtm import connectPorts, disconnectPorts
00027
00028 class TestHiro(unittest.TestCase):
00029
00030 @classmethod
00031 def setUpClass(cls):
00032 modelfile = rospy.get_param("hironx/collada_model_filepath")
00033 rtm.nshost = 'hiro014'
00034 robotname = "RobotHardware0"
00035
00036 cls.robot = hironx.HIRONX()
00037 cls.robot.init(robotname=robotname, url=modelfile)
00038
00039 @classmethod
00040 def tearDownClass(cls):
00041
00042 True
00043
00044 def test_log(self):
00045 av_r = [-0.6, -90, -100, 15.2, 9.4, 3.2]
00046 av_l = [0.6, -90, -100, -15.2, 9.4,-3.2]
00047 self.robot.servoOn()
00048 self.robot.goInitial()
00049 self.robot.log_svc.maxLength(200*15*3)
00050 self.robot.clearLog()
00051 for j in range(3):
00052 self.robot.setJointAnglesOfGroup( "rarm" , av_r, 5)
00053 self.robot.waitInterpolationOfGroup("rarm")
00054 self.robot.setJointAnglesOfGroup( "larm" , av_l, 5)
00055 self.robot.waitInterpolationOfGroup("larm")
00056 self.robot.goInitial(tm=5)
00057 self.robot.saveLog("/tmp/test_hironx_log")
00058 self.robot.goOffPose()
00059
00060
00061
00062 if __name__ == '__main__':
00063 import rostest
00064 rostest.rosrun(PKG, 'test_hronx_log', TestHiro)
00065 print("===================================================")
00066 print("# Please consult test result with following process")
00067 print("sh /tmp/check-test_hironx_log.sh")
00068 command = "scp hiro@hiro014:/tmp/test_hironx_log.* /tmp/;"
00069 command += "gnuplot -e 'set style line 1 pointsize 1; plot "
00070 for i in [5,6,7,8,9,10, 11,12,13,14,15,16]:
00071 command += '"/tmp/test_hironx_log.RobotHardware0_q" using 1:%d with point pointtype %d, '%(i, i-4)
00072 command += '"/tmp/test_hironx_log.sh_qOut" using 1:%d with linespoints pointtype %d, '%(i, i-4)
00073 command += "0; pause -1'"
00074 with open("/tmp/check-test_hironx_log.sh", "w") as f:
00075 f.write(command)
00076