16 from __future__
import print_function
19 from uuv_control_msgs.srv
import InitHelicalTrajectory
21 from geometry_msgs.msg
import Point
22 from std_msgs.msg
import Time
24 if __name__ ==
'__main__':
25 print(
'Starting the helical trajectory creator')
26 rospy.init_node(
'start_circular_trajectory')
28 if rospy.is_shutdown():
29 print(
'ROS master not running!')
33 start_time = rospy.Time.now().to_sec()
35 if rospy.has_param(
'~start_time'):
36 start_time = rospy.get_param(
'~start_time')
38 print(
'Negative start time, setting it to 0.0')
44 param_labels = [
'radius',
'center',
'n_points',
'heading_offset',
45 'duration',
'n_turns',
'delta_z',
'max_forward_speed']
48 for label
in param_labels:
49 if not rospy.has_param(
'~' + label):
50 print(
'{} must be provided for the trajectory generation!'.format(label))
53 params[label] = rospy.get_param(
'~' + label)
55 if len(params[
'center']) != 3:
56 raise rospy.ROSException(
'Center of circle must have 3 components (x, y, z)')
58 if params[
'n_points'] <= 2:
59 raise rospy.ROSException(
'Number of points must be at least 2')
61 if params[
'max_forward_speed'] <= 0:
62 raise rospy.ROSException(
'Velocity limit must be positive')
65 rospy.wait_for_service(
'start_helical_trajectory', timeout=20)
66 except rospy.ROSException:
67 raise rospy.ROSException(
'Service not available! Closing node...')
70 traj_gen = rospy.ServiceProxy(
'start_helical_trajectory', InitHelicalTrajectory)
71 except rospy.ServiceException
as e:
72 raise rospy.ROSException(
'Service call failed, error={}'.format(e))
74 print(
'Generating trajectory that starts at t={} s'.format(start_time))
76 success =
traj_gen(Time(rospy.Time(start_time)),
79 Point(params[
'center'][0], params[
'center'][1], params[
'center'][2]),
83 params[
'heading_offset'] * pi / 180,
84 params[
'max_forward_speed'],
90 print(
'Trajectory successfully generated!')