test_hironx_log.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 PKG = 'hironx_ros_bridge'
00005 # rosbuild needs load_manifest
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         #self.write_output_to_pdf("test-hironx.pdf") # don't know how to call this mehtod
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 #unittest.main()
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 


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37