test_thruster_allocator.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Copyright (c) 2016-2019 The UUV Simulator Authors.
3 # All rights reserved.
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 import rospy
17 import roslib
18 import numpy as np
19 import unittest
20 import time
21 from geometry_msgs.msg import WrenchStamped
22 from uuv_thruster_manager.srv import GetThrusterManagerConfig, ThrusterManagerInfo
23 from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
24 # Other imports
25 
26 PKG = 'uuv_thruster_manager'
27 roslib.load_manifest(PKG)
28 
29 NS = 'test_vehicle'
30 
31 AXIS_X_TAM = np.array([
32  [1, 0, 0, 0, 0, 0],
33  [0.87758256, 0, -0.47942554, 0.47942554, 0.47942554, 0.87758256],
34  [0.87758256, 0.47942554, 0, -0.47942554, 0.87758256, -0.87758256]
35 ]).T
36 
37 AXIS_Y_TAM = np.array([
38  [0, 0.87758256, 0.47942554, 0, 0.47942554, -0.87758256],
39  [0, 1, 0, 0, 0, 1],
40  [-0.47942554, 0.87758256, 0, -0.87758256, -0.47942554, 0.47942554]
41 ]).T
42 
43 AXIS_Z_TAM = np.array([
44  [0, -0.47942554, 0.87758256, 0, 0.87758256, 0.47942554],
45  [0.47942554, 0, 0.87758256, -0.87758256, -0.87758256, 0.47942554],
46  [0., 0., 1., 1., 0., 0.]
47 ]).T
48 
49 class TestThrusterAllocator(unittest.TestCase):
51  srvs = [
52  'thruster_manager/get_thrusters_info',
53  'thruster_manager/get_thruster_curve',
54  'thruster_manager/set_config',
55  'thruster_manager/get_config'
56  ]
57 
58  for srv in srvs:
59  rospy.wait_for_service('/{}/{}'.format(NS, srv), timeout=1000)
60 
61  def test_config(self):
62  axis = rospy.get_param('axis')
63  ref_config = rospy.get_param('/{}/thruster_manager'.format(NS))
64 
65  rospy.wait_for_service('/{}/thruster_manager/get_config'.format(NS), timeout=1000)
66  srv = rospy.ServiceProxy('/{}/thruster_manager/get_config'.format(NS), GetThrusterManagerConfig)
67  tm_config = srv()
68 
69  self.assertEqual(tm_config.tf_prefix, '/test_vehicle/')
70  self.assertEqual(tm_config.base_link, 'base_link')
71  self.assertEqual(tm_config.thruster_frame_base, 'thruster_')
72  self.assertEqual(tm_config.thruster_topic_suffix, '/input')
73  self.assertEqual(tm_config.timeout, -1)
74  self.assertEqual(tm_config.max_thrust, 1000.0)
75  self.assertEqual(tm_config.n_thrusters, 3)
76 
77  if axis == 'x':
78  tam_flat = AXIS_X_TAM.flatten()
79  elif axis == 'y':
80  tam_flat = AXIS_Y_TAM.flatten()
81  elif axis == 'z':
82  tam_flat = AXIS_Z_TAM.flatten()
83 
84  self.assertEqual(len(tm_config.allocation_matrix), tam_flat.size)
85  for x, y in zip(tam_flat, tm_config.allocation_matrix):
86  self.assertAlmostEqual(x, y)
87 
88 
89 if __name__ == '__main__':
90  import rosunit
91  rosunit.unitrun(PKG, 'test_thruster_allocator', TestThrusterAllocator)
92 
93 


uuv_thruster_manager
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:39