test_thruster_manager_proportional_correct.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 unittest
19 import numpy as np
20 from uuv_thrusters import ThrusterManager
21 
22 PKG = 'uuv_thruster_manager'
23 roslib.load_manifest(PKG)
24 
25 rospy.init_node('test_thruster_manager_proportional_correct', anonymous=True)
26 
27 MANAGER = ThrusterManager()
28 
29 REFERENCE_TAM = np.array([
30  [1, 0, 0, 0, 0, 0],
31  [0.87758256, 0, -0.47942554, 0.47942554, 0.47942554, 0.87758256],
32  [0.87758256, 0.47942554, 0, -0.47942554, 0.87758256, -0.87758256]
33 ]).T
34 
35 class TestThrusterManagerProportionalCorrect(unittest.TestCase):
36  def test_initialization(self):
37  self.assertEqual(MANAGER.namespace, '/test_vehicle/')
38 
39  self.assertEqual(MANAGER.config['tf_prefix'], 'test_vehicle')
40  self.assertEqual(MANAGER.config['base_link'], 'base_link')
41  self.assertEqual(MANAGER.config['thruster_topic_prefix'], 'thrusters/')
42  self.assertEqual(MANAGER.config['thruster_frame_base'], 'thruster_')
43  self.assertEqual(MANAGER.config['thruster_topic_suffix'], '/input')
44  self.assertEqual(MANAGER.config['timeout'], -1)
45  self.assertEqual(MANAGER.config['max_thrust'], 1000.0)
46  self.assertEqual(MANAGER.config['update_rate'], 50)
47 
48  self.assertEqual(MANAGER.n_thrusters, 3)
49 
50  self.assertEqual(REFERENCE_TAM.shape, MANAGER.configuration_matrix.shape)
51 
52  self.assertTrue(np.isclose(REFERENCE_TAM, MANAGER.configuration_matrix).all())
53 
54  def test_thrusters(self):
55  self.assertEqual(len(MANAGER.thrusters), 3)
56 
57  for i in range(len(MANAGER.thrusters)):
58  self.assertEqual(MANAGER.thrusters[i].index, i)
59  self.assertEqual(MANAGER.thrusters[i].topic, 'thrusters/{}/input'.format(i))
60  self.assertEqual(MANAGER.thrusters[i].LABEL, 'proportional')
61  self.assertTrue(np.isclose(REFERENCE_TAM[:, i].flatten(), MANAGER.thrusters[i].tam_column).all())
62 
64  for _ in range(10):
65  gen_force = np.random.rand(6) * 100
66  thrust_forces = MANAGER.compute_thruster_forces(gen_force)
67 
68  ref_thrust_forces = np.linalg.pinv(REFERENCE_TAM).dot(gen_force)
69 
70  self.assertTrue(np.isclose(ref_thrust_forces, thrust_forces).all())
71 
72 if __name__ == '__main__':
73  import rosunit
74  rosunit.unitrun(PKG, 'test_thruster_manager_proportional_correct', TestThrusterManagerProportionalCorrect)


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