Turtle_sim go_to_goal loop, keeps going in circle

asked 2018-12-04 07:35:38 -0500

hadi20107 gravatar image

updated 2018-12-04 07:36:20 -0500

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

C:\fakepath\Screenshot from 2018-12-04 13-30-53.pngimage description

edit retag flag offensive close merge delete