In this tutorial, we will build ROS nodes to move the turtlesim robot to goal locations.

Table of Contents

Real-World Applications

This project has a number of real-world applications:

Indoor Delivery Robots
Mapping of Underground Mines, Caves, and Hard-to-Reach Environments
Robot Vacuums
Order Fulfillment
Factories

You can imagine the turtlesim robot is like having an aerial view of a mobile robot moving around on a factory floor. We want to be able to navigate the robot around the floor to do its work, and in order to navigate it, we imagine the factory floor is like an x-y graph (e.g. like the image below from this tutorial). All we would need to do is enter an x-y coordinate, and the robot would move to that goal location (i.e. waypoint).

Prerequisites

You have ROS running on Ubuntu Linux
- I’m running my Ubuntu Linux inside a virtual machine on Windows 10. If you have MacOS or Linux, that will work just fine. Just make sure you have ROS installed.
You know how to run the turtlesim simulation.

Run the Turtlesim Simulation

To run the turtlesim simulation, open a terminal window and launch ROS.

roscore

See what nodes are available to run by typing:

rosrun turtlesim <TAB><TAB>

Open another terminal window, and launch the turtlesim_node.

rosrun turtlesim turtlesim_node

Let’s steer the robot.

rosrun rqt_robot_steering rqt_robot_steering

You should see a window pop up.

You’ll note that the default ROS topic is the /cmd_vel topic. This topic is where the rqt_robot_steering node will publish Twist messages. A Twist message in ROS is a special kind of topic that consists of the x, y, and z components of the linear and angular velocity.

Go back to the terminal window, and let’s see what topic the turtlesim node is subscribed to.

rostopic list

The turtlesim node gets its velocity commands from the /turtle1/cmd_vel topic.

List all the active nodes.

rosnode list

Find out what the /turtlesim node is subscribing to.

rosnode info /turtlesim

You can see that the /turtlesim node is subscribing to /turtle1/cmd_vel

Let’s find out more about this topic.

rostopic info /turtle1/cmd_vel

You can see that the type of message that passes through the /turtle1/cmd_vel topic is a Twist message.

Let’s change the topic name of the rqt_steering GUI to /turtle1/cmd_vel.

If you adjust the sliders for the linear and angular velocities, you will see the turtle move accordingly because the GUI is publishing linear and angular velocity values to the /turtle1/cmd_vel topic. The /turtlesim node reads the velocity values on this topic and then moves accordingly.

In a real-world robot, we can see how the rqt_steering node is useful. We can publish velocity commands to the /cmd_vel topic. A motor controller that subscribes to the /cmd_vel topic can convert those velocity commands into motor commands to make the robot drive accordingly.

Create a Package

Now that we’ve gotten reacquainted with turtlesim, let’s see how we get into the purpose of this tutorial: create a ROS node to move the turtlesim robot to a goal location.

The first thing we need to do is create a package.

Open a new terminal window.

Go to the src folder of your catkin_ws folder.

cd ~/catkin_ws/src

catkin_create_pkg go_to_goal_turtlesim std_msgs rospy roscpp

Move inside the package.

cd go_to_goal_turtlesim

Open the CMakeLists.txt file.

gedit CMakeLists.txt

Enable C++11 support be removing the hashtag # on line 5 by uncommenting the add_compile_option… line.

Here is how that should look now.

How to Move the Robot to a Desired X Coordinate

Create the ROS Node

Now let’s create a program (i.e. ROS Node) that will take the coordinates of the turtle as input and will output a velocity command as a Twist message. The velocity command will be calculated so as to get the turtlesim robot to move to a goal x coordinate that can be any number from 0 to 11.

In this first node, we will create a ROS node to move the turtlesim robot to a goal x coordinate.

The first thing we need to do is to open a new terminal window.

Move to the src folder of the go_to_goal_turtlesim package.

cd ~/catkin_ws/src/go_to_goal_turtlesim/src

Create a new C++ program called go_to_goal_x.cpp.

gedit go_to_goal_x.cpp

Add the following code to the file, and then click Save.

/// @file go_to_goal_x.cpp
/// @author Addison Sears-Collins
/// @date May 4, 2021
/// 
/// This program takes the coordinates of the turtlesim robot as input
/// and outputs a velocity command as a Twist message. The velocity command
/// will be calculated so as to get the turtlesim robot to move to a goal 
/// x coordinate that can be any number from 0 to 11.

#include <cstdlib> // Use for the absolute value method abs()
#include <iostream> // Enables command line input and output

#include "ros/ros.h" // Necessary header files for ROS
#include "geometry_msgs/Twist.h" // Twist messages (linear & angular velocity)
#include "geometry_msgs/Pose2D.h" // x, y position and theta orientation
#include "turtlesim/Pose.h" // x, y, theta, linear & angular velocity

// Remove the need to use std:: prefix
using namespace std;

// Key variable declarations 
geometry_msgs::Twist velCommand; // Linear and angular velocity in m/s 
geometry_msgs::Pose2D current; // Current x, y, and theta 
geometry_msgs::Pose2D desired; // Desired x, y, and theta 

// Goal x-value, which can be any number from 0 to 11 (inclusive)
const double GOAL = 1.3;

// The gain K, which is used to calculate the linear velocity
const double K_l = 1.0;

// The distance threshold in meters that will determine when 
// the turtlesim robot successfully reaches the goal.
const double distanceTolerance = 0.1;

// Initialized variables and take care of other setup tasks
void setup() {

  // Desired x goal coordinate
  desired.x = GOAL;
  
  // Initialize the Twist message.
  // Initial linear and angular velocities are 0 m/s and rad/s, respectively.
  velCommand.linear.x = 0.0;
  velCommand.linear.y = 0.0;
  velCommand.linear.z = 0.0;
  velCommand.angular.x = 0.0;
  velCommand.angular.y = 0.0;
  velCommand.angular.z = 0.0;
}

// Get the distance between the current x coordinate and 
// the desired x coordinate.
double getDistanceToGoal() {
  return desired.x - current.x;
}

// If we haven't yet reached the goal, set the velocity value.
// Otherwise, stop the robot.
void setVelocity() {
  if (abs(getDistanceToGoal()) > distanceTolerance) {

    // The magnitude of the robot's velocity is directly
    // proportional to the distance the robot is from the 
    // goal.
    velCommand.linear.x = K_l * getDistanceToGoal();
  }
  else {
    cout << "Goal has been reached!" << endl << endl;
    velCommand.linear.x = 0;
  }
}

// This callback function updates the current position and 
// orientation of the robot. 
void updatePose(const turtlesim::PoseConstPtr &currentPose) {
  current.x = currentPose->x;
  current.y = currentPose->y;
  current.theta = currentPose->theta;
}

int main(int argc, char **argv) {

  setup();  

  // Initiate ROS
  ros::init(argc, argv, "go_to_goal_x");
    
  // Create the main access point to communicate with ROS
  ros::NodeHandle node;

  // Subscribe to the robot's pose
  // Hold no messages in the queue. Automatically throw away 
  // any messages that are received that are not able to be
  // processed quickly enough.
  // Every time a new pose is received, update the robot's pose.
  ros::Subscriber currentPoseSub =
    node.subscribe("turtle1/pose", 0, updatePose);

  // Publish velocity commands to a topic.
  // Hold no messages in the queue. Automatically throw away 
  // any messages that are received that are not able to be
  // processed quickly enough.
  ros::Publisher velocityPub =
    node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 0);

  // Specify a frequency that want the while loop below to loop at
  // In this case, we want to loop 10 cycles per second
  ros::Rate loop_rate(10); 

  // Keep running the while loop below as long as the ROS Master is active. 
  while (ros::ok()) {

    // Here is where we call the callbacks that need to be called.
    ros::spinOnce();

    // After we call the callback function to update the robot's pose, we 
    // set the velocity values for the robot.
    setVelocity();

    // Publish the velocity command to the ROS topic
    velocityPub.publish(velCommand);

    // Print the output to the console
    cout << "Current x = " << current.x << endl
         << "Desired x = " << desired.x <<  endl
         << "Distance to Goal = " << getDistanceToGoal() << " m" << endl
         << "Linear Velocity (x) = " << velCommand.linear.x << " m/s" << endl
         << endl;

    // Sleep as long as we need to to make sure that we have a frequency of
    // 10Hz
    loop_rate.sleep();
  }

  return 0;
}

Now we need to add the C++ program we just wrote to the CMakeLists.txt file so that we can compile the program and launch it using ROS. Each time we write a new C++ ROS node, we have to make sure we add it to the CMakeLists.txt file.

cd ~/catkin_ws/src/go_to_goal_turtlesim

gedit CMakeLists.txt

Go to the bottom of the file.

Add the following lines.

Save the file, and close it.

Go to the root of the workspace.

cd ~/catkin_ws

Compile the code.

catkin_make

Launch the ROS Node

Now let’s launch the ROS node we created in the last section.

Open a new terminal window.

Start ROS.

roscore

Open another terminal window, and launch the turtlesim_node.

rosrun turtlesim turtlesim_node

We can see that the starting pose of the robot is as follows:

x = 5.544445
y = 5.544445
theta = 0.000000

Sorry the font is so small.

Open another terminal window, and launch the go_to_goal_x node.

rosrun go_to_goal_turtlesim go_to_goal_x

How to Move the Robot to a Desired X and Y Coordinate (i.e. Waypoint)

Create the Waypoint Publisher ROS Node

In this section, we’ll write a ROS node that can accept a waypoint (i.e. goal x and y coordinate) from the user. We want to then publish that goal location to a ROS topic named “waypoint”.

We will later write a ROS node that can read the waypoint and navigate the turtlesim robot to that waypoint.

First, move to the src folder of the go_to_goal_turtlesim package.

cd ~/catkin_ws/src/go_to_goal_turtlesim/src

Create a new C++ program called waypoint_publisher.cpp.

gedit waypoint_publisher.cpp

Add the following code to the file, and then click Save.

/// @file waypoint_publisher.cpp
/// @author Addison Sears-Collins
/// @date May 6, 2021
/// 
/// This program takes the user's desired waypoint (i.e. goal location 
/// or x,y coordinate) as input and publishes it to a topic as a 
/// geometry_msgs/Pose2D data type.

#include <iostream> // Enables command line input and output

#include "ros/ros.h" // Necessary header files for ROS
#include "geometry_msgs/Pose2D.h" // x, y position and theta orientation

// Remove the need to use std:: prefix
using namespace std;

// Key variable declarations 
geometry_msgs::Pose2D waypoint; // Goal location ... x, y, and theta 

// Ask user for the desired waypoint...Where do you want the robot to move to?
void set_waypoint() {

  cout << "Where do you want the robot to go?" << endl; 
  cout << "Enter waypoint x: ";
  cin >> waypoint.x;
  cout << "Enter waypoint y: ";
  cin >> waypoint.y;
  cout << endl;  
}

int main(int argc, char **argv) { 

  // Initiate ROS
  ros::init(argc, argv, "waypoint_publisher");
    
  // Create the main access point to communicate with ROS
  ros::NodeHandle node;

  // Publish waypoint to a topic.
  // Hold no messages in the queue. Automatically throw away 
  // any messages that are not able to be processed quickly enough.
  ros::Publisher waypointPub =
    node.advertise<geometry_msgs::Pose2D>("waypoint", 0);

  // Keep running the while loop below as long as the ROS Master is active. 
  while (ros::ok()) {

    // Ask the user where he wants the robot to go
    set_waypoint();

    // Publish the waypoint to the ROS topic
    waypointPub.publish(waypoint);

  }

  return 0;
}

Now we need to add the C++ program we just wrote to the CMakeLists.txt file.

cd ~/catkin_ws/src/go_to_goal_turtlesim

gedit CMakeLists.txt

Go to the bottom of the file.

Add the following lines.

Save the file, and close it.

Go to the root of the workspace.

cd ~/catkin_ws

Compile the code.

catkin_make

Launch the ROS Node

Now let’s launch the ROS node we created in the last section.

Open a new terminal window.

Start ROS.

roscore

Open another terminal window, and launch the waypoint_publisher node.

rosrun go_to_goal_turtlesim waypoint_publisher

Here is what you will see in the terminal.

Open a new terminal, and check out the active topics.

rostopic list

We can see one of the topics is /waypoint.

Let’s see what messages are passing through this topic.

rostopic echo /waypoint

You can see the pose of the robot (i.e. x, y, and theta values) is being published to the topic named /waypoint.

Create a ROS Node to Move the Robot to the Waypoint (i.e. X-Y Coordinate)

Now we need to create a ROS node that subscribes to the /waypoint topic and publishes a velocity command to move the turtle to the user’s desired waypoint (i.e. x-y coordinate).

Open a new terminal window.

Move to the src folder of the go_to_goal_turtlesim package.

cd ~/catkin_ws/src/go_to_goal_turtlesim/src

Create a new C++ program called go_to_goal_x_y.cpp.

gedit go_to_goal_x_y.cpp

Add the following code to the file, and then click Save.

/// @file go_to_goal_x_y.cpp
/// @author Addison Sears-Collins
/// @date May 7, 2021
/// 
/// This program takes the coordinates of the turtlesim robot and a goal 
/// waypoint (published by waypoint_publisher.cpp) as input and outputs 
/// a velocity command as a Twist message. The velocity command
/// will be calculated so as to get the turtlesim robot to move to the user's
/// desired goal waypoint (x, y coordinate). The x and y values can be any 
/// number from 0 to 11.

#include <cstdlib> // Use for the absolute value method abs()
#include <iostream> // Enables command line input and output

#include "ros/ros.h" // Necessary header files for ROS
#include "geometry_msgs/Twist.h" // Twist messages (linear & angular velocity)
#include "geometry_msgs/Pose2D.h" // x, y position and theta orientation
#include "turtlesim/Pose.h" // x, y, theta, linear & angular velocity

// Remove the need to use std:: prefix
using namespace std;

// Key variable declarations 
geometry_msgs::Twist velCommand; // Linear and angular velocity in m/s 
geometry_msgs::Pose2D current; // Current x, y, and theta 
geometry_msgs::Pose2D waypointGoal; // Waypoint x, y, and theta (the waypoint)
ros::Publisher velocityPub; // Object used for publishing velocity command

const double PI = 3.141592654;

// The gain K, which is used to calculate the linear velocity
const double K_l = 0.5;

// The gain K, which is used to calculate the angular velocity
const double K_a = 0.5;

// The distance threshold in meters that will determine when 
// the turtlesim robot successfully reaches the goal.
const double distanceTolerance = 0.1;

// The angle threshold in radians that will determine when 
// the turtlesim robot successfully reaches the goal.
const double angleTolerance = 0.1;

// This flag determines when the robot needs to either 
// move towards a waypoint or stop.
bool goToWaypoint = false;

// Initialized variables and take care of other setup tasks
void setup() {

  // We initialize with the default starting coordinate 
  // for the turtlesim simulator
  waypointGoal.x = 5.544445;
  waypointGoal.y = 5.544445;
  
  // Initialize the Twist message.
  // Initial linear and angular velocities are 0 m/s and rad/s, respectively.
  velCommand.linear.x = 0.0;
  velCommand.linear.y = 0.0;
  velCommand.linear.z = 0.0;
  velCommand.angular.x = 0.0;
  velCommand.angular.y = 0.0;
  velCommand.angular.z = 0.0;
}

// Get the distance between the current x,y coordinate and 
// the desired waypoint x,y coordinate.
double getDistanceToWaypoint() {
  return sqrt(pow(waypointGoal.x - current.x, 2) + pow(
    waypointGoal.y - current.y, 2));
}

// Get the heading error
// i.e. how many radians does the robot need 
// to turn to head towards the waypoint  
double getHeadingError() {

  double deltaX = waypointGoal.x - current.x;
  double deltaY = waypointGoal.y - current.y;
  double waypointHeading = atan2(deltaY, deltaX);
  double headingError = waypointHeading - current.theta;   
  
  // Make sure heading error falls within -PI to PI range
  if (headingError > PI) {
    headingError = headingError - (2 * PI);
  } 
  if (headingError < -PI) {
    headingError = headingError + (2 * PI);
  } 
  
  return headingError;
}

// If we haven't yet reached the goal, set the velocity value.
// Otherwise, stop the robot.
void setVelocity() {

  double distanceToWaypoint = getDistanceToWaypoint();
  double headingError = getHeadingError();

  // If we are not yet at the waypoint
  if (goToWaypoint == true && (abs(distanceToWaypoint) > distanceTolerance)) {
   
    // If the robot's heading is off, fix it.
    if (abs(headingError) > angleTolerance) {
      velCommand.linear.x = 0.0;
      velCommand.angular.z = K_a  * headingError;
    }
    // Just fix the distance gap between current pose and waypoint.
    // The magnitude of the robot's velocity is directly
    // proportional to the distance the robot is from the 
    // goal.
    else {
      velCommand.linear.x = K_l * distanceToWaypoint;
      velCommand.angular.z = 0.0;    
    }
  }
  else {
    cout << "Goal has been reached!" << endl << endl;
    velCommand.linear.x = 0.0;
    velCommand.angular.z = 0.0; 
    goToWaypoint == false;
  }
}

// This callback function updates the current position and 
// orientation of the robot. 
void updatePose(const turtlesim::PoseConstPtr &currentPose) {
  current.x = currentPose->x;
  current.y = currentPose->y;
  current.theta = currentPose->theta;
}

// This callback function updates the desired waypoint when a waypoint
// message is published to the /waypoint topic
void updateWaypoint(const geometry_msgs::Pose2D &waypointPose) {
  waypointGoal.x = waypointPose.x;
  waypointGoal.y = waypointPose.y;
  goToWaypoint = true;  
}

int main(int argc, char **argv) {

  setup();  

  // Initiate ROS
  ros::init(argc, argv, "go_to_goal_x_y");
    
  // Create the main access point to communicate with ROS
  ros::NodeHandle node;

  // Subscribe to the robot's pose
  // Hold no messages in the queue. Automatically throw away 
  // any messages that are received that are not able to be
  // processed quickly enough.
  // Every time a new pose is received, update the robot's pose.
  ros::Subscriber currentPoseSub =
    node.subscribe("turtle1/pose", 0, updatePose);
    
  // Subscribe to the user's desired waypoint
  // Hold no messages in the queue. Automatically throw away 
  // any messages that are received that are not able to be
  // processed quickly enough.
  // Every time a new waypoint is received, update the robot's 
  // desired waypoint.
  // The tcpNoDelay is to reduce latency between nodes and to make sure we are
  // not missing any critical waypoint messages.
  ros::Subscriber waypointPoseSub =
    node.subscribe("waypoint", 0, updateWaypoint, 
    ros::TransportHints().tcpNoDelay());

  // Publish velocity commands to a topic.
  // Hold no messages in the queue. Automatically throw away 
  // any messages that are received that are not able to be
  // processed quickly enough.
  velocityPub =
    node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 0);

  // Specify a frequency that want the while loop below to loop at
  // In this case, we want to loop 10 cycles per second
  ros::Rate loop_rate(10); 

  // Keep running the while loop below as long as the ROS Master is active. 
  while (ros::ok()) {

    // Here is where we call the callbacks that need to be called.
    ros::spinOnce();

    // After we call the callback function to update the robot's pose, we 
    // set the velocity values for the robot.
    setVelocity();

    // Publish the velocity command to the ROS topic
    velocityPub.publish(velCommand);

    // Print the output to the console
    cout << "Current (x,y) = " << "(" << current.x << "," << current.y << ")" 
         << endl
         << "Waypoint (x,y) = " << "(" << waypointGoal.x << "," 
         << waypointGoal.y << ")" 
         << endl
         << "Distance to Waypoint = " << getDistanceToWaypoint() << " m" 
         << endl
         << "Linear Velocity (x) = " << velCommand.linear.x << " m/s" 
         << endl << endl;

    // Sleep as long as we need to to make sure that we have a frequency of
    // 10Hz
    loop_rate.sleep();
  }

  return 0;
}

Now we need to add the C++ program we just wrote to the CMakeLists.txt file.

cd ~/catkin_ws/src/go_to_goal_turtlesim

gedit CMakeLists.txt

Go to the bottom of the file.

Add the following lines.

Save the file, and close it.

Go to the root of the workspace.

cd ~/catkin_ws

Compile the code.

catkin_make

Launch the ROS Node

Now let’s launch the ROS node we created in the last section.

Open a new terminal window.

Start ROS.

roscore

Open another terminal window, and launch the turtlesim_node.

cd ~/catkin_ws/

rosrun turtlesim turtlesim_node

Open another terminal window, and launch the waypoint_publisher node.

rosrun go_to_goal_turtlesim waypoint_publisher

Open another terminal window, and launch the go_to_goal_x_y node.

rosrun go_to_goal_turtlesim go_to_goal_x_y

Now, go back to the terminal window where you launched the waypoint_publisher node.

Enter any waypoint x value from 0 to 11. I will enter 1.2.

Enter any waypoint y value from 0 to 11. I will enter 2.7.

When the turtle gets close enough, you will see a message that says “Goal has been reached!”

Here is what the simulation looks like. You will see the robot moved from (5.544445, 5.544445) to (1.29721, 2.71791)…which is close enough to the waypoint. Once the turtle reached the waypoint, it stopped.

Let’s try another waypoint.

Go back to the terminal window where you launched the waypoint_publisher node.

Enter any waypoint x value from 0 to 11. I will enter 11.

Enter any waypoint y value from 0 to 11. I will enter 11.

Here is the output:

Create a Launch File

So that we don’t have to keep opening up ROS nodes in the future, we will create a launch file to make our life easier.

The first thing we need to do is to open a new terminal window and go to the go_to_goal_turtlesim package.

cd ~/catkin_ws/src/go_to_goal_turtlesim

Create a folder called ‘launch’.

mkdir launch

Create a new launch file inside the launch directory you just made.

cd launch

Open up the text editor.

gedit turtlesim_waypoint_follower.launch

Type the code below into the file, and save it.

<launch>
  <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" />
  <node pkg="go_to_goal_turtlesim" type="waypoint_publisher" name="waypoint_publisher" output="screen" />
  <node pkg="go_to_goal_turtlesim" type="go_to_goal_x_y" name="go_to_goal_x_y" />
</launch>

Save this file to the catkin_ws/src/go_to_goal_turtlesim/launch folder. This file will run C++ executables.

Change the permissions of the launch file we just created.

cd ~/catkin_ws/src/go_to_goal_turtlesim/launch

sudo chmod +x turtlesim_waypoint_follower.launch

Run the launch file. Open a new terminal window, and type:

cd ~/catkin_ws/

This command below is all a single command on one line.

roslaunch go_to_goal_turtlesim turtlesim_waypoint_follower.launch

References

Credit to the book Practical Robotics in C++: Build and Program Real Autonomous Robots Using Raspberry Pi (#ad) … a fantastic book, which I highly recommend … and the official ROS tutorials for Turtlesim.

Disclosure (#ad): As an Amazon Associate I earn from qualifying purchases.