16 from __future__
import print_function
18 from uuv_gazebo_ros_plugins_msgs.srv
import SetThrusterEfficiency
21 if __name__ ==
'__main__':
22 print(
'Set the thruster output efficiency for vehicle, namespace=', rospy.get_namespace())
23 rospy.init_node(
'set_thrusters_states')
25 if rospy.is_shutdown():
26 raise rospy.ROSException(
'ROS master not running!')
29 if rospy.has_param(
'~starting_time'):
30 starting_time = rospy.get_param(
'~starting_time')
32 print(
'Starting time= {} s'.format(starting_time))
35 if rospy.has_param(
'~duration'):
36 duration = rospy.get_param(
'~duration')
39 raise rospy.ROSException(
'Duration not set, leaving node...')
41 print(
'Duration [s]=', (
'Inf.' if duration < 0
else duration))
43 if rospy.has_param(
'~efficiency'):
44 efficiency = rospy.get_param(
'~efficiency')
45 if efficiency < 0
or efficiency > 1:
46 raise rospy.ROSException(
'Invalid thruster output efficiency, leaving node...')
48 raise rospy.ROSException(
'Thruster output efficiency not set, leaving node...')
50 if rospy.has_param(
'~thruster_id'):
51 thruster_id = rospy.get_param(
'~thruster_id')
53 raise rospy.ROSException(
'Thruster ID not given')
56 raise rospy.ROSException(
'Invalid thruster ID')
58 print(
'Setting thruster output efficiency #{} to {}'.format(thruster_id, 100 * efficiency))
60 vehicle_name = rospy.get_namespace().replace(
'/',
'')
62 srv_name =
'/%s/thrusters/%d/set_thrust_force_efficiency' % (vehicle_name, thruster_id)
65 rospy.wait_for_service(srv_name, timeout=2)
66 except rospy.ROSException:
67 raise rospy.ROSException(
'Service not available! Closing node...')
70 set_eff = rospy.ServiceProxy(srv_name, SetThrusterEfficiency)
71 except rospy.ServiceException
as e:
72 raise rospy.ROSException(
'Service call failed, error=' + e)
74 rate = rospy.Rate(100)
75 while rospy.get_time() < starting_time:
81 print(
'Time={} s'.format(rospy.get_time()))
82 print(
'Current thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))
85 rate = rospy.Rate(100)
86 while rospy.get_time() < starting_time + duration:
92 print(
'Time={} s'.format(rospy.get_time()))
93 print(
'Returning to previous thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))
95 print(
'Leaving node...')