19 from uuv_control_msgs.srv
import InitWaypointsFromFile
20 from std_msgs.msg
import String, Time
23 if __name__ ==
'__main__':
24 rospy.init_node(
'send_waypoint_file')
25 rospy.loginfo(
'Send a waypoint file, namespace=%s', rospy.get_namespace())
27 if rospy.is_shutdown():
28 rospy.logerr(
'ROS master not running!')
31 if rospy.has_param(
'~filename'):
32 filename = rospy.get_param(
'~filename')
34 raise rospy.ROSException(
'No filename found')
37 start_time = rospy.Time.now().to_sec()
39 if rospy.has_param(
'~start_time'):
40 start_time = rospy.get_param(
'~start_time')
42 rospy.logerr(
'Negative start time, setting it to 0.0')
50 rospy.loginfo(
'Start time=%.2f s' % start_time)
52 interpolator = rospy.get_param(
'~interpolator',
'lipb')
55 rospy.wait_for_service(
'init_waypoints_from_file', timeout=30)
56 except rospy.ROSException:
57 raise rospy.ROSException(
'Service not available! Closing node...')
60 init_wp = rospy.ServiceProxy(
61 'init_waypoints_from_file',
62 InitWaypointsFromFile)
63 except rospy.ServiceException
as e:
64 raise rospy.ROSException(
'Service call failed, error=%s', str(e))
66 success =
init_wp(Time(rospy.Time.from_sec(start_time)),
72 rospy.loginfo(
'Waypoints file successfully received, ' 73 'filename=%s', filename)
75 rospy.loginfo(
'Failed to send waypoints')