16 from __future__
import print_function
18 from uuv_gazebo_ros_plugins_msgs.srv
import SetThrusterState
21 if __name__ ==
'__main__':
22 print(
'Set the state of thrusters for vehicle, namespace=', rospy.get_namespace())
23 rospy.init_node(
'set_thrusters_states')
25 if rospy.is_shutdown():
26 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(
'~is_on'):
44 is_on = bool(rospy.get_param(
'~is_on'))
46 raise rospy.ROSException(
'State flag not provided')
48 if rospy.has_param(
'~thruster_id'):
49 thruster_id = rospy.get_param(
'~thruster_id')
51 raise rospy.ROSException(
'Thruster ID not given')
54 raise rospy.ROSException(
'Invalid thruster ID')
56 print(
'Setting state of thruster #{} as {}'.format(thruster_id,
'ON' if is_on
else 'OFF'))
58 vehicle_name = rospy.get_namespace().replace(
'/',
'')
60 srv_name =
'/%s/thrusters/%d/set_thruster_state' % (vehicle_name, thruster_id)
63 rospy.wait_for_service(srv_name, timeout=2)
64 except rospy.ROSException:
65 raise rospy.ROSException(
'Service not available! Closing node...')
68 set_state = rospy.ServiceProxy(srv_name, SetThrusterState)
69 except rospy.ServiceException
as e:
70 raise rospy.ROSException(
'Service call failed, error=' + e)
72 rate = rospy.Rate(100)
73 while rospy.get_time() < starting_time:
79 print(
'Time={} s'.format(rospy.get_time()))
80 print(
'Current state of thruster #{}={}'.format(thruster_id,
'ON' if is_on
else 'OFF'))
83 rate = rospy.Rate(100)
84 while rospy.get_time() < starting_time + duration:
90 print(
'Time={} s'.format(rospy.get_time()))
91 print(
'Returning to previous state of thruster #{}={}'.format(thruster_id,
'ON' if not is_on
else 'OFF'))
93 print(
'Leaving node...')