test_default_fossen_vehicle.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Copyright (c) 2016 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 from __future__ import print_function
17 import rospy
18 import rostest
19 import unittest
20 from geometry_msgs.msg import Vector3, Inertia
21 from uuv_gazebo_ros_plugins_msgs.msg import UnderwaterObjectModel
23 
24 PKG = 'uuv_gazebo_ros_plugins'
25 NAME = 'test_default_fossen_vehicle'
26 
27 import roslib; roslib.load_manifest(PKG)
28 
29 
30 class TestDefaultFossenVehicle(unittest.TestCase):
31  # def tearDownClass(self):
32  # FIXME Temporary solution to avoid gzserver lingering after the
33  # simulation node is killed (Gazebo 9.1)
34  # os.system('killall -9 gzserver')
35 
37  rospy.wait_for_service('/vehicle/get_model_properties')
38 
39  s = rospy.ServiceProxy('/vehicle/get_model_properties',
40  GetModelProperties)
41  models = s()
42 
43  self.assertEqual(len(models.link_names), 1)
44  self.assertEqual(len(models.models), 1)
45 
46  # Test the name of the link
47  self.assertEqual(
48  models.link_names[0], 'vehicle/base_link',
49  'Link name is invalid, name=' + str(models.link_names[0]))
50 
51  # Test message types
52  self.assertIsInstance(models.models[0].added_mass, tuple)
53  self.assertIsInstance(models.models[0].linear_damping, tuple)
54  self.assertIsInstance(models.models[0].linear_damping_forward_speed, tuple)
55  self.assertIsInstance(models.models[0].quadratic_damping, tuple)
56  self.assertIsInstance(models.models[0].volume, float)
57  self.assertIsInstance(models.models[0].bbox_length, float)
58  self.assertIsInstance(models.models[0].bbox_width, float)
59  self.assertIsInstance(models.models[0].bbox_height, float)
60  self.assertIsInstance(models.models[0].fluid_density, float)
61  self.assertIsInstance(models.models[0].neutrally_buoyant, bool)
62  self.assertIsInstance(models.models[0].cob, Vector3)
63  self.assertIsInstance(models.models[0].inertia, Inertia)
64 
65  # Test size of the parameter lists
66  # Generate index numbers for the diagonal elements of the matrices
67  d_idxs = [i*6 + j for i, j in zip(range(6), range(6))]
68  self.assertEqual(len(models.models[0].added_mass), 36)
69  for i in range(len(models.models[0].added_mass)):
70  if i in d_idxs:
71  self.assertEqual(models.models[0].added_mass[i], 1.0)
72  else:
73  self.assertEqual(models.models[0].added_mass[i], 0.0)
74 
75  self.assertEqual(len(models.models[0].linear_damping), 36)
76  for i in range(len(models.models[0].linear_damping)):
77  if i in d_idxs:
78  self.assertEqual(models.models[0].linear_damping[i], 1.0)
79  else:
80  self.assertEqual(models.models[0].linear_damping[i], 0.0)
81 
82  self.assertEqual(len(models.models[0].linear_damping_forward_speed), 36)
83  for i in range(len(models.models[0].linear_damping_forward_speed)):
84  if i in d_idxs:
85  self.assertEqual(models.models[0].linear_damping_forward_speed[i], 1.0)
86  else:
87  self.assertEqual(models.models[0].linear_damping_forward_speed[i], 0.0)
88 
89  self.assertEqual(len(models.models[0].quadratic_damping), 36)
90  for i in range(len(models.models[0].quadratic_damping)):
91  if i in d_idxs:
92  self.assertEqual(models.models[0].quadratic_damping[i], 1.0)
93  else:
94  self.assertEqual(models.models[0].quadratic_damping[i], 0.0)
95 
96  # Tests if some of the parameters match to the ones given in the URDF
97  self.assertEqual(models.models[0].fluid_density, 1028.0)
98  self.assertEqual(models.models[0].volume, 1.0)
99  self.assertEqual(models.models[0].bbox_height, 1.0)
100  self.assertEqual(models.models[0].bbox_length, 1.0)
101  self.assertEqual(models.models[0].bbox_width, 1.0)
102 
104  rospy.wait_for_service('/vehicle/set_fluid_density')
105  set_func = rospy.ServiceProxy('/vehicle/set_fluid_density', SetFloat)
106 
107  rospy.wait_for_service('/vehicle/get_fluid_density')
108  get_func = rospy.ServiceProxy('/vehicle/get_fluid_density', GetFloat)
109 
110  self.assertEqual(get_func().data, 1028.0)
111  self.assertTrue(set_func(1025.0).success)
112  self.assertEqual(get_func().data, 1025.0)
113  self.assertTrue(set_func(1028.0).success)
114 
116  rospy.wait_for_service('/vehicle/set_volume_offset')
117  set_func = rospy.ServiceProxy('/vehicle/set_volume_offset', SetFloat)
118 
119  rospy.wait_for_service('/vehicle/get_volume_offset')
120  get_func = rospy.ServiceProxy('/vehicle/get_volume_offset', GetFloat)
121 
122  rospy.wait_for_service('/vehicle/get_model_properties')
123  get_model = rospy.ServiceProxy('/vehicle/get_model_properties',
124  GetModelProperties)
125 
126  # Test that offset has changed
127  self.assertEqual(get_func().data, 0.0)
128  self.assertTrue(set_func(1.0).success)
129  self.assertEqual(get_func().data, 1.0)
130 
131  # Test that the actual volume has NOT changed, offset should only
132  # be used during the computation of forces
133  self.assertEqual(get_model().models[0].volume, 1.0)
134 
135  self.assertTrue(set_func(0.0).success)
136 
138  rospy.wait_for_service('/vehicle/set_added_mass_scaling')
139  set_func = rospy.ServiceProxy('/vehicle/set_added_mass_scaling', SetFloat)
140 
141  rospy.wait_for_service('/vehicle/get_added_mass_scaling')
142  get_func = rospy.ServiceProxy('/vehicle/get_added_mass_scaling', GetFloat)
143 
144  self.assertEqual(get_func().data, 1.0)
145  self.assertTrue(set_func(0.8).success)
146  self.assertEqual(get_func().data, 0.8)
147  self.assertTrue(set_func(1.0).success)
148 
150  rospy.wait_for_service('/vehicle/set_damping_scaling')
151  set_func = rospy.ServiceProxy('/vehicle/set_damping_scaling', SetFloat)
152 
153  rospy.wait_for_service('/vehicle/get_damping_scaling')
154  get_func = rospy.ServiceProxy('/vehicle/get_damping_scaling', GetFloat)
155 
156  self.assertEqual(get_func().data, 1.0)
157  self.assertTrue(set_func(0.8).success)
158  self.assertEqual(get_func().data, 0.8)
159  self.assertTrue(set_func(1.0).success)
160 
162  rospy.wait_for_service('/vehicle/set_volume_scaling')
163  set_func = rospy.ServiceProxy('/vehicle/set_volume_scaling', SetFloat)
164 
165  rospy.wait_for_service('/vehicle/get_volume_scaling')
166  get_func = rospy.ServiceProxy('/vehicle/get_volume_scaling', GetFloat)
167 
168  self.assertEqual(get_func().data, 1.0)
169  self.assertTrue(set_func(0.8).success)
170  self.assertEqual(get_func().data, 0.8)
171  self.assertTrue(set_func(1.0).success)
172 
174  rospy.wait_for_service('/vehicle/set_added_mass_offset')
175  set_func = rospy.ServiceProxy('/vehicle/set_added_mass_offset', SetFloat)
176 
177  rospy.wait_for_service('/vehicle/get_added_mass_offset')
178  get_func = rospy.ServiceProxy('/vehicle/get_added_mass_offset', GetFloat)
179 
180  self.assertEqual(get_func().data, 0.0)
181 
182  self.assertTrue(set_func(1.0).success)
183  self.assertEqual(get_func().data, 1.0)
184  self.assertTrue(set_func(0.0).success)
185 
187  rospy.wait_for_service('/vehicle/set_linear_damping_offset')
188  set_func = rospy.ServiceProxy('/vehicle/set_linear_damping_offset', SetFloat)
189 
190  rospy.wait_for_service('/vehicle/get_linear_damping_offset')
191  get_func = rospy.ServiceProxy('/vehicle/get_linear_damping_offset', GetFloat)
192 
193  self.assertEqual(get_func().data, 0.0)
194  self.assertTrue(set_func(1.0).success)
195  self.assertEqual(get_func().data, 1.0)
196  self.assertTrue(set_func(0.0).success)
197 
199  rospy.wait_for_service('/vehicle/set_linear_forward_speed_damping_offset')
200  set_func = rospy.ServiceProxy('/vehicle/set_linear_forward_speed_damping_offset', SetFloat)
201 
202  rospy.wait_for_service('/vehicle/get_linear_forward_speed_damping_offset')
203  get_func = rospy.ServiceProxy('/vehicle/get_linear_forward_speed_damping_offset', GetFloat)
204 
205  self.assertEqual(get_func().data, 0.0)
206  self.assertTrue(set_func(1.0).success)
207  self.assertEqual(get_func().data, 1.0)
208  self.assertTrue(set_func(0.0).success)
209 
211  rospy.wait_for_service('/vehicle/set_nonlinear_damping_offset')
212  set_func = rospy.ServiceProxy('/vehicle/set_nonlinear_damping_offset', SetFloat)
213 
214  rospy.wait_for_service('/vehicle/get_nonlinear_damping_offset')
215  get_func = rospy.ServiceProxy('/vehicle/get_nonlinear_damping_offset', GetFloat)
216 
217  self.assertEqual(get_func().data, 0.0)
218  self.assertTrue(set_func(1.0).success)
219  self.assertEqual(get_func().data, 1.0)
220  self.assertTrue(set_func(0.0).success)
221 
222 
223 if __name__ == '__main__':
224  import rosunit
225  rosunit.unitrun(PKG, NAME, TestDefaultFossenVehicle)


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