Move to Goal Controller and Cleaning Application

From Introduction to Robotics (CS460)
Jump to: navigation, search
[WEEK 5] Lab 01: Move to Goal Proportional Controller and Cleaning Application


In this lab, we aim at developing a cleaning application using the turtlesim simulator in ROS.
In the lectures, we agreed to first develop functions that we will need for the navigation of the robot and that will be used to develop the cleaning application. For this, we have developed the following functions:

  • void move(double speed, double distance, bool isForward) which makes the robot move with a certain linear velocity for a certain distance in a forward or backward straight direction.
  • void rotate (double angular_speed, double angle, bool clockwise) which makes the robot turn with a certain angular velocity, for a certain distance in either clockwise or counter-clockwise direction.
  • double setAbsoluteOrientation (double desired_angle) which makes the robot turn towards a specified desired absolute orientation.

As ROS deals with angles in radians, and we - humans - provide inputs in degree, we also developed the method double degrees2radians(double angle_in_degrees) to convert angles from degrees to radians.
For this, we needed to implement a subscriber to get the instantaneous pose of the robot:

ros::Subscriber pose_subscriber = n.subscribe("/turtle1/pose", 10, poseCallback);

The poseCallback is the function that is automatically executes whenever a new pose is published by Turtlesim and it updates the global variable The turtlesim::Pose turtlesim_pose to update the pose variable.

void poseCallback(const turtlesim::Pose::ConstPtr & pose_message);

Lab Objectives

The objective of this lab is to complete the code to build the cleaning application. The turtlesim must over all the area.
First, we need to implement a simple proportional controller to make the turtlesim go towards a certain goal location. Then, we use all the functions defined to build an algorithm for the cleaning application.

Source File Code

As a starting point, use the following robot_cleaner.cpp and complete the missing code of the empty functions.
You can copy/paste this code or replace your robot_cleaner.cpp with the file above.


You can any task in any order, but better to start with Task 1 and use it Task 2.

Task 1: Implement the Proportional Controller

In this task, you will implement a P-Controller that you have learnt in the Kinematics lecture for a robot to move to a point.
We remind that:

  • The linear velocity is controlled by the following equation: v = Kv * distance (robot, goal)
  • The angular velocity is controlled by the following equation: w = Kw * (Goal_Orientation - Robot_Orientation)

Kv and Kw are two constant of your choice.
Remember that for finding the angle between the robot center point with coordinate (xr, yr) and the goal point with coordinate (xt, yt), we use the arctangent function atan2 available in all common programming languages including C++ and Python. So, angle = atan2((yt-yr)/(xt-xr)). A good property of atan2 is that it always returns an angle between [-PI, PI] radians.

Test this function carefully by trying several values and observe and study their effects on the quality of controller. In general, Kw should be greater than Kv in this case, but you can try different configurations.

Task 2: Implement the cleaning application

In this task, you will implement a cleaning application (similar to what was demonstrated today in lecture), where you make the robot covers the entire area, like vacuum cleaner robot.
First, design an algorithm that makes the turtlesim robot cover the area of interest. Then, implement your algorithm using the different methods we have already implemented for the motion of the robot. You must count the total traveled distance, that is the length of the path for the cleaning. The total traveled distance can be calculated as the sum of all segments (lines) run by the robot, which also can be estimated as the sum of distance between any two points visited by the robot.
There is no single solution for the cleaning algorithm. One possible option is to make the robot moves to an inital location, for example (0.5, 0.5), then make straight motions and rotations to cover the area. Make sure that distance between two parallel lines is 0.5. Also, there must be as little number of intersections as possible.

What to Submit

  • A one-page report presenting
    • your observation of the effect of Kv and Kw constants on the quality of the P-Controller.
    • a brief description of your cleaning algorithm.
    • Your added code

How to Submit

You will be make a demonstration in the lecture.


  • Grade: 10 points
  • Bonus: The best P-Controller will get 0.5 extra point. The best Cleaning algorithm will take 0.5 extra point. The best cleaner is the one with (1) make a full coverage of the area, which is expressed by the estimated traveled distance divided by the total area of the surface (11*11) (2) lowest execution time (t1-t0), (3) minimum number of intersection between segments.
  • Deadline: Sunday March 08, 2015 (Demonstration in the lecture)


Send an email to