Assignment 2, Part 2

Part 2: Control


If you are not familiar with the creation and use of classes in Python, you should read chapter 9 of the official Python tutorial.

Ensure that the following packages are installed on your system (they are already installed in the lab):

The following zip file contains a package called comp6912_a2_world.  Unzip this file into your catkin_ws/src directory:

Execute the following command:

You will see a little robot and a red target square.  A laser range finder is simulated, but we will not be using this sensor for this assignment (although you should feel free to experiment).  To teleoperate the robot, open another terminal and execute the following:

Note that will need to have the terminal selected for your keypresses to be able to control the robot.  You can combine the launch of the world with teleoperation by launching “all.launch”.  Take a few minutes to drive around, check out the topics published by Stage, and mouse around with the Simulator.  Once you are ready to work, create a package called a2 as follows:

Note: In the tasks that follow we are assuming that the robot has perfect access to its own pose.  This is an unrealistic assumption that we will tackle later in the course.

Create a scripts directory in a2 and place in that directory.

Task 1: Smooth Controller 1

Implement “smooth controller 1″ discussed in the third set of notes on kinematics.  Your task is to create a class called SmoothController1 that fulfills the requirements in  This class should go in a file called  Note that the parameter settings for “smooth controller 1″ are not critical (both can be set to 1 or you can experiment with different values).  You can use the following template for (make the appropriate changes for tasks 2 and 3):

Test your code by launching the Stage simulation in one terminal (roslaunch a2_world world.launch) and executing the following in another terminal:

rosrun a2

The controller should drive the robot to the red square (which the robot can pass through).  Note that there is no goal angle specified with smooth controller 1.  Important: you should not use the tf library here.  The arguments passed into the constructor and the get_twist method provide everything that you need.  The same goes for task 2.

Task 2: Smooth Controller 2

As above, only implement smooth controller 2.  Your class should go in (which can start from the same template as  You will have to edit in order to use smooth2 instead of smooth1 (look for the comments labelled “STUDENT”).

Note that this controller should guide the robot to the red square such that it ends up pointing down the negative x-axis.  You should experiment with setting different goal angles to ensure that it works reliably.  Note that smooth controller 2 will work well only with certain parameter settings.  I have found good results with the following:

  • K_RHO = 0.6
  • K_ALPHA = 1.6
  • K_THETA = 0.3

Task 3: Smooth Controller 1 using TF

Complete the first set of tutorials on the tf library under “Learning tf”.  You should follow the Python stream of tutorials.

Re-implement smooth controller 1 to utilize tf.  This should allow you to eliminate almost all of the math from the get_twist method (which now takes no arguments, since we are using tf internally).  The class should be named SmoothController1TF and should be placed in  Once again, you will need to edit in order to activate smooth1_tf.

Note that all of the required functions from the tf library are demonstrated through the tf tutorials.  For this part you should pay special attention to the “Adding a frame” tutorial.  However, you should be able to avoid creating an additional “broadcaster” node.  Please take a look at the code that already exists within


Submit,, and through D2L by the deadline given here.


  • To calculate the arctangent, you are recommended to use atan2 (see documentation here).  To understand why see the second answer here.
  • For Task 3:
    • The “/odom” frame is fixed to the centre of the simulation
    • The “/base_footprint” frame moves with the robot.  Notice that to get the robot’s current position in we lookup the transform between “/odom” and “/base_footprint”.
    • The “/odom” frame is not guaranteed to be available at the moment your script starts.  So it is a good idea to use a try-except block, just as is done in when you lookup a transform.