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 Goal

Description: This tutorial is based on Turtlesim Video Tutorials

Tutorial Level: INTERMEDIATE

Contents

Preparing for work
Understanding the code
The Code
Testing the code

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.

The Subscriber

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 Proportional Controller

In order for our robot to move, we will use a Proportional control 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·

Tolerance

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.

The Code

   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:

  27     def update_pose(self, data):
  28         """Callback function which is called when a new message of type Pose is
  29         received by the subscriber."""
  30         self.pose = data
  31         self.pose.x = round(self.pose.x, 4)
  32         self.pose.y = round(self.pose.y, 4)

The euclidean_distance method will be used to calculate the distance between the previously saved turtle position and the goal position:

  34     def euclidean_distance(self, goal_pose):
  35         """Euclidean distance between current pose and the goal."""
  36         return sqrt(pow((goal_pose.x - self.pose.x), 2) +
  37                     pow((goal_pose.y - self.pose.y), 2))

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:

  85         # Stopping our robot after the movement is over.
  86         vel_msg.linear.x = 0
  87         vel_msg.angular.z = 0
  88         self.velocity_publisher.publish(vel_msg)

The following statement causes the node to publish at the desired rate:

  82             # Publish at the desired rate.
  83             self.rate.sleep()

The following statement guarantees that if we press CTRL + C our code will stop:

  90         # If we press control + C, the node will stop.
  91         rospy.spin()

Finally, we call our function move2goal after having created an object x of type TurtleBot:

  93 if __name__ == '__main__':
  94     try:
  95         x = TurtleBot()
  96         x.move2goal()
  97     except rospy.ROSInterruptException:
  98         pass

Testing the code

In a new terminal, run:

$ roscore

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!

ROS 2 Documentation

Wiki

Page

User