16 from __future__
import print_function
22 if __name__ ==
'__main__':
23 print(
'Starting current perturbation node')
24 rospy.init_node(
'set_timed_current_perturbation')
26 print(
'Programming the generation of a current perturbation')
27 if rospy.is_shutdown():
28 print(
'ROS master not running!')
32 if rospy.has_param(
'~starting_time'):
33 starting_time = rospy.get_param(
'~starting_time')
34 if starting_time < 0.0:
35 print(
'Negative starting time, setting it to 0.0')
38 print(
'Starting time=', starting_time)
41 if rospy.has_param(
'~end_time'):
42 end_time = rospy.get_param(
'~end_time')
43 if end_time != -1
and end_time <= starting_time:
44 raise rospy.ROSException(
'End time is smaller than the starting time')
46 print(
'End time=', (end_time
if end_time != -1
else 'Inf.'))
49 if rospy.has_param(
'~current_velocity'):
50 vel = rospy.get_param(
'~current_velocity')
52 print(
'Current velocity [m/s]=', vel)
55 if rospy.has_param(
'~horizontal_angle'):
56 horz_angle = rospy.get_param(
'~horizontal_angle')
57 horz_angle *= pi / 180
59 print(
'Current horizontal angle [deg]=', horz_angle * 180 / pi)
62 if rospy.has_param(
'~vertical_angle'):
63 vert_angle = rospy.get_param(
'~vertical_angle')
64 vert_angle *= pi / 180
66 print(
'Current vertical angle [deg]=', horz_angle * 180 / pi)
69 rospy.wait_for_service(
'/hydrodynamics/set_current_velocity', timeout=20)
70 except rospy.ROSException:
71 print(
'Current velocity services not available! Closing node...')
75 set_current = rospy.ServiceProxy(
'/hydrodynamics/set_current_velocity', SetCurrentVelocity)
76 except rospy.ServiceException
as e:
77 print(
'Service call failed, error=', e)
81 rate = rospy.Rate(100)
82 if rospy.get_time() < starting_time:
83 while rospy.get_time() < starting_time:
86 print(
'Applying current model...')
88 print(
'Current velocity changed successfully at %f s! vel= %f m/s' % (rospy.get_time(), vel))
90 print(
'Failed to change current velocity')
93 while rospy.get_time() < end_time:
96 print(
'TIMEOUT, setting current velocity to zero...')
99 print(
'Leaving node...')