In this tutorial, we will build ROS nodes to move the turtlesim robot to goal locations.
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 ¤tPose) {
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
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 ¤tPose) {
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.
