16 from __future__
import print_function
19 from uuv_control_msgs.srv
import InitCircularTrajectory
21 from geometry_msgs.msg
import Point
22 from std_msgs.msg
import Time
25 if __name__ ==
'__main__':
26 print(
'Starting the circular trajectory creator')
27 rospy.init_node(
'start_circular_trajectory')
29 if rospy.is_shutdown():
30 print(
'ROS master not running!')
34 start_time = rospy.Time.now().to_sec()
36 if rospy.has_param(
'~start_time'):
37 start_time = rospy.get_param(
'~start_time')
39 print(
'Negative start time, setting it to 0.0')
45 param_labels = [
'radius',
'center',
'n_points',
'heading_offset',
46 'duration',
'max_forward_speed']
49 for label
in param_labels:
50 if not rospy.has_param(
'~' + label):
51 print(
'{} must be provided for the trajectory generation!'.format(label))
54 params[label] = rospy.get_param(
'~' + label)
56 if len(params[
'center']) != 3:
57 print(
'Center of circle must have 3 components (x, y, z)')
60 if params[
'n_points'] <= 2:
61 print(
'Number of points must be at least 2')
64 if params[
'max_forward_speed'] <= 0:
65 print(
'Velocity limit must be positive')
69 rospy.wait_for_service(
'start_circular_trajectory', timeout=20)
70 except rospy.ROSException:
71 print(
'Service not available! Closing node...')
75 traj_gen = rospy.ServiceProxy(
'start_circular_trajectory', InitCircularTrajectory)
76 except rospy.ServiceException
as e:
77 print(
'Service call failed, error={}'.format(e))
80 print(
'Generating trajectory that starts at t={} s'.format(start_time))
82 success =
traj_gen(Time(rospy.Time(start_time)),
85 Point(params[
'center'][0], params[
'center'][1], params[
'center'][2]),
89 params[
'heading_offset'] * pi / 180,
90 params[
'max_forward_speed'],
94 print(
'Trajectory successfully generated!')