How to stop a turtle moving in circular path after reaching its initial position
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import sys
robot_x = 0 distance = 0
def pose_callback(pose): global robot_x
rospy.loginfo("Robot X = %f\n",pose.x)
robot_x = pose.x
def move_turtle(lin_vel,ang_vel):
global robot_x
rospy.init_node('move_turtle', anonymous=True)
pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
rospy.Subscriber('/turtle1/pose',Pose, pose_callback)
rate = rospy.Rate(10) # 10hz
vel = Twist()
while not rospy.is_shutdown():
vel.linear.x = lin_vel
vel.linear.y = 0
vel.linear.z = 0
vel.angular.x = 0
vel.angular.y = 0
vel.angular.z = ang_vel
rospy.loginfo("Linear Vel = %f: Angular Vel = %f",lin_vel,ang_vel)
global distance
distance = 2*(3.1415)*(lin_vel/ang_vel)
if(robot_x == distance):
rospy.loginfo("Robot Reached destination")
rospy.logwarn("Stopping robot")
break
pub.publish(vel)
rate.sleep()
if __name__ == '_main_': try: move_turtle(float(sys.argv[1]),float(sys.argv[2])) except rospy.ROSInterruptException: pass