Added circle_tests.py

This commit is contained in:
Matas Keras 2022-07-13 10:31:19 -04:00
parent 76ce88cc4e
commit 777a6114f7
1 changed files with 26 additions and 0 deletions

26
src/turtle_test.py Executable file
View File

@ -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()