|
|
@@ -0,0 +1,139 @@
|
|
|
+#!/usr/bin/env python
|
|
|
+# -*- encoding: utf-8 -*-
|
|
|
+# Example of script to send a trajectory to a robot, start and stop motion.
|
|
|
+
|
|
|
+import time
|
|
|
+
|
|
|
+from geometry_msgs.msg import Pose
|
|
|
+from robot_coordination.msg import Waypoint
|
|
|
+from robot_coordination.srv import AddPath
|
|
|
+from robot_coordination.srv import StartMovement
|
|
|
+from robot_coordination.srv import StopMovement
|
|
|
+import rospy
|
|
|
+import random
|
|
|
+import sys
|
|
|
+
|
|
|
+# Trajectory as a tuple of (x, y, time)
|
|
|
+g_trajectory = (
|
|
|
+# (1.8, 1.6, 12),
|
|
|
+# (4.8, 3.6, 10),
|
|
|
+ (5.6, 1.9, 10),
|
|
|
+ (3.8, 2.6, 10)
|
|
|
+)
|
|
|
+
|
|
|
+
|
|
|
+def wait_for_service(server_ns, srv_name):
|
|
|
+ while not rospy.is_shutdown():
|
|
|
+ try:
|
|
|
+ rospy.wait_for_service(server_ns + '/' + srv_name, 5)
|
|
|
+ return True
|
|
|
+ except rospy.ROSException:
|
|
|
+ rospy.logwarn('Could not connect to service {}/{}, trying again'.format(
|
|
|
+ server_ns, srv_name))
|
|
|
+ except (rospy.ROSInterruptException, KeyboardInterrupt):
|
|
|
+ return False
|
|
|
+
|
|
|
+
|
|
|
+def start_movement(server_ns):
|
|
|
+ """Start the robot motion by calling the service 'start_movement'"""
|
|
|
+ srv_name = 'start_movement'
|
|
|
+ if not wait_for_service(server_ns, srv_name):
|
|
|
+ return False
|
|
|
+ try:
|
|
|
+ start_movement_srv = rospy.ServiceProxy(server_ns + '/' + srv_name, StartMovement)
|
|
|
+ reply = start_movement_srv()
|
|
|
+ return reply.ack
|
|
|
+ except rospy.ServiceException as e:
|
|
|
+ rospy.logerr('Service call failed: {}'.format(e))
|
|
|
+ return False
|
|
|
+
|
|
|
+
|
|
|
+def stop_movement(server_ns):
|
|
|
+ """Stop the robot motion by calling the service 'stop_movement'"""
|
|
|
+ srv_name = 'stop_movement'
|
|
|
+ if not wait_for_service(server_ns, srv_name):
|
|
|
+ return False
|
|
|
+ try:
|
|
|
+ stop_movement_srv = rospy.ServiceProxy(server_ns + '/' + srv_name, StopMovement)
|
|
|
+ reply = stop_movement_srv()
|
|
|
+ return reply.ack
|
|
|
+ except rospy.ServiceException as e:
|
|
|
+ rospy.logerr('Service call failed: {}'.format(e))
|
|
|
+ return False
|
|
|
+
|
|
|
+
|
|
|
+def waypoint_list_from_tuple(traj):
|
|
|
+ """Return a list of Waypoint instances from a list of (x, y, timepoint)"""
|
|
|
+ waypoint_list = []
|
|
|
+ for x, y, timepoint in traj:
|
|
|
+ #for i in range(1, 5):
|
|
|
+ waypoint = Waypoint()
|
|
|
+ waypoint.pose = Pose()
|
|
|
+ #waypoint.pose.position.x = random.uniform(0.8, 6.0)
|
|
|
+ #waypoint.pose.position.y = random.uniform(0.8, 3.0)
|
|
|
+ #waypoint.timepoint = rospy.Duration.from_sec(10)
|
|
|
+ waypoint.pose.position.x = x
|
|
|
+ waypoint.pose.position.y = y
|
|
|
+ waypoint.timepoint = rospy.Duration.from_sec(timepoint)
|
|
|
+ waypoint_list.append(waypoint)
|
|
|
+ return waypoint_list
|
|
|
+
|
|
|
+
|
|
|
+def add_path(server_ns, trajectory):
|
|
|
+ """Add a path with data from trajectory"""
|
|
|
+ waypoint_list = waypoint_list_from_tuple(trajectory)
|
|
|
+ srv_name = 'add_path'
|
|
|
+ if not wait_for_service(server_ns, srv_name):
|
|
|
+ return False
|
|
|
+ try:
|
|
|
+ add_path_srv = rospy.ServiceProxy(server_ns + '/' + srv_name, AddPath)
|
|
|
+ reply = add_path_srv(waypoint_list)
|
|
|
+ return reply.result
|
|
|
+ except rospy.ServiceException as e:
|
|
|
+ rospy.logerr('Service call failed: {}'.format(e))
|
|
|
+ return False
|
|
|
+
|
|
|
+
|
|
|
+def main():
|
|
|
+ rospy.init_node('example_robot_control')
|
|
|
+ server_ns_parameter_name = '~server_namespace'
|
|
|
+ try:
|
|
|
+ server_ns = rospy.get_param(server_ns_parameter_name)
|
|
|
+ except KeyError:
|
|
|
+ rospy.logerr('Parameter {} must be set'.format(
|
|
|
+ rospy.resolve_name(server_ns_parameter_name)))
|
|
|
+ return
|
|
|
+
|
|
|
+ if not add_path(server_ns, g_trajectory):
|
|
|
+ rospy.logerr('Could not add path, exiting')
|
|
|
+ return
|
|
|
+ rospy.loginfo('Path added')
|
|
|
+
|
|
|
+ if not start_movement(server_ns):
|
|
|
+ rospy.logerr('Could not start motion, exiting')
|
|
|
+ return
|
|
|
+ rospy.loginfo('Movement started')
|
|
|
+
|
|
|
+ while not rospy.is_shutdown():
|
|
|
+ time.sleep(10)
|
|
|
+ if not add_path(server_ns, g_trajectory):
|
|
|
+ rospy.logerr('Could not add path, exiting')
|
|
|
+ return
|
|
|
+ rospy.loginfo('Path added')
|
|
|
+
|
|
|
+ if not stop_movement(server_ns):
|
|
|
+ rospy.logerr('Could not stop motion, exiting')
|
|
|
+ return
|
|
|
+ rospy.loginfo('Movement stopped')
|
|
|
+
|
|
|
+ if not add_path(server_ns, []):
|
|
|
+ rospy.logerr('Could not clear path, exiting')
|
|
|
+ return
|
|
|
+ rospy.loginfo('Path cleared')
|
|
|
+
|
|
|
+
|
|
|
+if __name__ == '__main__':
|
|
|
+ try:
|
|
|
+ main()
|
|
|
+ except rospy.ROSInterruptException:
|
|
|
+ pass
|
