diff --git a/src/turtle_test.py b/src/turtle_test.py new file mode 100755 index 0000000..e592326 --- /dev/null +++ b/src/turtle_test.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python +import rospy +from turtlesim.srv import * +from geometry_msgs.msg import PoseStamped + + +def callback(data): + teleport_turtle(data.pose.position.x/2+6,data.pose.position.y/2+6,0) + + + + +def turtle_follower(): + rospy.wait_for_service('turtle1/teleport_absolute') + rospy.Subscriber("mavros/local_position/pose", PoseStamped, callback) + rospy.spin() + +if __name__ == '__main__': + rospy.wait_for_service('turtle1/teleport_absolute') + try: + teleport_turtle = rospy.ServiceProxy('turtle1/teleport_absolute',TeleportAbsolute) + rospy.init_node('turtle_follower',anonymous=True) + + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + turtle_follower()