Unpredictable Behavior with Goal Oriented turtlesim script

edit

Hi,

I wrote a short script in python to move the turtle to 4 different goal positions, similar to the "go to goal" code found here: http://wiki.ros.org/turtlesim/Tutoria...

When the turtle has only 1 goal (comment out the last 3 goals), the code works fine. However, with 2 or more consecutive goals, the turtle moves sporadically and never makes it to any of the goal poses. Instead, the turtle will endlessly try to move off the turtlesim window. Here is the code I wrote:

class TurtleBot(object):

def __init__(self):
    rospy.init_node('turtlebot', anonymous=True)

    self.k_linear = 2
    self.k_angular = 3.0
    self.dist_thresh = 0.5

    self.pose = Pose()
    self.rate = rospy.Rate(10)

    self.turtle_sub = rospy.Subscriber("/turtle1/pose", Pose, self.callback)
    self.vel_pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)

def callback(self, data):
    self.pose = data


def moveToGoal(self, goal_pose):
    while self.dist(goal_pose) >= self.dist_thresh:
        vel_msg = Twist()
        #linear velocity
        vel_msg.linear.x = self.k_linear * self.dist(goal_pose)
        vel_msg.linear.y = 0
        vel_msg.linear.z = 0
        #angular velocity
        vel_msg.angular.x = 0
        vel_msg.angular.y = 0
        vel_msg.angular.z = self.k_angular * (atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.theta)
        self.vel_pub.publish(vel_msg)
        print(self.dist(goal_pose))
        self.rate.sleep()
    vel_msg.linear.x = 0
    vel_msg.angular.z = 0
    self.vel_pub.publish(vel_msg)


def dist(self, goal_pose):
    #distance = np.sqrt( (goal_pose.x - self.pose.x)**2 + (goal_pose.y - self.pose.y)**2 )
    distance = sqrt( pow(goal_pose.x - self.pose.x, 2) + pow(goal_pose.y - self.pose.y, 2) )
    return distance

def newGoal(self, x_pose, y_pose):
    goal = Pose()
    goal.x = x_pose
    goal.y = y_pose
    return goal

def main(args):
turtle1 = TurtleBot()

#random goal x,y positions
goal1 = turtle1.newGoal(7, 7)
goal2 = turtle1.newGoal(3, 7)
goal3 = turtle1.newGoal(2, 3)
goal4 = turtle1.newGoal(7, 2)

turtle1.moveToGoal(goal1)
turtle1.moveToGoal(goal2)
turtle1.moveToGoal(goal3)
turtle1.moveToGoal(goal4)

try:
    rospy.spin()
except KeyboardInterrupt:
    print("Shutting down")


if __name__=='__main__':
    main(sys.argv)

The underlying math is the same as the 'go to goal' code referenced above (or at least, it should be). I'm not sure what is causing the poor behavior. Any help is appreciated!

Misc. Info: Running on Ubuntu 16.04 VM, ROS Kinetic, using ROS turtlesim package