Introduction to ROS

(Robot Operating System)

Raoul DIFFOUO
Tshwane University of Technology
Department of Computer Engineering
April 2013

Table of Contents
Introduction .................................................................................................................................... 3
1.

ROS Basics ............................................................................................................................... 3

What is ROS? .............................................................................................................................. 3
ROS Distributions ................................................................................................................... 3
Where is it used? ........................................................................................................................ 4
How does ROS work? ................................................................................................................. 4
Key concepts and Terminology ................................................................................................. 6
ROS Filesystem ....................................................................................................................... 6
Packages .............................................................................................................................. 6
Manifests.............................................................................................................................. 6
Stacks ................................................................................................................................... 6
Stack Manifests.................................................................................................................... 6
Message Types .................................................................................................................... 6
Services types....................................................................................................................... 6
ROS Computation Graph Level ............................................................................................. 7
Nodes ................................................................................................................................... 7
Master .................................................................................................................................. 7
Messages ............................................................................................................................. 7
Topics ................................................................................................................................... 7
Services ................................................................................................................................ 7
ROS Community Level ........................................................................................................... 8
Installing and Configuring your ROS Environment ................................................................... 8
Create a ROS Workspace ...................................................................................................... 8
Create a ROS Package ........................................................................................................... 8
Writing our first ROS node in C++ ............................................................................................ 9
The Publisher .......................................................................................................................... 9
The code .............................................................................................................................. 9
Code Breakdown ............................................................................................................... 10
The Listener .......................................................................................................................... 12
The code ............................................................................................................................ 12

Code Breakdown ............................................................................................................... 13

Building and executing the nodes ...................................................................................... 14
2.

NXT Robot and ROS ............................................................................................................. 15

Installation and Configurations ................................................................................................ 15
Configure the computer ...................................................................................................... 15
Installing ROS and NXT ROS ............................................................................................... 16
Lets Try It! ............................................................................................................................. 17
Teleoperate the NXT using Keyboard ..................................................................................... 18
Follow Objects with your NXT and ROS .................................................................................. 21
The code ............................................................................................................................... 22
Code Breakdown .................................................................................................................. 25
The header files ................................................................................................................. 25
NxtFollow Class definition ................................................................................................ 25
The constructor ................................................................................................................. 26
The control loop ................................................................................................................ 27
The callback method ......................................................................................................... 28
The publish method .......................................................................................................... 28
The main ............................................................................................................................ 28
Build and Run the Execute the Node ................................................................................. 29

3.

Useful Links ............................................................................................................................ 31

Learn C++ ............................................................................................................................. 31
ROS Wiki ............................................................................................................................... 31
NXT ROS ............................................................................................................................... 31

Introduction
This document is an attempt to give an overview of ROS to robotic hobbyist. While we cant
hope to provide you with a full coverage of ROS capabilities, we present some of the most
important key concepts and in the second part, go through a practical usage with the NXT
Robot platform. You are in the meantime invited to visit the official documentation
maintained by the developer of ROS, where you will find number of tutorials, at
http://www.ros.org

1. ROS Basics
What is ROS?
ROS (Robot Operating System) is an open-source, meta-operating system for your robot. It
provides libraries and tools to help software developers create robot application. It also
provide hardware abstraction, device drivers, visualizers, message-passing, package
management, and more. ROS was originally developed in 2007 under the name switchyard
by the Stanford AI Laboratory and was further developed at Willow Garage with
contributions all around the world.
ROS Distributions
ROS releases might not be compatible with other. They are often referred to by code name
rather than version number. The major releases so far are listed in the table below:
ROS Distributions
ROS Groovy
ROS Fuerte
ROS Electric
ROS Diamondback
ROS C Turtle
ROS Box Turtle

Release Date
December, 2012
April, 2012
August, 2011
March, 2011
August, 2010
March, 2010

Wiki Page
http://ros.org/wiki/groovy
http://ros.org/wiki/fuerte
http://ros.org/wiki/electric
http://ros.org/wiki/diamondback
http://ros.org/wiki/cturtle
http://ros.org/wiki/boxturtle

ROS currently runs on Unix-based platform. Software for ROS is primarily tested on Ubuntu
and Mac OS X systems. The ROS community has been contributing support for Fedora, Arch
Linux, and other Linux platforms. A Port to Microsoft Windows is currently under
development and still at an experimental stage.
It is highly recommended to install ROS on Ubuntu. Go to the following link for installation
guide.
http://www.ros.org/wiki/ROS/Installation

Where is it used?
From small differential-drive robots to mobile manipulators to autonomous cars, robots of
every size and shape are using ROS to do interesting research and applications
development. Groups around the world are also releasing free, open-source software to get
you started on your own robot. Find a list of robots using ROS at:
http://www.ros.org/wiki/Robots

How does ROS work?

The previous pictures depict a situation where we would like to control a simple differential
drive robot using ROS. The robot is connected to a PC running ROS, and is connected via
USB. On the ROS software layout, we have a Node for our Robot, robot-node that is set to
receive velocities on cmd_vel Topic. The robot Node also creates a Topic for all the sensors
available, on which it publishes their respective values. On the other hand we have a control
node which in this case receives distance values from the Ultrasonic sensor on Topic
ultrasonic. Control node then publishes velocities on Topic cmd_vel.
The ROS Master here plays a very essential role in the system. When systems starts every
Nodes needs to present themselves to the Master and specify the topic they will be
subscribing or publishing to. The Master is responsible for setting the communication
between Nodes and making sure that every Node finds each other and exchange messages
through the proper channels.

Assume we want to add a camera to our system, in order to capture image and move the
robot based on what we receive. All we would have to do is create a Node to handle camera
that will publish a message images on Topic image. Then a Node camvision that receives
images on topic image then publish camdata on topic image_data. Lastly our control node
would be modified to subscribe to topic image-data instead and still publish velocities on
cmd_vel. It would at the end look something like this

Key concepts and Terminology

Before we dive into using ROS it is important for use to understand its terminology and
structure. ROS has three levels of concepts: the Filesystem level, the Computational Graph
level, and the Community level. A brief description of each concept and will give more details
in later sections of this document.
ROS Filesystem
Packages
Packages are the main unit for organizing software in ROS.
A package contain ROS runtime processes (Nodes),
configurations files, build files, launch files, datasets or
anything that is usefully organised together. A Package can
contain any number of nodes, and all the code in a package
should be related.
Manifests
Manifests (manifest.xml) provide metadata about the
package, including license information and dependencies as
well as language-specific information such as compiler flags.
Stacks
Stacks are a collection of packages that provide aggregate functionality. As an example the
Navigation Stack contains all the necessary packages for finding where the robot is and how
it can get somewhere else. ROS software are also released in a form of Stacks and have
associated version numbers.
Stack Manifests
Stack manifest (stack.xml) provide data about a stack, including its license information and
its dependencies on other stacks.
Message Types
Messages descriptions, store in my_package/msg/MyMessageType.msg, define the data
structures for messages sent in ROS.
Services types
Services descriptions, stored in my_package/srv/MyServicesType.srv, define the request and
response data structures for services in ROS.

ROS Computation Graph Level

This is peer-to-peer network of ROS processes that are processing data together. The basic
Computation Graph concepts of ROS are nodes, Master, Parameter Server, messages,
services, topics, and bags all of which provide data to the Graph in different ways.
Nodes
A Node is a process that performs computation. A robot will control system is typically
composed of many nodes. For example, one node will be used to control a specific sensor,
one node to control the wheel motors, one node to perform teleoperation, one node to
perform path planning, and so on. Nodes may reside in different systems.
Master
The ROS Master provides naming, registration services and lookup to the rest of the nodes
in the ROS system. The Master is the one responsible for making sure that every ROS Nodes
find each other, exchange messages, or invoke Services.
Messages
Nodes make use of Messages to communicate with one another. A Message is a simple data
structure, containing typed fields. Standard primitive types such as integer, floating point,
Boolean, etc. are supported, as well as arrays of primitive types. Messages can include
arbitrarily nested structures and arrays (much like C structs).
Topics
Messages are routed via a transport system using publish / subscribe semantics. A node sends
out a message by publishing it to a given Topic. The Topic is a Name that is used to identify
the content of a message. Whenever a Node is interested in acquiring a certain kind of data,
it just need to subscribe to the appropriate Topic. A Topic can be seen as a Bus by which
information travel in a ROS system. Each Bus has a name, and anyone can connect to the
bus to send and receive messages as long as they are of the right type.
Services
The publish / subscribe model is a very flexible communication paradigm, but its many-tomany, one-way transport is not appropriate for request / reply interactions, which are often
required in distributed system. Request / Reply is done via Services, which are defined by pair
of message structures, which are defined by a pair of message structures: one for the request
and one for the reply.

ROS Community Level

The ROS Community Level concepts are ROS resources that enable separate communities
to exchange software and knowledge. These resources include Distributions, Repositories,
the ROS Wiki, etc. Find out more at
http://www.ros.org/wiki/ROS/Concepts#ROS_Community_Level

Installing and Configuring your ROS Environment

Go to the following link for installation guide for Linux Ubuntu, and instructions on how to
setup your environment variables.
http://www.ros.org/wiki/groovy/Installation/Ubuntu
After successfully installing and configuring ROS, we can now create our workspace. When
working with ROS source code, it is often useful to do so in a "workspace". We are going to
see how to do so in the next section.
Create a ROS Workspace
The following command (execute from the Terminal) creates a new workspace
in ~/ros_workspace which extends the set of packages installed in /opt/ros/<distro> (replace
<distro> with the name of the distribution installed on your system):
$ rosws init ~/ros_workspace /opt/ros/<distro>

Next we create a sandbox directory where we will store packages created during the tutorial.
New packages need to be put in a path that is in the variable ROS_PACKAGE_PATH. All
directory managed by rosws are automatically added to the ROS_PACKAGE_PATH.
$ mkdir ~/ros_workspace/sandbox
$ rosws set ~/ros_workspace/sandbox

This will create a sandbox folder in our workspace. Next we need to re-source
~/ros_workspace/setup.bash to make sure that the updated ROS_PACKAGE_PATH is used.
Next we are going to create a package for our next section where we will be writing our first
ROS node in C++.
Create a ROS Package
To do so open a new terminal windows. Move to the sandbox directory.
$ cd ~/ros_workspace/sandbox

Then create the package by entering the following command

$ roscreate-pkg [package_name] [depend1] [depend2] [depend3]

where [package_name] will be the name of your package followed by the dependencies of
that package [depend1] [depend2] etc.
For our tutorial we will type in the following. Our package will depend on the roscpp and
std_msgs packages
$ roscreate-pkg ros_intro std_msgs roscpp

To move into the directory of the package

$ roscd ros_intro
$ pwd

The following should be printed if the package was correctly created

YOUR_PACKAGE_PATH/ros_intro

Now that we created the package we can start writing some code.

Writing our first ROS node in C++

The code we are going to use for our first example are from ROS tutorials
http://www.ros.org/wiki/ROS/Tutorials
Take time to go through these tutorials on your own.
In this first example we will create two nodes, talker and listener. They will both use the topic
chatter to talk to each other. Talker will publish messages on chatter, and listener on the
other hand will subscribe to chatter in other to listen to what talker has to say.
The Publisher
In this first example we will create two nodes, talker and listener. They will both use the topic
chatter to talk to each other. Talker will publish messages on chatter, and listener on the
other hand will subscribe to chatter in other to listen to what talker has to say.
The code
First create the src/talker.cpp file within the ros_intro package and write the code provided
below.
$ roscd ros_intro
$ gedit src/talker.cpp
C++ Code - talker.cpp

1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char **argv){
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok()){
std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}

Code Breakdown
1.
2.
3.

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>

ros/ros.h is a convenience include that includes all the headers necessary to use the most
common public pieces of the ROS system.
Std_msgs/String.h is includes the description of the message type that will be used for this
example. You need to include a different header if you have to use a different message type.
stream is responsible for some string manipulation in C++.
6.
7.

ros::init(argc, argv, "talker");

ros::NodeHandle n;

ros::init Initializes ROS. It is responsible for collecting ROS specific information from
arguments passed at the comment line. This is also where we specify the name of our node.
Node names must be unique in a running system.
ros::NodeHandle Create a handle to this process' node. The first NodeHandle created will
actually do the initialization of the node, and the last one destructed will clean up any
resources the node was using. It allows you to interact with the node associated with this
process.

1.
2.

ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

ros::Rate loop_rate(10);

NodeHandle::advertise() returns a ros::Publisher object, which serves two purposes: 1) it

contains a publish() method that lets you publish messages onto the topic it was created
with, and 2) when it goes out of scope, it will automatically unadvertise. This is the function
in charge of making the XML/RPC call to the ROS Master advertising std_msgs::String on the
topic named chatter
A ros::Rate object allows you to specify a frequency that you would like to loop at. It will keep
track of how long it has been since the last call to Rate::sleep(), and sleep for the correct
amount of time. In this case it maintain the frequency of publishing at 10 Hz.
10.
11.
12.

int count = 0;
while (ros::ok()){

count here will just be used to keep track of the number of messages transmitted. Its value
will be attached to the string message that is published.
By default roscpp will install a SIGINT handler which provides Ctrl-C handling which will
cause ros::ok() to return false if that happens.
ros::ok() will return false if:

a SIGINT is received (Ctrl-C)

we have been kicked off the network by another node with the same name
ros::shutdown() has been called by another part of the application.
all Ros::NodeHandle have been destroyed

Once ros::ok() returns false, all ROS calls will fail.

13.
14.
15.
16.

std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();

These 4 lines perform some strings manipulation too add count to the message the will be
broadcasted.
17.
18.

ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);

ROS_INFO is the ROS replacement for printf or cout. For more details, see the rosconsole
documentation.
ros::Publisher::publish() sends the message to all the subscribers.
19.

ros::spinOnce();

20.
21.

loop_rate.sleep();
++count;

Calling the ros::spinOnce() in this example is note really necessary, because we are not
subscribing to any topic. The callbacks for receiving messages on those topics are not called
immediately, instead they are placed in a queue which is processed when a call to
ros::spinOnce() is made. But it is good practice to always have it in a program.
ros::Rate::sleep() allows us here to keep a particular publishing frequency
count gets incremented to keep track of messages.
To sum up, here is a condensed version of whats going on with the talker program:
Initialize the ROS system
Advertise that we are going to be publishing std_msgs/String messages on the chatter
topic to the master
Loop while publishing messages to chatter 10 times a second

We will now look into our receiving node.

The Listener
The code
First create the src/listener.cpp file within the ros_intro package and write the code provided
below.
$ roscd ros_intro
$ gedit src/listener.cpp
C++ Code - listener.cpp
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.

#include "ros/ros.h"
#include "std_msgs/String.h"
void chatterCallback(const std_msgs::String::ConstPtr& msg){
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char **argv){
ros::init(argc, argv, "listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
ros::spin();
return 0;
}

Code Breakdown
1.
2.
3.
4.
5.
6.

#include "ros/ros.h"
#include "std_msgs/String.h"
void chatterCallback(const std_msgs::String::ConstPtr& msg){
ROS_INFO("I heard: [%s]", msg->data.c_str());
}

We include the same headers as before.

chatterCallback() is the function we defined that gets called whenever we receive a message
on the subscribed topic. This is where we retrieve the content of the message for display as
we did in this case, or we could store it in a variable for later processing.
8. int main(int argc, char **argv){
9.
ros::init(argc, argv, "listener");
10.
ros::NodeHandle n;
11.
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
12.
ros::spin();
13.
14.
return 0;
15. }

ros::NodeHandle::subscribe makes an XML/RPC call to the ROS Master. It subscribes to the

topic chatter. The second argument is the queue size, in case we are not able to process
messages fast enough. In this case, if the queue reaches 1000 messages, we will start
throwing away old messages as new ones arrive. ROS will call the chatterCallback() function
whenever a new message arrives.
NodeHandle::subscribe() returns a ros::Subscriber object, that should be hold until you want
to unsubscribe. When the Subscriber object is destructed, it will automatically unsubscribe
from the chatter topic.
ros::spin() loops around calling message callbacks as fast as possible. It will exit once ros::ok()
returns false, meaning ros::shutdown() has been called, either by the default Ctrl-C handler,
or it being called manually.
In sum, here is a condensed version of whats happening:

Initialize the ROS system

Subscribe to the chatter topic
Spin, waiting for messages to arrive
When a message arrives, the chatterCallback() function is called.

Building and executing the nodes

First we need to edit CMakeLists.txt from our package. Open the file with the following
command from the terminal.
$ gedit ros_intro/CMakeLists.txt

Then add the following 2 lines to the file

Rosbuild_add_executable(talker src/talker.cpp)
Rosbuild_add_executable(listener src/listener.cpp)

This will create two executables, talker and listener, which by default go into the bin
directory. Save and exit.
For more information on using CMake with ROS, check CMakeLists
Now lets build our package.
$ rosmake ros_intro

In separate terminal windows, run the following programs

$ roscore
$ rosrun ros_intro talker
$ rosrun ros_intro listener

You should see the following results

Talker Node

Listener Node

2. NXT Robot and ROS

There is quite a range of robots being used with ROS at this day. ROS has quite become a
great tools for hobbyist and researchers. For this second part we will apply some of the
concept we learned to a physical robot. We will be using the NXT Lego Robot. A ROS stack
is available for the NXT. It has basic interfaces for interacting with ROS and NXT.
The bridge between NXT and ROS is the nxt_ros package. This package creates a ROS topic
for each NXT sensor, and publish the sensors data on this topic. It also creates topic for each
NXT motor, which allows for the command of motors from ROS. The nxt_ros bindings talks
directly to the NXT brick, either over USB or Bluetooth.

Installation and Configurations

The following will require you to have a PC running Ubuntu 10.04 or newer. I will be using
Ubuntu 11.10.
Configure the computer
In order for your computer to talk to the NXT brick you need to set your udev1 rules. To do
so you are going to first add a lego group using the following command:
$ sudo groupadd lego

Then add yourself to that group:

$ sudo usermod a G lego <username>

Replace <username> in the previous command with yours. Next create a udev rules file for
the lego group that you just created.
$ echo "BUS==\"usb\", ATTRS{idVendor}==\"0694\", GROUP=\"lego\", MODE=\"0660\"" > /tmp/70lego.rules && sudo mv /tmp/70-lego.rules /etc/udev/rules.d/70-lego.rules

Now restart the udev:

$ sudo restart udev

To complete the configuration, log out and log back in.

For more information on udev visit http://en.wikipedia.org/wiki/Udev

Installing ROS and NXT ROS

Before you can install ROS on your computer, you have to configure the Ubuntu repositories
to allow restricted, universe, and multiverse

Once that is done open a terminal window and setup up your computer to accept software
from ROS.org
$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu oneiric main" >
/etc/apt/sources.list.d/ros-latest.list'

This is for Ubuntu 11.10. If you are running a different version please refers to this page1 to
get the correct command.
Next set up your Keys
$ wget http://packages.ros.org/ros.key -O - | sudo apt-key add -

Installation of ROS Electric. We will get the Desktop-Full Install which includes ROS packages,
robot-generic libraries, 2D/3D simulators, navigation and 2D/3D perception.
$ sudo apt-get update
$ sudo apt-get install ros-electric-desktop-full

The installation will take quite a while. You need to make sure your internet connection is
always on. Once ROS installation is complete. Set up your environment variables:
$ echo source /opt/ros/electric/setup.bash >> ~/.bashrc
. ~/.bashrc

Now that the installation of ROS is complete, you can install the NXT-ROS bindings:

http://www.ros.org/wiki/Robots/NXT/electric#Installation_Instructions

$ sudo apt-get update

$ sudo apt-get install ros-electric-nxtall

When installation is complete you should be ready to test it with your robot.
Lets Try It!
IMPORTANT: For NXT-ROS work with your Mindstorms, you need to have the firmware 1.28 or higher. If you do not know
how to upgrade, you can follow the following tutorial: Updating NXT Firmware.

Make sure you have your NXT ready and connected to your PC. We are now going to run a
program to test if the NXT can indeed talk to ROS. For that connect a touch sensor to Port
1 of you NXT. Then in a terminal window, type the following
$ roscore

Then in another terminal, run the following

$ rosrun nxt_python touch_sensor_test.py

You should get the following results in your terminal. When press the touch sensor, it will
print True and False when released.

Testing the touch sensor

If instead you see the following, then you probably didnt set something right or the Brick
is not connected or powered on.

NXT Brick not found

If you got the touch sensor test right, we can now write a small application to run our NXT
using the robot car sensor1 model provided with the ROS package. If you like you can go
through this tutorial2 to create your own robot for ROS.

Teleoperate the NXT using Keyboard

We will now walk through the steps to control the NXT using your keyboard. The following
diagram, will attempt to present some of the important nodes used to achieve that, and
how they inter connect

1
2

Robot car Sensor - http://www.ros.org/wiki/nxt_robot_sensor_car

Create your own robot model - http://www.ros.org/wiki/nxt/Tutorials

Inside the nxt_robot_car_sensor package, there a file robot.yaml which is a description file
for the robot, where we need to specify the parameters for the sensors, motors, and define
on which port they will be physically connected.
On my PC the file is located at the following location.
/opt/ros/electric/stacks/nxt_robots/nxt_robot_sensor_car

You will have to edit this file in order to match your robot configuration. The initial file
looks like this.
robot.yaml
1. nxt_robot:
2.
- type: motor
3.
name: r_wheel_joint
4.
port: PORT_A
5.
desired_frequency: 20.0
6.
7.
- type: motor
8.
name: l_wheel_joint
9.
port: PORT_B
10.
desired_frequency: 20.0
11.
12.
- type: motor
13.
name: m_wheel_joint
14.
port: PORT_C
15.
desired_frequency: 1.0
16.
17.
- type: gyro
18.
name: gyro
19.
frame_id: gyro_link
20.
port: PORT_3
21.
offset: 0
22.
desired_frequency: 20.0
23.
24.
- type: ultrasonic
25.
frame_id: ultrasonic_link
26.
name: ultrasonic_sensor
27.
port: PORT_2
28.
spread_angle: 0.2
29.
min_range: 0.01
30.
max_range: 2.5
31.
desired_frequency: 5.0
32.
33.
- type: color
34.
frame_id: color_link
35.
name: color_sensor
36.
port: PORT_1
37.
desired_frequency: 10.0

This file defines a robot with a gyroscope, ultrasonic sensor, color sensor, and 3 motors. The
one I will be using only make use of 2 motors (PORT A and B) and an ultrasonic sensor
(PORT 4). I will then have to edit this file so it looks like this.
robot.yaml
1. nxt_robot:
2.
- type: motor
3.
name: r_wheel_joint
4.
port: PORT_A
5.
desired_frequency: 20.0
6.
7.
- type: motor
8.
name: l_wheel_joint
9.
port: PORT_B
10.
desired_frequency: 20.0
11.
12.
- type: ultrasonic
13.
frame_id: ultrasonic_link
14.
name: ultrasonic_sensor
15.
port: PORT_4
16.
spread_angle: 0.2
17.
min_range: 0.01
18.
max_range: 2.5
19.
desired_frequency: 5.0
20.

Feel free to add any sensors you might have to the robot.yaml file. After editing the file we
now have to build the package again from the terminal for our system to consider the
changes.
$ rosmake nxt_robot_sensor_car

Once thats done, you can now run the robot with the following command.
$ roscore

First start the ROS Master with roscore. Always make sure its running before you try to
execute a ROS process. Now execute the following from another terminal
$ roslaunch nxt_robot_sensor_car robot.launch

Keep this program running, then open a new terminal window and run the control program.
$ roslaunch nxt_teleop teleop_keyboard.launch

You should have a prompt on your screen instructing you to use the direction keys to move
the robot. If you will like to see a graph showing all the running nodes and their connections
run the following command

$ rxgraph

And you should get this

You can also use a joystick to control your robot. Instead of the teleop_keyboard.launch, you
will just need to launch the teleop_joy.launch
$ roslaunch nxt_teleop teleop_joy.launch

Follow Objects with your NXT and ROS

Now that we can control our NXT with keyboard/joystick lets have our robot follow an object
place in front of it. Basically what we want to achieve here is read the distance provided by
the ultrasonic sensor. When the object is within a certain range of the robot and moving
away, the robot should move forward, following the object. If the object is too close of the
robot, it should back up (move backward). When the object stops moving the robot should
also stop within a given range.
In order to achieve that we will have to write a node that will subscribe to the topic
ultrasonic_sensor, in order to read distance. Then based on the distance it receives will publish
velocities on topic cmd_vel, to control the movements of the robot.

We will create a new package in the sandbox folder of our workspace (see section 1.5).
$ roscreate-pkg nxt_follow_obj nxt_ros geometry_msgs nxt_msgs

This package will depend on the nxt_ros package, the geometry_msgs, and nxt_msgs.
Next we need to write the code for the node.

The code
Create the src/follow_obj.cpp file within the nxt_follow_obj package and write the code
provided below.
C++ Code follow_obj.cpp
1. #include <ros/ros.h>
2. #include <geometry_msgs/Twist.h>
3. #include <nxt_msgs/Range.h>
4. #include <signal.h>
5. #include <termios.h>
6. #include <stdio.h>
7. #include "boost/thread/mutex.hpp"
8. #include "boost/thread/thread.hpp"
9.
10. class NxtFollow {
11.
public:
12.
NxtFollow();
13.
void controlLoop();
14.
void watchdog();
15.
16.
private:
17.
ros::NodeHandle nh_, ph_;

18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.

double linear_, angular_;

double l_scale_, a_scale_;
double dist_from_obj_;
ros::Publisher vel_pub_;
ros::Subscriber range_sub_;
ros::Time last_publish_;
boost::mutex publish_mutex_;
void publish(double, double);
void callback(const nxt_msgs::Range::ConstPtr& Range);
};
NxtFollow::NxtFollow() : linear_(0),
angular_(0),
l_scale_(1.0),
a_scale_(1.0)
{
ph_.param("scale_angular", a_scale_, a_scale_);
ph_.param("scale_linear", l_scale_, l_scale_);
vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
range_sub_ = nh_.subscribe("ultrasonic_sensor", 1, &NxtFollow::callback, this);
}
int kfd = 0;
struct termios cooked, raw;
void NxtFollow::watchdog() {
boost::mutex::scoped_lock lock(publish_mutex_);
if (ros::Time::now() > last_publish_ + ros::Duration(0.15))
publish(0, 0);
}
void NxtFollow::controlLoop() {
char c;
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
puts("Let me follow a box");
puts("---------------------------");
puts("Place an object in front of the nxt to start");
while(ros::ok()) {
linear_= angular_= 0;
ROS_DEBUG("value: 0x%02X\n", c);
ROS_INFO("controlLoop");
if (dist_from_obj_ > 0.3 && dist_from_obj_ <= 1.0) {
ROS_INFO("Following");
ROS_DEBUG("Follow the Object");
linear_ = 1.0;
angular_ = 0.0;

79.
} else if (dist_from_obj_ < 0.1) {
80.
ROS_INFO("Moving away");
81.
ROS_DEBUG("Move away from the Object");
82.
linear_ = -1.0;
83.
angular_ = 0.0;
84.
} else {
85.
ROS_INFO("Not Moving");
86.
ROS_DEBUG("Don't Move");
87.
linear_ = 0.0;
88.
angular_ = 0.0;
89.
}
90.
91.
boost::mutex::scoped_lock lock(publish_mutex_);
92.
last_publish_ = ros::Time::now();
93.
publish(angular_, linear_);
94.
}
95.
96. }
97.
98. void NxtFollow::callback(const nxt_msgs::Range::ConstPtr& Range) {
99.
ROS_INFO("Range: [%f]", Range->range);
100.
dist_from_obj_ = Range->range;
101. }
102.
103. void NxtFollow::publish(double angular, double linear) {
104.
geometry_msgs::Twist vel;
105.
vel.angular.z = a_scale_*angular;
106.
vel.linear.x = l_scale_*linear;
107.
108.
vel_pub_.publish(vel);
109.
110.
return;
111. }
112.
113. void quit(int sig) {
114.
tcsetattr(kfd, TCSANOW, &cooked);
115.
ros::shutdown();
116.
exit(0);
117. }
118.
119. int main(int argc, char** argv) {
120.
ros::init(argc, argv, "nxt_follow");
121.
NxtFollow nxt_follow;
122.
123.
ros::NodeHandle n;
124.
125.
signal(SIGINT, quit);
126.
127.
boost::thread my_thread(boost::bind(&NxtFollow::controlLoop, &nxt_follow));
128.
129.
ros::Timer timer = n.createTimer(ros::Duration(0.1), boost::bind(&NxtFollow::watchdog, &nxt
_follow));
130.
131.
ros::spin();
132.
133.
my_thread.interrupt();
134.
my_thread.join();
135.
136.
return(0);
137. }

Code Breakdown
Now lets go through the code and see whats happening
The header files
1.
2.
3.
4.
5.
6.
7.
8.

#include
#include
#include
#include
#include
#include
#include
#include

<ros/ros.h>
<geometry_msgs/Twist.h>
<nxt_msgs/Range.h>
<signal.h>
<termios.h>
<stdio.h>
"boost/thread/mutex.hpp"
"boost/thread/thread.hpp"

ros/ros.h is a convenience include that includes all the headers necessary to use the most
common public pieces of the ROS system.
geometry_msgs provides messages from common geometric primitives. Twist.h expresses
the velocity in free space broken into its linear and angular parts.
nxt_msgs/Range.h is the message type published by the ultrasonic sensor. We will need this
to extract the range (distance) returned by our ultrasonic sensor.
signal.h required to handle signals during the execution of the program. A signal is a limited
form of inter-process communication used in Unix. It is an asynchronous notification sent to
a process within a process in order to notify it of an event that occurred. They can be viewed
as Interrupts Events.
termios.h functions describe a general terminal interface that is provided to control
asynchronous communications ports.
stdio.h C++ standart I/O Library
mutex.hpp and thread.hpp both from the boost C++ Libraries are used for multithreading.

NxtFollow Class definition

The following block code is the definition of the class NxtFollow. All the methods and data
members needed to make our robot achieve its task will be declared here. In the chatter
example we did before, we wrote used simple function to perform our task. For this example
we will see how to write a Node using a class

10. class NxtFollow {

11.
public:

12.
13.

NxtFollow();
//Class Constructor
void controlLoop();
//ControlLoop this where we will check distance and decide to
//move the robot or not
14.
void watchdog();
15.
16.
private:
17.
ros::NodeHandle nh_, ph_; //Node Handles
18.
19.
double linear_, angular_; //Linear and angular composite of the velocity
20.
double l_scale_, a_scale_; // The amount to scale the keyboard input for the command
//velocity output.
21.
double dist_from_obj_;
//Will hold the distance from the object
22.
23.
ros::Publisher vel_pub_;
//Publisher Will publish velocities
24.
ros::Subscriber range_sub_;
//Subscriber Will subscribe to ultrasonic sensor
25.
ros::Time last_publish_;
26.
27.
boost::mutex publish_mutex_;
28.
29.
void publish(double, double);
//Method will be used to publish velocity
30.
void callback(const nxt_msgs::Range::ConstPtr& Range);
//Callback
31. };

The constructor
The constructor will be call when a new NxtFollow object will be created to initialize the data
members and parameters to default values.
Here we set the linear and angular composite of the velocity to 0
33. NxtFollow::NxtFollow() : linear_(0),
34.
angular_(0),
35.
l_scale_(1.0),
36.
a_scale_(1.0)
37. {
38.
ph_.param("scale_angular", a_scale_, a_scale_);
39.
ph_.param("scale_linear", l_scale_, l_scale_);
40.
41.
vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
42.
43.
range_sub_ = nh_.subscribe("ultrasonic_sensor", 1, &NxtFollow::callback, this);
44. }

nh_.advertise() will return a ros::Publisher object, which contains a publish() method that will
let us publish messages onto the cmd_vel topic, and when it goes out of scope, it will
automatically unadvertise. This is the function in charge of making the XML/RPC call to the
ROS Master advertising geometry_msgs::Twist on the topic named cmd_vel
nh_.subscribe() makes an XML/RPC call to the ROS Master. It subscribes to the topic
ultrasonic_sensor. The second argument is the queue size, in case we are not able to
process messages fast enough. In this case, if the queue reaches 1 message, we will start
throwing away old messages as new ones arrive. ROS will call the callback() function
whenever a new message arrives. Since we are using a class, we need to pass in a 4th
parameter which is the address of the Node.

The control loop

55. void NxtFollow::controlLoop() {
56.
char c;
57.
tcgetattr(kfd, &cooked);
58.
memcpy(&raw, &cooked, sizeof(struct termios));
59.
raw.c_lflag &=~ (ICANON | ECHO);
60.
61.
raw.c_cc[VEOL] = 1;
62.
raw.c_cc[VEOF] = 2;
63.
tcsetattr(kfd, TCSANOW, &raw);
64.
65.
puts("Let me follow a box");
66.
puts("---------------------------");
67.
puts("Place an object in front of the nxt to start");
68.
69.
while(ros::ok()) {
70.
linear_= angular_= 0;
71.
ROS_DEBUG("value: 0x%02X\n", c);
72.
ROS_INFO("controlLoop");
73.
74.
if (dist_from_obj_ > 0.3 && dist_from_obj_ <= 1.0) {
75.
ROS_INFO("Following");
76.
ROS_DEBUG("Follow the Object");
77.
linear_ = 1.0;
78.
angular_ = 0.0;
79.
} else if (dist_from_obj_ < 0.1) {
80.
ROS_INFO("Moving away");
81.
ROS_DEBUG("Move away from the Object");
82.
linear_ = -1.0;
83.
angular_ = 0.0;
84.
} else {
85.
ROS_INFO("Not Moving");
86.
ROS_DEBUG("Don't Move");
87.
linear_ = 0.0;
88.
angular_ = 0.0;
89.
}
90.
91.
boost::mutex::scoped_lock lock(publish_mutex_);
92.
last_publish_ = ros::Time::now();
93.
publish(angular_, linear_);
94.
}
95.
96. }

The while loop will execute until Ctrl-C is pressed, which will cause the program to stop. This
is where we check for the distance from the object, then decide whether we want the robot
to move towards the object, move away from it or not move at all. We assign the values to
linear_ and angular_ variable then at the end of the loop we make a call to the publish() to
publish to the topic cmd_vel.

The callback method

98. void NxtFollow::callback(const nxt_msgs::Range::ConstPtr& Range) {
99.
ROS_INFO("Range: [%f]", Range->range);
100.
dist_from_obj_ = Range->range;
101. }

The callback() method gets called everytime we receive a message on the subscribed topic.
Then we extract the range and assign it to the variable dist_from_obj that is used in the
control loop to decide on the robots move.
The publish method
103. void NxtFollow::publish(double angular, double linear) {
104.
geometry_msgs::Twist vel;
105.
vel.angular.z = a_scale_*angular;
106.
vel.linear.x = l_scale_*linear;
107.
108.
vel_pub_.publish(vel);
109.
110.
return;
111. }

The publish() gets called in the control loop to publish the velocity to the cmd_vel.
The main
119. int main(int argc, char** argv) {
120.
ros::init(argc, argv, "nxt_follow"); //Initialize ROS
121.
NxtFollow nxt_follow;
//Instanciate an NxtFollow object
122.
123.
ros::NodeHandle n; //NodeHandle
124.
125.
signal(SIGINT, quit); //When Ctrl-C signal is sent from keyboard, quit() is called
126.
127.
boost::thread my_thread(boost::bind(&NxtFollow::controlLoop, &nxt_follow));
128.
129.
ros::Timer timer = n.createTimer(ros::Duration(0.1), boost::bind(&NxtFollow::watchdog, &nxt_fo
llow));
130.
131.
ros::spin();
132.
133.
my_thread.interrupt();
134.
my_thread.join();
135.
136.
return(0);
137. }

In the main we create instance of the NxtFollow class that will subscribe to ultrasonic_sensor
topic and publish cmd_vel topic

Build and Run the Execute the Node

First we need to edit CMakeLists.txt from our package. Open the file with the following
command from the terminal.
$ gedit nxt_follow_obj/CMakeLists.txt

Then add the following line to the file

Rosbuild_add_executable(follow_obj src/follow_obj.cpp)

This will create the executable for follow_obj, which by default go into the bin directory. Save
and exit.
Now lets build our package.
$ rosmake nxt_follow_obj

In separate terminal windows, run the following programs

$ roscore
$ rosrun nxt_robot_sensor_car robot.launch
$ rosrun nxt_follow_obj follow_obj

You should see the following results. And if you place an object in front of you robot it
should start following it.

You should see this after starting robot.launch

follow_obj program running

In the previous windows, we have our follow_obj running and continuously printing the state
of the robot.

3. Useful Links
Learn C++
http://www.learncpp.com/
http://www.cplusplus.com/doc/tutorial/
ROS Wiki
http://www.ros.org
http://www.ros.org/wiki/roscpp
http://www.ros.org/wiki/roscpp_tutorials/Tutorials/UsingClassMethodsAsCallbacks
http://www.ros.org/wiki/roscpp/Overview/NodeHandles
NXT ROS
http://www.ros.org/wiki/Robots/NXT/
http://www.ros.org/wiki/nxt_robot_sensor_car?distro=electric