hironx_ros_bridge: test_hironx_controller.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 from test_hironx import *
00005 
00006 class TestHiroController(TestHiro):
00007 
00008     # https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_impedance_controller.py.in
00009     def test_impedance_controller(self): # https://github.com/start-jsk/rtmros_hironx/issues/337
00010         if not self.robot.ic or self.robot.ic.ref.get_component_profile().version < '315.3.0':
00011             self.assertTrue(True)
00012             return True
00013         # although this is not stable rtc, we'll test them
00014         self.robot.goInitial(tm=1)
00015         ret = self.robot.startImpedance('rarm') # this returns ret, this is bug
00016         #self.assertTrue(ret)
00017         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00018                                               10,0,0,0,0,0,], 2.0);
00019         #self.assertTrue(ret)
00020         self.robot.seq_svc.waitInterpolation();
00021         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00022                                               0,0,0,0,0,0,], 2.0);
00023         #self.assertTrue(ret)
00024         self.robot.seq_svc.waitInterpolation();
00025         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00026                                               0,10,0,0,0,0,], 2.0);
00027         #self.assertTrue(ret)
00028         self.robot.seq_svc.waitInterpolation();
00029         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00030                                               0,0,0,0,0,0,], 2.0);
00031         #self.assertTrue(ret)
00032         self.robot.seq_svc.waitInterpolation();
00033         ret = self.robot.seq_svc.setWrenches([0,0,0,0,0,0,
00034                                               0,0,10,0,0,0,], 2.0);
00035         #self.assertTrue(ret)
00036         self.robot.seq_svc.waitInterpolation();
00037         #self.assertTrue(ret)
00038         ret = self.robot.stopImpedance('rarm')
00039         #self.assertTrue(ret)
00040         self.assertTrue(True) # this is dummy, current simulate hiro does not have force sensor so it retunrs None
00041 
00042     def test_hands_controller(self):
00043         '''
00044         If Servo Controller RTC are running (which will be always true for
00045         the tests because it runs on simulation), kill it then test if servoOn
00046         method still succeeds.
00047         '''
00048         if self.robot.sc_svc:
00049             self.robot.sc_svc = None
00050         else:
00051             print('Servo Controller RTC is not running, so skipping this test.')
00052             pass
00053         self.assertEqual(self.robot.servoOn(), 1)
00054 
00055 if __name__ == '__main__':
00056     import rostest
00057     rostest.rosrun(PKG, 'test_hronx_controller', TestHiroController)