|Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.|
Go to GoalDescription: This tutorial is based on Turtlesim Video Tutorials
Tutorial Level: INTERMEDIATE
You can find the complete package at: https://github.com/clebercoutof/turtlesim_cleaner
Now we are going to move the turtle to a specified location.
Preparing for work
Let's create our file gotogoal.py (or any name you want) and paste it in the source directory of our package: if you followed the past tutorial, it will be: ~/catkin_ws/src/turtlesim_cleaner/src. Then, don't forget to make the node executable:
$ chmod u+x ~/catkin_ws/src/turtlesim_cleaner/src/gotogoal.py
Understanding the code
The TurtleBot Class
The class TurtleBot will contain all the aspects of our robot, such as pose, the publisher and subscriber, the subscriber callback function and the "move to goal" function.
Our subscriber will subscribe to the topic '/turtle1/pose', which is the topic to which the actual turtlesim position is published. The function update_pose is called when a message is received and saves the actual position in a class attribute called pose.
The euclidean_distance method
This method will use the previously saved turtle position (i.e. self.pose) and the argument (i.e. data) to calculate the point-to-point (Euclidean) distance between the turtle and the goal.
The PID Controller
In order for our robot to move, we will use a PID controller for linear speed and angular velocity. The linear speed will consist of a constant multiplied by the distance between the turtle and the goal and the angular speed will depend on the arctangent of the distance in the y-axis by the distance in the x-axis multiplied by a constant. You can check the Go to Goal Turtlesim Video Tutorials for better explanation·
We have to create a tolerance zone around our goal point, since a really high precision to get exactly to the goal would be needed. In this code, if we use a really small precision the turtle would go crazy (you can have a try!). In other words, the code and the simulator are simplified, so it won't work with full precision.
1 #!/usr/bin/env python 2 #!/usr/bin/env python 3 import rospy 4 from geometry_msgs.msg import Twist 5 from turtlesim.msg import Pose 6 from math import pow, atan2, sqrt 7 8 9 class TurtleBot: 10 11 def __init__(self): 12 # Creates a node with name 'turtlebot_controller' and make sure it is a 13 # unique node (using anonymous=True). 14 rospy.init_node('turtlebot_controller', anonymous=True) 15 16 # Publisher which will publish to the topic '/turtle1/cmd_vel'. 17 self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', 18 Twist, queue_size=10) 19 20 # A subscriber to the topic '/turtle1/pose'. self.update_pose is called 21 # when a message of type Pose is received. 22 self.pose_subscriber = rospy.Subscriber('/turtle1/pose', 23 Pose, self.update_pose) 24 25 self.pose = Pose() 26 self.rate = rospy.Rate(10) 27 28 def update_pose(self, data): 29 """Callback function which is called when a new message of type Pose is 30 received by the subscriber.""" 31 self.pose = data 32 self.pose.x = round(self.pose.x, 4) 33 self.pose.y = round(self.pose.y, 4) 34 35 def euclidean_distance(self, goal_pose): 36 """Euclidean distance between current pose and the goal.""" 37 return sqrt(pow((goal_pose.x - self.pose.x), 2) + 38 pow((goal_pose.y - self.pose.y), 2)) 39 40 def linear_vel(self, goal_pose, constant=1.5): 41 """See video: https://www.youtube.com/watch?v=Qh15Nol5htM.""" 42 return constant * self.euclidean_distance(goal_pose) 43 44 def steering_angle(self, goal_pose): 45 """See video: https://www.youtube.com/watch?v=Qh15Nol5htM.""" 46 return atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) 47 48 def angular_vel(self, goal_pose, constant=6): 49 """See video: https://www.youtube.com/watch?v=Qh15Nol5htM.""" 50 return constant * (self.steering_angle(goal_pose) - self.pose.theta) 51 52 def move2goal(self): 53 """Moves the turtle to the goal.""" 54 goal_pose = Pose() 55 56 # Get the input from the user. 57 goal_pose.x = float(input("Set your x goal: ")) 58 goal_pose.y = float(input("Set your y goal: ")) 59 60 # Please, insert a number slightly greater than 0 (e.g. 0.01). 61 distance_tolerance = input("Set your tolerance: ") 62 63 vel_msg = Twist() 64 65 while self.euclidean_distance(goal_pose) >= distance_tolerance: 66 67 # Porportional controller. 68 # https://en.wikipedia.org/wiki/Proportional_control 69 70 # Linear velocity in the x-axis. 71 vel_msg.linear.x = self.linear_vel(goal_pose) 72 vel_msg.linear.y = 0 73 vel_msg.linear.z = 0 74 75 # Angular velocity in the z-axis. 76 vel_msg.angular.x = 0 77 vel_msg.angular.y = 0 78 vel_msg.angular.z = self.angular_vel(goal_pose) 79 80 # Publishing our vel_msg 81 self.velocity_publisher.publish(vel_msg) 82 83 # Publish at the desired rate. 84 self.rate.sleep() 85 86 # Stopping our robot after the movement is over. 87 vel_msg.linear.x = 0 88 vel_msg.angular.z = 0 89 self.velocity_publisher.publish(vel_msg) 90 91 # If we press control + C, the node will stop. 92 rospy.spin() 93 94 if __name__ == '__main__': 95 try: 96 x = TurtleBot() 97 x.move2goal() 98 except rospy.ROSInterruptException: 99 pass
First, we import the libraries that will be needed. The rospy and geometry_msgs were discussed in the previous tutorials. The math library contains the function that will be used, such as atan, sqrt and round. The turtlesim.msg contains the Pose message type, which is the one published to the topic '/turtle1/pose'. You can check with the following command:
$ rostopic info /turtle1/pose
You should see the following screen:
The pose message is composed by the x and y coordinates, the theta angle, linear velocity and angular velocity. You can see this info with the following command:
$ rosmsg show turtlesim/Pose
You should see the following screen:
Then we create our class. In the __init__ method, we initiate the node, publisher, subscriber and the pose object. It's necessary to set a "publishing rate" in this case too:
10 def __init__(self): 11 # Creates a node with name 'turtlebot_controller' and make sure it is a 12 # unique node (using anonymous=True). 13 rospy.init_node('turtlebot_controller', anonymous=True) 14 15 # Publisher which will publish to the topic '/turtle1/cmd_vel'. 16 self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', 17 Twist, queue_size=10) 18 19 # A subscriber to the topic '/turtle1/pose'. self.update_pose is called 20 # when a message of type Pose is received. 21 self.pose_subscriber = rospy.Subscriber('/turtle1/pose', 22 Pose, self.update_pose) 23 24 self.pose = Pose() 25 self.rate = rospy.Rate(10)
The update_pose method is a callback function which will be used by the subscriber: it will get the turtle current pose and save it in the self.pose attribute:
The euclidean_distance method will be used to calculate the distance between the previously saved turtle position and the goal position:
The move2goal method will be the one which moves the turtle. First, we create the goal_pose object, which will receive the user's input, and it has the same type as the self.pose object. Then, we declare the vel_msg object, which will be published in '/turtle1/cmd_vel':
51 def move2goal(self): 52 """Moves the turtle to the goal.""" 53 goal_pose = Pose() 54 55 # Get the input from the user. 56 goal_pose.x = float(input("Set your x goal: ")) 57 goal_pose.y = float(input("Set your y goal: ")) 58 59 # Please, insert a number slightly greater than 0 (e.g. 0.01). 60 distance_tolerance = input("Set your tolerance: ") 61 62 vel_msg = Twist()
In the while loop, we will keep publishing until the distance of the turtle to the goal is less than the distance_tolerance:
64 while self.euclidean_distance(goal_pose) >= distance_tolerance:
At the end of the loop, we order the turtle to stop:
The following statement causes the node to publish at the desired rate:
The following statement guarantees that if we press CTRL + C our code will stop:
Finally, we call our function move2goal after having created an object x of type TurtleBot:
Testing the code
In a new terminal, run:
In a new terminal, run:
$ rosrun turtlesim turtlesim_node
The turtlesim window will open:
Now, in a new terminal, run our code:
$ rosrun turtlesim_cleaner gotogoal.py
Just type your inputs and the turtle will move! Here we have an example:
rosrun turtlesim_cleaner gotogoal.py Set your x goal: 1 Set your y goal: 1 Set your tolerance: 0.5
The turtle will move like this:
Congratulations! You have finished the tutorials!