Ask Your Question

turtlesim tutorial "Go to Goal"

asked 2017-12-10 13:09:41 -0600

Brale gravatar image

Hi, recently, while i was learning on how to control turtle in turtlesim i stumbled upon a tutorial on ros wiki on how to make a code in python that moves turtle to a specific position. The code is following:

#!/usr/bin/env python
import rospy
from geometry_msgs.msg  import Twist
from turtlesim.msg import Pose
from math import pow,atan2,sqrt

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 = input("Set your x goal:")
    goal_pose.y = input("Set your y goal:")
    distance_tolerance = input("Set your tolerance:")
    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
    #Stopping our robot after the movement is over
    vel_msg.linear.x = 0
    vel_msg.angular.z =0


if __name__ == '__main__':
        #Testing our function
        x = turtlebot()

except rospy.ROSInterruptException: pass

My first question is about get_distance function, is this function pointless in this code? Its defined but its not called anywhere. I also think there is a bug in this code, it doesnt work properly for all given coordinates, for example i put x coordinate to be 1 and y coordinate to be 9 with 0.5 distance tolerance, but turtle, when it started approaching given coordinates, it started rapidly spinning out and bumping into the wall on the right side(you can run the code on your own to get a better vision of situation). Can someone figure out where is the problem?

edit retag flag offensive close merge delete


Hi, I have the same problem. Did you managed to find a solution to this issue ?

It's late but I think the function get_distance() was supposed to be use like this:

while self.get_distance() >= distance_tolerance:
        #Proportional Controller
        #linear velocity in the x-axis:
        vel_msg.linear.x = 1.5 * self.get_distance()

In my cas the robot never stop moving.

Shanika gravatar image Shanika  ( 2019-06-25 04:34:12 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2020-09-21 03:56:47 -0600

DiegoGuffanti gravatar image

Please see this link: Your code is out of date.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2017-12-10 13:09:41 -0600

Seen: 1,628 times

Last updated: Dec 10 '17