Turtle_sim go_to_goal loop, keeps going in circle
Hi I would like to tell the turtle sim to go to random coordinates from 0 to 10 in loop. For instance go to x,y then pause for a few seconds then go to a different x and y. I tried changing the script go_to_goal but it keeps going in circle. Any suggestions? Thanks in advance
class turtlebot():
def __init__(self): #Creating our node,publisher and subscriber rospy.init_node('turtlebot_controller', anonymous=True) self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, self.callback) self.pose = Pose() self.rate = rospy.Rate(10) #Callback function implementing the pose value received def callback(self, data): self.pose = data self.pose.x = round(self.pose.x, 4) self.pose.y = round(self.pose.y, 4) def get_distance(self, goal_x, goal_y): distance = sqrt(pow((goal_x - self.pose.x), 2) + pow((goal_y - self.pose.y), 2)) return distance def move2goal(self): goal_pose = Pose() goal_pose.x = randint(1, 10) goal_pose.y = randint(1, 10) distance_tolerance = 0.1 vel_msg = Twist() while sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2)) >= distance_tolerance: #Porportional Controller #linear velocity in the x-axis: vel_msg.linear.x = 1.5 * sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2)) vel_msg.linear.y = 0 vel_msg.linear.z = 0 #angular velocity in the z-axis: vel_msg.angular.x = 0 vel_msg.angular.y = 0 vel_msg.angular.z = 4 * (atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.theta) #Publishing our vel_msg self.velocity_publisher.publish(vel_msg) self.rate.sleep() time.sleep(2) goal_pose.x = randint(1, 10) goal_pose.y = randint(1, 10) rospy.loginfo("stopping") # Stopping our robot after the movement is over vel_msg.linear.x = 0 vel_msg.angular.z = 0 self.velocity_publisher.publish(vel_msg) rospy.loginfo("x: " + str(goal_pose.x) + "y: " + str(goal_pose.y)) self.pose.x = goal_pose.x self.pose.y = goal_pose.y rospy.spin()
if __name__ == '__main__': try: #Testing our function x = turtlebot() x.move2goal()
except rospy.ROSInterruptException: pass