test_khi_robot_control_as.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # coding:utf-8
3 
4 # Software License Agreement (BSD License)
5 #
6 # Copyright (c) 2019, Kawasaki Heavy Industries, LTD.
7 # All rights reserved.
8 #
9 # Redistribution and use in source and binary forms, with or without
10 # modification, are permitted provided that the following conditions
11 # are met:
12 #
13 # * Redistributions of source code must retain the above copyright
14 # notice, this list of conditions and the following disclaimer.
15 # * Redistributions in binary form must reproduce the above
16 # copyright notice, this list of conditions and the following
17 # disclaimer in the documentation and/or other materials provided
18 # with the distribution.
19 # * Neither the name of the Willow Garage nor the names of its
20 # contributors may be used to endorse or promote products derived
21 # from this software without specific prior written permission.
22 #
23 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 # POSSIBILITY OF SUCH DAMAGE.
35 
36 PKG = 'khi_robot_test'
37 import roslib; roslib.load_manifest(PKG)
38 
39 import copy
40 import math
41 import sys
42 import unittest
43 import rospy
44 import geometry_msgs.msg
45 from khi_robot_msgs.srv import *
46 
47 if rospy.has_param('/test_group_name'):
48  gn = '/' + rospy.get_param('/test_group_name')
49 else:
50  gn = ''
51 service = gn + '/khi_robot_command_service'
52 
53 def cmdhandler_client(type_arg , cmd_arg):
54  rospy.wait_for_service(service)
55  try:
56  khi_robot_command_service = rospy.ServiceProxy(service,KhiRobotCmd)
57  resp1 = khi_robot_command_service(type_arg, cmd_arg)
58  return resp1
59  except rospy.ServiceException, e:
60  rospy.loginfo('Service call failed: %s', e)
61 
63  ret = cmdhandler_client('driver' , 'get_status')
64  return ret
65 
66 class TestKhiRobotControl(unittest.TestCase):
67  def test_program(self):
68  # ROS -> HOLDED
69  cmdhandler_client('driver', 'hold')
70  rospy.sleep(3)
71  ret = get_driver_state()
72  self.assertEqual('HOLDED', ret.cmd_ret)
73 
74  # ROS -> INACTIVE
75  cmdhandler_client('as', 'hold 1:')
76  rospy.sleep(3)
77  ret = cmdhandler_client('as', 'type switch(cs)')
78  self.assertEqual(0, int(ret.cmd_ret))
79  ret = get_driver_state()
80  self.assertEqual('INACTIVE', ret.cmd_ret)
81 
82  # execute robot program by AS
83  cmdhandler_client('as', 'zpow on')
84  cmdhandler_client('as', 'execute 1: rb_rtc1,-1')
85  ret = cmdhandler_client('as', 'type switch(cs)')
86  self.assertEqual(-1, int(ret.cmd_ret))
87  ret = get_driver_state()
88  self.assertEqual('INACTIVE', ret.cmd_ret)
89 
90  # ROS -> ACTIVE
91  cmdhandler_client('driver', 'restart')
92  rospy.sleep(3)
93  ret = get_driver_state()
94  self.assertEqual('ACTIVE', ret.cmd_ret)
95 
96  def test_signal(self):
97  size = 512
98  offset = [0, 2000, 2000]
99 
100  cmdhandler_client('as', 'ZINSIG ON')
101  cmdhandler_client('as', 'RESET')
102 
103  for i in range(size):
104  for j in range(3):
105  rospy.loginfo('SIGNAL %d', i+1+offset[j])
106  # positive
107  set_cmd = 'set_signal ' + str(i+1+offset[j])
108  cmdhandler_client('driver', set_cmd)
109  get_cmd = 'get_signal ' + str(i+1+offset[j])
110  ret = cmdhandler_client('driver', get_cmd)
111  self.assertEqual('-1', ret.cmd_ret)
112 
113  # negative
114  set_cmd = 'set_signal -' + str(i+1+offset[j])
115  cmdhandler_client('driver', set_cmd)
116  get_cmd = 'get_signal ' + str(i+1+offset[j])
117  ret = cmdhandler_client('driver', get_cmd)
118  self.assertEqual('0', ret.cmd_ret)
119 
120  def test_variable(self):
121  cmdhandler_client('as', 'test_val = 1')
122  ret = cmdhandler_client('as', 'type test_val')
123  self.assertEqual(1, int(ret.cmd_ret))
124 
125 if __name__ == '__main__':
126  import rostest
127  rospy.init_node("test_khi_robot_control_node")
128  cmdhandler_client('driver', 'restart')
129  rostest.rosrun(PKG, 'test_khi_robot_control', TestKhiRobotControl)
def cmdhandler_client(type_arg, cmd_arg)


khi_robot_test
Author(s): nakamichi_d
autogenerated on Fri Mar 26 2021 02:34:39