Hello Everyone,

I am trying to make my turtlebot go to different locations on my map (sequentialy), I followed the tutorial at learn.turtlebot.com for going to a specific point on the map and tried to tweek it a bit so that it goes to different pointS on the map. I publish the points that I want to reach (an array of floats) to a topic called "Goal_Pos" and each time I reach a position, I send a new position to the topic. My problem is once a position is reached, the subscriber doesn't get the new array being published right away, (it takes it about 40 second to actually get the new value that has been published) and after reaching the second goal, it gets lost and start spinning on itself.

Can anyone please help me? either by telling me what's wrong on my mess or what would be a better way to do it.

Thank you for you time

class GoToPose():
def __init__(self):
    rospy.init_node('nav_test', anonymous=False)
    self.sub = rospy.Subscriber("Goal_Pos", Float64MultiArray, self.callback)
    self.pub = rospy.Publisher("goal_achieved", Int32, queue_size=100)




def callback(self, ros_data):

    my_array= list(ros_data.data)
    # a condition to know that that we have already reached the final pose
    if(my_array[0]<-100):
        self.pub.publish(1)
        self.sub.unregister()
        rospy.loginfo("Final destination achieved")
        return
    #tell the action client that we want to spin a thread by default
    self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
    rospy.loginfo("wait for the action server to come up")
    #allow up to 5 seconds for the action server to come up
    self.move_base.wait_for_server(rospy.Duration(5))
    #we'll send a goal to the robot to tell it to move to a pose that's near the docking station
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'
    goal.target_pose.header.stamp = rospy.Time.now()
    #customize the following Point() values so they are appropriate for your location
    #following Point() values so they are appropriate for your location
    goal.target_pose.pose = Pose(Point(my_array[0], my_array[1], my_array[2]), Quaternion(my_array[3], my_array[4], my_array[5], my_array[6]))
    #start moving
    self.move_base.send_goal(goal)
    #allow TurtleBot up to 90 seconds to complete task
    success = self.move_base.wait_for_result(rospy.Duration(90))
    if success:
        state = self.move_base.get_state()
        if state == GoalStatus.SUCCEEDED:
            rospy.loginfo("Hooray, reached the desired pose")
    if not success:
                self.move_base.cancel_goal()
                rospy.loginfo("The base failed to reach the desired pose")
                self.pub.publish(0)
if __name__ == '__main__':
try:
    GoToPose()
except rospy.ROSInterruptException:
    rospy.loginfo("Exception thrown")

`