test_thrusters.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 import rospy
17 import unittest
18 import sys
19 from time import sleep
20 from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
21 from uuv_gazebo_ros_plugins_msgs.srv import GetThrusterConversionFcn, \
22  SetThrusterState, GetThrusterState, SetThrusterEfficiency, \
23  GetThrusterEfficiency
24 
25 PKG = 'uuv_gazebo_ros_plugins'
26 NAME = 'test_thrusters'
27 
28 import roslib; roslib.load_manifest(PKG)
29 
30 
31 class TestThrusters(unittest.TestCase):
32  def __init__(self, *args):
33  super(TestThrusters, self).__init__(*args)
34  rospy.init_node('test_thrusters', anonymous=True)
35 
36  self.thruster_input_pub = dict()
37  for i in range(3):
38  self.thruster_input_pub[i] = rospy.Publisher(
39  '/vehicle/thrusters/%d/input' % i, FloatStamped, queue_size=1)
40 
42  pub = rospy.Publisher('/vehicle/thrusters/0/input', FloatStamped,
43  queue_size=1)
44 
45  for k in self.thruster_input_pub:
46  # Publishing set point to rotor velocity
47  input_message = FloatStamped()
48  input_message.header.stamp = rospy.Time.now()
49  input_message.data = 0.2
50 
51  self.thruster_input_pub[k].publish(input_message)
52  sleep(1)
53 
54  output = rospy.wait_for_message('/vehicle/thrusters/%d/thrust' % k,
55  FloatStamped, timeout=30)
56  self.assertIsNot(output.data, 0.0)
57 
58  # Turning thruster off
59  input_message.data = 0.0
60  pub.publish(input_message)
61 
63  # Testing thruster #0 - basic/proportional model
64  rospy.wait_for_service(
65  '/vehicle/thrusters/0/get_thruster_conversion_fcn')
66 
67  get_thruster_convertion_fcn = rospy.ServiceProxy(
68  '/vehicle/thrusters/0/get_thruster_conversion_fcn',
69  GetThrusterConversionFcn)
70 
71  fcn = get_thruster_convertion_fcn()
72 
73  self.assertEqual(fcn.fcn.function_name, 'Basic')
74  self.assertEqual(len(fcn.fcn.tags), len(fcn.fcn.data))
75  self.assertEqual(len(fcn.fcn.tags), 1)
76  self.assertIn('rotor_constant', fcn.fcn.tags)
77  self.assertEqual(fcn.fcn.data[0], 0.001)
78 
79  # Testing thruster #1 - Bessa/nonlinear model
80  rospy.wait_for_service(
81  '/vehicle/thrusters/1/get_thruster_conversion_fcn')
82 
83  get_thruster_convertion_fcn = rospy.ServiceProxy(
84  '/vehicle/thrusters/1/get_thruster_conversion_fcn',
85  GetThrusterConversionFcn)
86 
87  fcn = get_thruster_convertion_fcn()
88 
89  bessa_tags = ['rotor_constant_l', 'rotor_constant_r', 'delta_l',
90  'delta_r']
91  bessa_params = [0.001, 0.001, -0.01, 0.01]
92  self.assertEqual(fcn.fcn.function_name, 'Bessa')
93  self.assertEqual(len(fcn.fcn.tags), len(fcn.fcn.data))
94  self.assertEqual(len(fcn.fcn.tags), 4)
95  for t, p in zip(fcn.fcn.tags, fcn.fcn.data):
96  self.assertIn(t, bessa_tags)
97  self.assertEqual(p, bessa_params[bessa_tags.index(t)])
98 
99  # Testing thruster #2 - Linear interpolation
100  rospy.wait_for_service(
101  '/vehicle/thrusters/2/get_thruster_conversion_fcn')
102 
103  get_thruster_convertion_fcn = rospy.ServiceProxy(
104  '/vehicle/thrusters/2/get_thruster_conversion_fcn',
105  GetThrusterConversionFcn)
106 
107  fcn = get_thruster_convertion_fcn()
108 
109  self.assertEqual(fcn.fcn.function_name, 'LinearInterp')
110  self.assertEqual(len(fcn.fcn.tags), len(fcn.fcn.data))
111  self.assertEqual(len(fcn.fcn.tags), 0)
112  self.assertEqual(len(fcn.fcn.lookup_table_input),
113  len(fcn.fcn.lookup_table_output))
114  self.assertListEqual([-0.1, 0.0, 0.1],
115  list(fcn.fcn.lookup_table_input))
116  self.assertListEqual([-0.01, 0.0, 0.01],
117  list(fcn.fcn.lookup_table_output))
118 
120  for i in range(3):
121  rospy.wait_for_service(
122  '/vehicle/thrusters/%d/set_thruster_state' % i)
123  set_state = rospy.ServiceProxy(
124  '/vehicle/thrusters/%d/set_thruster_state' % i,
125  SetThrusterState)
126  self.assertTrue(set_state(False).success)
127 
128  # Test that thruster is off
129  rospy.wait_for_service(
130  '/vehicle/thrusters/%d/get_thruster_state' % i)
131  get_state = rospy.ServiceProxy(
132  '/vehicle/thrusters/%d/get_thruster_state' % i,
133  GetThrusterState)
134 
135  self.assertFalse(get_state().is_on)
136 
137  # Turn thruster on again
138  self.assertTrue(set_state(True).success)
139  self.assertTrue(get_state().is_on)
140 
142  for i in range(3):
143  rospy.wait_for_service(
144  '/vehicle/thrusters/%d/set_thrust_force_efficiency' % i)
145  set_efficiency = rospy.ServiceProxy(
146  '/vehicle/thrusters/%d/set_thrust_force_efficiency' % i,
147  SetThrusterEfficiency)
148  self.assertTrue(set_efficiency(0.5).success)
149 
150  # Test that thruster is off
151  rospy.wait_for_service(
152  '/vehicle/thrusters/%d/get_thrust_force_efficiency' % i)
153  get_efficiency = rospy.ServiceProxy(
154  '/vehicle/thrusters/%d/get_thrust_force_efficiency' % i,
155  GetThrusterEfficiency)
156 
157  self.assertEqual(get_efficiency().efficiency, 0.5)
158 
159  # Turn thruster on again
160  self.assertTrue(set_efficiency(1.0).success)
161  self.assertEqual(get_efficiency().efficiency, 1.0)
162 
164  for i in range(3):
165  rospy.wait_for_service(
166  '/vehicle/thrusters/%d/set_dynamic_state_efficiency' % i)
167  set_efficiency = rospy.ServiceProxy(
168  '/vehicle/thrusters/%d/set_dynamic_state_efficiency' % i,
169  SetThrusterEfficiency)
170  self.assertTrue(set_efficiency(0.5).success)
171 
172  # Test that thruster is off
173  rospy.wait_for_service(
174  '/vehicle/thrusters/%d/get_dynamic_state_efficiency' % i)
175  get_efficiency = rospy.ServiceProxy(
176  '/vehicle/thrusters/%d/get_dynamic_state_efficiency' % i,
177  GetThrusterEfficiency)
178 
179  self.assertEqual(get_efficiency().efficiency, 0.5)
180 
181  # Turn thruster on again
182  self.assertTrue(set_efficiency(1.0).success)
183  self.assertEqual(get_efficiency().efficiency, 1.0)
184 
185 
186 if __name__ == '__main__':
187  import rostest
188  rostest.rosrun(PKG, NAME, TestThrusters, sys.argv)


uuv_gazebo_ros_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Mon Jul 1 2019 19:39:15